From 91bfdf05ca32047d5a2e119dc239d21792f7a89e Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 23 Jun 2014 00:47:45 +0100 Subject: [PATCH] Move mw loop() code into separate methods. Cleanup related code. If a given feature or mode is off the next task is not processed in the current loop but will be processed during the next loop iteration for simplification, this allowed the cleanup of return values in other code. --- src/main/flight/imu.c | 20 +- src/main/flight/imu.h | 2 +- src/main/mw.c | 395 ++++++++++++++++++++--------------- src/main/sensors/barometer.c | 18 +- src/main/sensors/barometer.h | 9 +- src/main/sensors/compass.c | 20 +- src/main/sensors/compass.h | 2 +- 7 files changed, 256 insertions(+), 210 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 31b35f4a0..df4407e77 100755 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -362,8 +362,8 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) } #ifdef BARO -#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) - +// 40hz update rate (20hz LPF on acc) +#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) #define DEGREES_80_IN_DECIDEGREES 800 @@ -405,10 +405,9 @@ int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old) return baroPID; } -int getEstimatedAltitude(void) +void calculateEstimatedAltitude(uint32_t currentTime) { - static uint32_t previousT; - uint32_t currentT = micros(); + static uint32_t previousTime; uint32_t dTime; int32_t baroVel; float dt; @@ -420,10 +419,11 @@ int getEstimatedAltitude(void) static float accAlt = 0.0f; static int32_t lastBaroAlt; - dTime = currentT - previousT; - if (dTime < UPDATE_INTERVAL) - return 0; - previousT = currentT; + dTime = currentTime - previousTime; + if (dTime < BARO_UPDATE_FREQUENCY_40HZ) + return; + + previousTime = currentTime; if (!isBaroCalibrationComplete()) { performBaroCalibrationCycle(); @@ -469,7 +469,5 @@ int getEstimatedAltitude(void) BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old); accZ_old = accZ_tmp; - - return 1; } #endif /* BARO */ diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index ad2b0478a..d7c6707bc 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s { void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband); -int getEstimatedAltitude(void); +void calculateEstimatedAltitude(uint32_t currentTime); void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration); void calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); diff --git a/src/main/mw.c b/src/main/mw.c index 4f96476be..4303d6e25 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -349,175 +349,227 @@ void updateInflightCalibrationState(void) } } -void loop(void) +void updateMagHold(void) +{ + if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) { + int16_t dif = heading - magHold; + if (dif <= -180) + dif += 360; + if (dif >= +180) + dif -= 360; + dif *= -masterConfig.yaw_control_direction; + if (f.SMALL_ANGLE) + rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30; // 18 deg + } else + magHold = heading; +} + +typedef enum { +#ifdef MAG + UPDATE_COMPASS_TASK, +#endif +#ifdef BARO + UPDATE_BARO_TASK, + CALCULATE_ALTITUDE_TASK, +#endif +#ifdef SONAR + UPDATE_SONAR_TASK, +#endif + UPDATE_GPS_TASK +} periodicTasks; + +#define PERIODIC_TASK_COUNT (UPDATE_GPS_TASK + 1) + + +void executePeriodicTasks(void) +{ + static int periodicTaskIndex = 0; + + switch (periodicTaskIndex++) { +#ifdef MAG + case UPDATE_COMPASS_TASK: + if (sensors(SENSOR_MAG)) { + updateCompass(&masterConfig.magZero); + } + break; +#endif + +#ifdef BARO + case UPDATE_BARO_TASK: + if (sensors(SENSOR_BARO)) { + baroUpdate(currentTime); + } + break; + + case CALCULATE_ALTITUDE_TASK: + if (sensors(SENSOR_BARO) && isBaroReady()) { + calculateEstimatedAltitude(currentTime); + } + break; +#endif + + case UPDATE_GPS_TASK: + // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck + // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will + // change this based on available hardware + if (feature(FEATURE_GPS)) { + gpsThread(); + } + break; +#ifdef SONAR + case UPDATE_SONAR_TASK: + if (sensors(SENSOR_SONAR)) { + Sonar_update(); + } + break; +#endif + } + + if (periodicTaskIndex >= PERIODIC_TASK_COUNT) { + periodicTaskIndex = 0; + } +} + +void processRx(void) { int i; - static uint32_t loopTime; uint32_t auxState = 0; + calculateRxChannelsAndUpdateFailsafe(currentTime); + + // in 3D mode, we need to be able to disarm by switch at any time + if (feature(FEATURE_3D)) { + if (!rcOptions[BOXARM]) + mwDisarm(); + } + + updateRSSI(currentTime); + + if (feature(FEATURE_FAILSAFE)) { + + if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) { + failsafe->vTable->enable(); + } + + failsafe->vTable->updateState(); + } + + throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + + if (throttleStatus == THROTTLE_LOW) { + resetErrorAngle(); + resetErrorGyro(); + } + + processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile.activate, masterConfig.retarded_arm); + + if (feature(FEATURE_INFLIGHT_ACC_CAL)) { + updateInflightCalibrationState(); + } + + // Check AUX switches + + // auxState is a bitmask, 3 bits per channel. aux1 is first. + // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8 + // + // the three bits are as follows: + // bit 1 is SET when the stick is less than 1300 + // bit 2 is SET when the stick is between 1300 and 1700 + // bit 3 is SET when the stick is above 1700 + // if the value is 1300 or 1700 NONE of the three bits are set. + + for (i = 0; i < 4; i++) { + auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | + (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | + (rcData[AUX1 + i] > 1700) << (3 * i + 2); + auxState |= ((rcData[AUX5 + i] < 1300) << (3 * i) | + (1300 < rcData[AUX5 + i] && rcData[AUX5 + i] < 1700) << (3 * i + 1) | + (rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16; + } + for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) + rcOptions[i] = (auxState & currentProfile.activate[i]) > 0; + + if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { + // bumpless transfer to Level mode + if (!f.ANGLE_MODE) { + resetErrorAngle(); + f.ANGLE_MODE = 1; + } + } else { + f.ANGLE_MODE = 0; // failsafe support + } + + if (rcOptions[BOXHORIZON]) { + f.ANGLE_MODE = 0; + if (!f.HORIZON_MODE) { + resetErrorAngle(); + f.HORIZON_MODE = 1; + } + } else { + f.HORIZON_MODE = 0; + } + + if (f.ANGLE_MODE || f.HORIZON_MODE) { + LED1_ON; + } else { + LED1_OFF; + } + +#ifdef BARO + if (sensors(SENSOR_BARO)) { + updateAltHoldState(); + } +#endif + +#ifdef MAG + if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { + if (rcOptions[BOXMAG]) { + if (!f.MAG_MODE) { + f.MAG_MODE = 1; + magHold = heading; + } + } else { + f.MAG_MODE = 0; + } + if (rcOptions[BOXHEADFREE]) { + if (!f.HEADFREE_MODE) { + f.HEADFREE_MODE = 1; + } + } else { + f.HEADFREE_MODE = 0; + } + if (rcOptions[BOXHEADADJ]) { + headFreeModeHold = heading; // acquire new heading + } + } +#endif + + if (sensors(SENSOR_GPS)) { + updateGpsWaypointsAndMode(); + } + + if (rcOptions[BOXPASSTHRU]) { + f.PASSTHRU_MODE = 1; + } else { + f.PASSTHRU_MODE = 0; + } + + if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) { + f.HEADFREE_MODE = 0; + } +} + +void loop(void) +{ + static uint32_t loopTime; + updateRx(); if (shouldProcessRx(currentTime)) { - calculateRxChannelsAndUpdateFailsafe(currentTime); - - // in 3D mode, we need to be able to disarm by switch at any time - if (feature(FEATURE_3D)) { - if (!rcOptions[BOXARM]) - mwDisarm(); - } - - updateRSSI(currentTime); - - if (feature(FEATURE_FAILSAFE)) { - - if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) { - failsafe->vTable->enable(); - } - - failsafe->vTable->updateState(); - } - - throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - - if (throttleStatus == THROTTLE_LOW) { - resetErrorAngle(); - resetErrorGyro(); - } - - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile.activate, masterConfig.retarded_arm); - - if (feature(FEATURE_INFLIGHT_ACC_CAL)) { - updateInflightCalibrationState(); - } - - // Check AUX switches - - // auxState is a bitmask, 3 bits per channel. aux1 is first. - // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8 - // - // the three bits are as follows: - // bit 1 is SET when the stick is less than 1300 - // bit 2 is SET when the stick is between 1300 and 1700 - // bit 3 is SET when the stick is above 1700 - // if the value is 1300 or 1700 NONE of the three bits are set. - - for (i = 0; i < 4; i++) { - auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | - (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | - (rcData[AUX1 + i] > 1700) << (3 * i + 2); - auxState |= ((rcData[AUX5 + i] < 1300) << (3 * i) | - (1300 < rcData[AUX5 + i] && rcData[AUX5 + i] < 1700) << (3 * i + 1) | - (rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16; - } - for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) - rcOptions[i] = (auxState & currentProfile.activate[i]) > 0; - - if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { - // bumpless transfer to Level mode - if (!f.ANGLE_MODE) { - resetErrorAngle(); - f.ANGLE_MODE = 1; - } - } else { - f.ANGLE_MODE = 0; // failsafe support - } - - if (rcOptions[BOXHORIZON]) { - f.ANGLE_MODE = 0; - if (!f.HORIZON_MODE) { - resetErrorAngle(); - f.HORIZON_MODE = 1; - } - } else { - f.HORIZON_MODE = 0; - } - - if (f.ANGLE_MODE || f.HORIZON_MODE) { - LED1_ON; - } else { - LED1_OFF; - } - -#ifdef BARO - if (sensors(SENSOR_BARO)) { - updateAltHoldState(); - } -#endif - -#ifdef MAG - if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { - if (rcOptions[BOXMAG]) { - if (!f.MAG_MODE) { - f.MAG_MODE = 1; - magHold = heading; - } - } else { - f.MAG_MODE = 0; - } - if (rcOptions[BOXHEADFREE]) { - if (!f.HEADFREE_MODE) { - f.HEADFREE_MODE = 1; - } - } else { - f.HEADFREE_MODE = 0; - } - if (rcOptions[BOXHEADADJ]) { - headFreeModeHold = heading; // acquire new heading - } - } -#endif - - if (sensors(SENSOR_GPS)) { - updateGpsWaypointsAndMode(); - } - - if (rcOptions[BOXPASSTHRU]) { - f.PASSTHRU_MODE = 1; - } else { - f.PASSTHRU_MODE = 0; - } - - if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) { - f.HEADFREE_MODE = 0; - } - } else { // not in rc loop - static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes - switch (taskOrder) { - case 0: - taskOrder++; -#ifdef MAG - if (sensors(SENSOR_MAG) && compassGetADC(&masterConfig.magZero)) - break; -#endif - case 1: - taskOrder++; -#ifdef BARO - if (sensors(SENSOR_BARO) && baroUpdate(currentTime) != BAROMETER_ACTION_NOT_READY) - break; -#endif - case 2: - taskOrder++; -#ifdef BARO - if (sensors(SENSOR_BARO) && getEstimatedAltitude()) - break; -#endif - case 3: - // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck - // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will - // change this based on available hardware - taskOrder++; - if (feature(FEATURE_GPS)) { - gpsThread(); - break; - } - case 4: - taskOrder = 0; -#ifdef SONAR - if (sensors(SENSOR_SONAR)) { - Sonar_update(); - } -#endif - } + processRx(); + } else { + // not in rc loop + executePeriodicTasks(); } currentTime = micros(); @@ -537,17 +589,7 @@ void loop(void) #ifdef MAG if (sensors(SENSOR_MAG)) { - if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) { - int16_t dif = heading - magHold; - if (dif <= -180) - dif += 360; - if (dif >= +180) - dif -= 360; - dif *= -masterConfig.yaw_control_direction; - if (f.SMALL_ANGLE) - rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30; // 18 deg - } else - magHold = heading; + updateMagHold(); } #endif @@ -570,7 +612,12 @@ void loop(void) } // PID - note this is function pointer set by setPIDController() - pid_controller(¤tProfile.pidProfile, ¤tProfile.controlRateConfig, masterConfig.max_angle_inclination, ¤tProfile.accelerometerTrims); + pid_controller( + ¤tProfile.pidProfile, + ¤tProfile.controlRateConfig, + masterConfig.max_angle_inclination, + ¤tProfile.accelerometerTrims + ); mixTable(); writeServos(); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 1ca148e8e..808dbb094 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -30,7 +30,7 @@ baro_t baro; // barometer access functions uint16_t calibratingB = 0; // baro calibration = get new ground pressure value int32_t baroPressure = 0; int32_t baroTemperature = 0; -uint32_t baroPressureSum = 0; +static uint32_t baroPressureSum = 0; int32_t BaroAlt = 0; #ifdef BARO @@ -82,13 +82,19 @@ typedef enum { BAROMETER_NEEDS_CALCULATION } barometerState_e; -barometerAction_e baroUpdate(uint32_t currentTime) +static bool baroReady = false; + +bool isBaroReady(void) { + return baroReady; +} + +void baroUpdate(uint32_t currentTime) { static uint32_t baroDeadline = 0; static barometerState_e state = BAROMETER_NEEDS_SAMPLES; if ((int32_t)(currentTime - baroDeadline) < 0) - return BAROMETER_ACTION_NOT_READY; + return; baroDeadline = currentTime; @@ -98,17 +104,17 @@ barometerAction_e baroUpdate(uint32_t currentTime) baro.start_ut(); baroDeadline += baro.ut_delay; baro.calculate(&baroPressure, &baroTemperature); + baroReady = true; state = BAROMETER_NEEDS_SAMPLES; - return BAROMETER_ACTION_PERFORMED_CALCULATION; + break; case BAROMETER_NEEDS_SAMPLES: - default: baro.get_ut(); baro.start_up(); baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); state = BAROMETER_NEEDS_CALCULATION; baroDeadline += baro.up_delay; - return BAROMETER_ACTION_OBTAINED_SAMPLES; + break; } } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 4e32933e2..b82985784 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -26,19 +26,14 @@ typedef struct barometerConfig_s { float baro_cf_alt; // apply CF to use ACC for height estimation } barometerConfig_t; -typedef enum { - BAROMETER_ACTION_NOT_READY = 0, - BAROMETER_ACTION_OBTAINED_SAMPLES, - BAROMETER_ACTION_PERFORMED_CALCULATION -} barometerAction_e; - extern int32_t BaroAlt; #ifdef BARO void useBarometerConfig(barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); -barometerAction_e baroUpdate(uint32_t currentTime); +void baroUpdate(uint32_t currentTime); +bool isBaroReady(void); int32_t baroCalculateAltitude(void); void performBaroCalibrationCycle(void); #endif diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 864e94ba0..fb36fa77e 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -50,23 +50,25 @@ void compassInit(void) magInit = 1; } -int compassGetADC(flightDynamicsTrims_t *magZero) +#define COMPASS_UPDATE_FREQUENCY_10HZ (1000 * 100) + +void updateCompass(flightDynamicsTrims_t *magZero) { - static uint32_t t, tCal = 0; + static uint32_t nextUpdateAt, tCal = 0; static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; uint32_t axis; - if ((int32_t)(currentTime - t) < 0) - return 0; //each read is spaced by 100ms - t = currentTime + 100000; + if ((int32_t)(currentTime - nextUpdateAt) < 0) + return; + + nextUpdateAt = currentTime + COMPASS_UPDATE_FREQUENCY_10HZ; - // Read mag sensor hmc5883lRead(magADC); alignSensors(magADC, magADC, magAlign); if (f.CALIBRATE_MAG) { - tCal = t; + tCal = nextUpdateAt; for (axis = 0; axis < 3; axis++) { magZero->raw[axis] = 0; magZeroTempMin.raw[axis] = magADC[axis]; @@ -82,7 +84,7 @@ int compassGetADC(flightDynamicsTrims_t *magZero) } if (tCal != 0) { - if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions + if ((nextUpdateAt - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions LED0_TOGGLE; for (axis = 0; axis < 3; axis++) { if (magADC[axis] < magZeroTempMin.raw[axis]) @@ -99,7 +101,5 @@ int compassGetADC(flightDynamicsTrims_t *magZero) saveAndReloadCurrentProfileToCurrentProfileSlot(); } } - - return 1; } #endif diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 2f4d95d83..eb1540aa4 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -19,7 +19,7 @@ #ifdef MAG void compassInit(void); -int compassGetADC(flightDynamicsTrims_t *magZero); +void updateCompass(flightDynamicsTrims_t *magZero); #endif extern int16_t magADC[XYZ_AXIS_COUNT];