rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Dominic Clifton 2014-06-23 00:47:45 +01:00
parent 8d0509dbfb
commit 91bfdf05ca
7 changed files with 256 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/main/flight/imu.c
View File

@ -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 */

2
src/main/flight/imu.h
View File

@ -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);

395
src/main/mw.c
View File

@ -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(&currentProfile.pidProfile, &currentProfile.controlRateConfig, masterConfig.max_angle_inclination, &currentProfile.accelerometerTrims);
pid_controller(
&currentProfile.pidProfile,
&currentProfile.controlRateConfig,
masterConfig.max_angle_inclination,
&currentProfile.accelerometerTrims
);
mixTable();
writeServos();

18
src/main/sensors/barometer.c
View File

@ -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;
}
}

9
src/main/sensors/barometer.h
View File

@ -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

20
src/main/sensors/compass.c
View File

@ -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

2
src/main/sensors/compass.h
View File

@ -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];