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

Replace global flags with stateFlags, flightModeFlags and armingFlags. 

Each flag was previously a whole byte, now all of the flags only take up
4 bytes as they are represented by bit masks.

This is cleaner because the different kind of flags are now separated.

Additionally this changes the behaviour of arming slightly.  When using
a switch to arm the aircraft will not arm unless the switch has been in
the off state once.  This prevents arming if you power the aircraft with
a low throttle and the switch in the on position.
This commit is contained in:
Dominic Clifton 2014-08-24 12:11:30 +01:00
parent 1ea014ae25
commit 3f0754d295
18 changed files with 220 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/config/runtime_config.c
View File

@ -20,7 +20,9 @@
#include "config/runtime_config.h"
flags_t f;
uint8_t armingFlags = 0;
uint8_t stateFlags = 0;
uint16_t flightModeFlags = 0;
// each entry in the array is a bitmask, 3 bits per aux channel (only aux 1 to 4), aux1 is first, each bit corresponds to an rc channel reading
// bit 1 - stick LOW

64
src/main/config/runtime_config.h
View File

@ -45,27 +45,51 @@ enum {
extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT];
// FIXME some of these are flight modes, some of these are general status indicators
typedef struct flags_t {
uint8_t OK_TO_ARM;
uint8_t PREVENT_ARMING;
uint8_t ARMED;
uint8_t ANGLE_MODE;
uint8_t HORIZON_MODE;
uint8_t MAG_MODE;
uint8_t BARO_MODE;
uint8_t GPS_HOME_MODE;
uint8_t GPS_HOLD_MODE;
uint8_t HEADFREE_MODE;
uint8_t PASSTHRU_MODE;
uint8_t GPS_FIX;
uint8_t GPS_FIX_HOME;
uint8_t SMALL_ANGLE;
uint8_t CALIBRATE_MAG;
uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code
uint8_t AUTOTUNE_MODE;
} flags_t;
typedef enum {
OK_TO_ARM = (1 << 0),
PREVENT_ARMING = (1 << 1),
SEEN_BOXARM_OFF = (1 << 2),
ARMED = (1 << 3)
} armingFlag_e;
extern uint8_t armingFlags;
#define DISABLE_ARMING_FLAG(mask) (armingFlags &= ~(mask))
#define ENABLE_ARMING_FLAG(mask) (armingFlags |= mask)
#define ARMING_FLAG(mask) (armingFlags & mask)
typedef enum {
ANGLE_MODE = (1 << 0),
HORIZON_MODE = (1 << 1),
MAG_MODE = (1 << 2),
BARO_MODE = (1 << 3),
GPS_HOME_MODE = (1 << 4),
GPS_HOLD_MODE = (1 << 5),
HEADFREE_MODE = (1 << 6),
AUTOTUNE_MODE = (1 << 7),
PASSTHRU_MODE = (1 << 8),
} flightModeFlags_e;
extern uint16_t flightModeFlags;
#define DISABLE_FLIGHT_MODE(mask) (flightModeFlags &= ~(mask))
#define ENABLE_FLIGHT_MODE(mask) (flightModeFlags |= mask)
#define FLIGHT_MODE(mask) (flightModeFlags & mask)
typedef enum {
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
#define ENABLE_STATE(mask) (stateFlags |= mask)
#define STATE(mask) (stateFlags & mask)
extern uint8_t stateFlags;
extern flags_t f;
bool sensors(uint32_t mask);
void sensorsSet(uint32_t mask);

8
src/main/flight/altitudehold.c
View File

@ -105,7 +105,7 @@ static void fixedWingAltHold()
void updateAltHold(void)
{
if (f.FIXED_WING) {
if (STATE(FIXED_WING)) {
fixedWingAltHold();
} else {
multirotorAltHold();
@ -116,15 +116,15 @@ void updateAltHoldState(void)
{
// Baro alt hold activate
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
if (!FLIGHT_MODE(BARO_MODE)) {
ENABLE_FLIGHT_MODE(BARO_MODE);
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorVelocityI = 0;
BaroPID = 0;
}
} else {
f.BARO_MODE = 0;
DISABLE_FLIGHT_MODE(BARO_MODE);
}
}

6
src/main/flight/failsafe.c
View File

@ -105,7 +105,7 @@ void failsafeAvoidRearm(void)
{
// This will prevent the automatic rearm if failsafe shuts it down and prevents
// to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
f.PREVENT_ARMING = 1;
ENABLE_ARMING_FLAG(PREVENT_ARMING);
}
void onValidDataReceived(void)
@ -129,7 +129,7 @@ void updateState(void)
return;
}
if (shouldForceLanding(f.ARMED)) { // Stabilize, and set Throttle to specified level
if (shouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level
failsafeAvoidRearm();
for (i = 0; i < 3; i++) {
@ -139,7 +139,7 @@ void updateState(void)
failsafe.events++;
}
if (shouldHaveCausedLandingByNow() || !f.ARMED) {
if (shouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
mwDisarm();
}
}

16
src/main/flight/flight.c
View File

@ -120,13 +120,13 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
}
#endif
if (f.ANGLE_MODE) {
if (FLIGHT_MODE(ANGLE_MODE)) {
// it's the ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->A_level;
} else {
//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
if (f.HORIZON_MODE) {
if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level;
}
@ -185,7 +185,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// **** PITCH & ROLL & YAW PID ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
@ -207,7 +207,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroData[axis] / 4;
@ -219,11 +219,11 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
}
if (f.HORIZON_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
if (FLIGHT_MODE(HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (f.ANGLE_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
@ -277,9 +277,9 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
}
#endif
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
}

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

@ -239,7 +239,7 @@ void acc_calc(uint32_t deltaT)
rotateV(&accel_ned.V, &rpy);
if (imuRuntimeConfig->acc_unarmedcal == 1) {
if (!f.ARMED) {
if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
@ -329,7 +329,11 @@ static void getEstimatedAttitude(void)
EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
}
f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
if (EstG.A[Z] > smallAngle) {
ENABLE_STATE(SMALL_ANGLE);
} else {
DISABLE_STATE(SMALL_ANGLE);
}
// Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);

16
src/main/flight/mixer.c
View File

@ -296,9 +296,9 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
// set flag that we're on something with wings
if (currentMixerConfiguration == MULTITYPE_FLYING_WING ||
currentMixerConfiguration == MULTITYPE_AIRPLANE)
f.FIXED_WING = 1;
ENABLE_STATE(FIXED_WING);
else
f.FIXED_WING = 0;
DISABLE_STATE(FIXED_WING);
mixerResetMotors();
}
@ -351,7 +351,7 @@ void writeServos(void)
pwmWriteServo(0, servo[5]);
} else {
// otherwise, only move servo when copter is armed
if (f.ARMED)
if (ARMING_FLAG(ARMED))
pwmWriteServo(0, servo[5]);
else
pwmWriteServo(0, 0); // kill servo signal completely.
@ -412,7 +412,7 @@ static void airplaneMixer(void)
int16_t flapperons[2] = { 0, 0 };
int i;
if (!f.ARMED)
if (!ARMING_FLAG(ARMED))
servo[7] = escAndServoConfig->mincommand; // Kill throttle when disarmed
else
servo[7] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
@ -435,7 +435,7 @@ static void airplaneMixer(void)
servo[2] += rxConfig->midrc;
}
if (f.PASSTHRU_MODE) { // Direct passthru from RX
if (FLIGHT_MODE(PASSTHRU_MODE)) { // Direct passthru from RX
servo[3] = rcCommand[ROLL] + flapperons[0]; // Wing 1
servo[4] = rcCommand[ROLL] + flapperons[1]; // Wing 2
servo[5] = rcCommand[YAW]; // Rudder
@ -489,12 +489,12 @@ void mixTable(void)
break;
case MULTITYPE_FLYING_WING:
if (!f.ARMED)
if (!ARMING_FLAG(ARMED))
servo[7] = escAndServoConfig->mincommand;
else
servo[7] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
motor[0] = servo[7];
if (f.PASSTHRU_MODE) {
if (FLIGHT_MODE(PASSTHRU_MODE)) {
// do not use sensors for correction, simple 2 channel mixing
servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]);
servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]);
@ -587,7 +587,7 @@ void mixTable(void)
motor[i] = escAndServoConfig->mincommand;
}
}
if (!f.ARMED) {
if (!ARMING_FLAG(ARMED)) {
motor[i] = motor_disarmed[i];
}
}

40
src/main/flight/navigation.c
View File

@ -255,13 +255,13 @@ void onGpsNewData(void)
uint16_t speed;
if (!(f.GPS_FIX && GPS_numSat >= 5)) {
if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
return;
}
if (!f.ARMED)
f.GPS_FIX_HOME = 0;
if (!f.GPS_FIX_HOME && f.ARMED)
if (!ARMING_FLAG(ARMED))
DISABLE_STATE(GPS_FIX_HOME);
if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
GPS_reset_home_position();
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
@ -296,7 +296,7 @@ void onGpsNewData(void)
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
if (!f.GPS_FIX_HOME) { // If we don't have home set, do not display anything
if (!STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
@ -304,8 +304,10 @@ void onGpsNewData(void)
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE) { // ok we are navigating
// do gps nav calculations here, these are common for nav and poshold
if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
// we are navigating
// gps nav calculations, these are common for nav and poshold
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
@ -345,13 +347,13 @@ void onGpsNewData(void)
void GPS_reset_home_position(void)
{
if (f.GPS_FIX && GPS_numSat >= 5) {
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
nav_takeoff_bearing = heading; // save takeoff heading
// Set ground altitude
f.GPS_FIX_HOME = 1;
ENABLE_STATE(GPS_FIX_HOME);
}
}
@ -648,22 +650,22 @@ void updateGpsWaypointsAndMode(void)
{
static uint8_t GPSNavReset = 1;
if (f.GPS_FIX && GPS_numSat >= 5) {
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
// if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
if (!STATE(GPS_HOME_MODE)) {
ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPSNavReset = 0;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
DISABLE_STATE(GPS_HOME_MODE);
if (rcOptions[BOXGPSHOLD] && areSticksInApModePosition(gpsProfile->ap_mode)) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
ENABLE_STATE(GPS_HOLD_MODE);
GPSNavReset = 0;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
@ -671,7 +673,7 @@ void updateGpsWaypointsAndMode(void)
nav_mode = NAV_MODE_POSHOLD;
}
} else {
f.GPS_HOLD_MODE = 0;
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
// both boxes are unselected here, nav is reset if not already done
if (GPSNavReset == 0) {
GPSNavReset = 1;
@ -680,8 +682,8 @@ void updateGpsWaypointsAndMode(void)
}
}
} else {
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
nav_mode = NAV_MODE_NONE;
}
}

8
src/main/io/beeper.c
View File

@ -71,7 +71,7 @@ void beepcodeUpdateState(bool warn_vbat)
}
//===================== Beeps for failsafe =====================
if (feature(FEATURE_FAILSAFE)) {
if (failsafe->vTable->shouldForceLanding(f.ARMED)) {
if (failsafe->vTable->shouldForceLanding(ARMING_FLAG(ARMED))) {
warn_failsafe = FAILSAFE_LANDING;
if (failsafe->vTable->shouldHaveCausedLandingByNow()) {
@ -79,7 +79,7 @@ void beepcodeUpdateState(bool warn_vbat)
}
}
if (failsafe->vTable->hasTimerElapsed() && !f.ARMED) {
if (failsafe->vTable->hasTimerElapsed() && !ARMING_FLAG(ARMED)) {
warn_failsafe = FAILSAFE_FIND_ME;
}
@ -91,7 +91,7 @@ void beepcodeUpdateState(bool warn_vbat)
#ifdef GPS
//===================== GPS fix notification handling =====================
if (sensors(SENSOR_GPS)) {
if ((rcOptions[BOXGPSHOME] || rcOptions[BOXGPSHOLD]) && !f.GPS_FIX) { // if no fix and gps funtion is activated: do warning beeps
if ((rcOptions[BOXGPSHOME] || rcOptions[BOXGPSHOLD]) && !STATE(GPS_FIX)) { // if no fix and gps funtion is activated: do warning beeps
warn_noGPSfix = 1;
} else {
warn_noGPSfix = 0;
@ -114,7 +114,7 @@ void beepcodeUpdateState(bool warn_vbat)
beep_code('S','S','S','M'); // beeperon
else if (warn_vbat)
beep_code('S','M','M','D');
else if (f.AUTOTUNE_MODE)
else if (FLIGHT_MODE(AUTOTUNE_MODE))
beep_code('S','M','S','M');
else if (toggleBeep > 0)
beep(50); // fast confirmation beep

20
src/main/io/gps.c
View File

@ -318,7 +318,7 @@ void gpsThread(void)
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
f.GPS_FIX = 0;
DISABLE_STATE(GPS_FIX);
gpsSetState(GPS_INITIALIZING);
break;
@ -506,7 +506,11 @@ static bool gpsNewFrameNMEA(char c)
gps_Msg.longitude *= -1;
break;
case 6:
f.GPS_FIX = string[0] > '0';
if (string[0] > '0') {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
@ -543,7 +547,7 @@ static bool gpsNewFrameNMEA(char c)
switch (gps_frame) {
case FRAME_GGA:
frameOK = 1;
if (f.GPS_FIX) {
if (STATE(GPS_FIX)) {
GPS_coord[LAT] = gps_Msg.latitude;
GPS_coord[LON] = gps_Msg.longitude;
GPS_numSat = gps_Msg.numSat;
@ -728,18 +732,22 @@ static bool UBLOX_parse_gps(void)
GPS_coord[LON] = _buffer.posllh.longitude;
GPS_coord[LAT] = _buffer.posllh.latitude;
GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
f.GPS_FIX = next_fix;
if (next_fix) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
_new_position = true;
break;
case MSG_STATUS:
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
if (!next_fix)
f.GPS_FIX = false;
DISABLE_STATE(GPS_FIX);
break;
case MSG_SOL:
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix)
f.GPS_FIX = false;
DISABLE_STATE(GPS_FIX);
GPS_numSat = _buffer.solution.satellites;
// GPS_hdop = _buffer.solution.position_DOP;
break;

12
src/main/io/ledstrip.c
View File

@ -277,7 +277,7 @@ void applyLedModeLayer(void)
if (!(ledConfig->flags & LED_FUNCTION_MODE)) {
if (ledConfig->flags & LED_FUNCTION_ARM_STATE) {
if (!f.ARMED) {
if (!ARMING_FLAG(ARMED)) {
setLedColor(ledIndex, &green);
} else {
setLedColor(ledIndex, &blue);
@ -288,15 +288,15 @@ void applyLedModeLayer(void)
applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors);
if (f.HEADFREE_MODE) {
if (FLIGHT_MODE(HEADFREE_MODE)) {
applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors);
#ifdef MAG
} else if (f.MAG_MODE) {
} else if (FLIGHT_MODE(MAG_MODE)) {
applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors);
#endif
} else if (f.HORIZON_MODE) {
} else if (FLIGHT_MODE(HORIZON_MODE)) {
applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors);
} else if (f.ANGLE_MODE) {
} else if (FLIGHT_MODE(ANGLE_MODE)) {
applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors);
}
}
@ -388,7 +388,7 @@ static void applyLedAnimationLayer(void)
{
const ledConfig_t *ledConfig;
if (f.ARMED) {
if (ARMING_FLAG(ARMED)) {
return;
}

30
src/main/io/rc_controls.c
View File

@ -83,16 +83,21 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
rcSticks = stTmp;
// perform actions
if (throttleStatus == THROTTLE_LOW) {
if (activate[BOXARM] > 0) { // Arming via ARM BOX
if (rcOptions[BOXARM] && f.OK_TO_ARM)
mwArm();
}
}
if (activate[BOXARM] > 0) {
if (activate[BOXARM] > 0) { // Disarming via ARM BOX
if (!rcOptions[BOXARM] && f.ARMED) {
mwDisarm();
// Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) {
if (rcOptions[BOXARM] && ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
// Disarming via ARM BOX
if (!rcOptions[BOXARM]) {
ENABLE_ARMING_FLAG(SEEN_BOXARM_OFF);
if (ARMING_FLAG(ARMED)) {
mwDisarm();
}
}
}
@ -100,10 +105,13 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
return;
}
if (f.ARMED) { // actions during armed
if (ARMING_FLAG(ARMED)) {
// actions during armed
// Disarm on throttle down + yaw
if (activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
mwDisarm();
// Disarm on roll (only when retarded_arm is enabled)
if (retarded_arm && activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
mwDisarm();
@ -174,7 +182,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
// Calibrating Mag
f.CALIBRATE_MAG = 1;
ENABLE_STATE(CALIBRATE_MAG);
return;
}

46
src/main/io/serial_msp.c
View File

@ -466,17 +466,17 @@ static bool processOutCommand(uint8_t cmdMSP)
// the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE FUCKING LOGIC IN THIS, FUCKWADS.
// Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
junk = 0;
tmp = f.ANGLE_MODE << BOXANGLE |
f.HORIZON_MODE << BOXHORIZON |
f.BARO_MODE << BOXBARO |
f.MAG_MODE << BOXMAG |
f.HEADFREE_MODE << BOXHEADFREE |
tmp = FLIGHT_MODE(ANGLE_MODE) << BOXANGLE |
FLIGHT_MODE(HORIZON_MODE) << BOXHORIZON |
FLIGHT_MODE(BARO_MODE) << BOXBARO |
FLIGHT_MODE(MAG_MODE) << BOXMAG |
FLIGHT_MODE(HEADFREE_MODE) << BOXHEADFREE |
rcOptions[BOXHEADADJ] << BOXHEADADJ |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB |
rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME |
f.GPS_HOLD_MODE << BOXGPSHOLD |
f.PASSTHRU_MODE << BOXPASSTHRU |
FLIGHT_MODE(GPS_HOME_MODE) << BOXGPSHOME |
FLIGHT_MODE(GPS_HOLD_MODE) << BOXGPSHOLD |
FLIGHT_MODE(PASSTHRU_MODE) << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON |
rcOptions[BOXLEDMAX] << BOXLEDMAX |
rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
@ -485,7 +485,7 @@ static bool processOutCommand(uint8_t cmdMSP)
rcOptions[BOXOSD] << BOXOSD |
rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
rcOptions[BOXAUTOTUNE] << BOXAUTOTUNE |
f.ARMED << BOXARM;
ARMING_FLAG(ARMED) << BOXARM;
for (i = 0; i < numberBoxItems; i++) {
int flag = (tmp & (1 << availableBoxes[i]));
if (flag)
@ -636,7 +636,7 @@ static bool processOutCommand(uint8_t cmdMSP)
#ifdef GPS
case MSP_RAW_GPS:
headSerialReply(16);
serialize8(f.GPS_FIX);
serialize8(STATE(GPS_FIX));
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
@ -715,7 +715,7 @@ static bool processInCommand(void)
switch (currentPort->cmdMSP) {
case MSP_SELECT_SETTING:
if (!f.ARMED) {
if (!ARMING_FLAG(ARMED)) {
masterConfig.current_profile_index = read8();
if (masterConfig.current_profile_index > 2) {
masterConfig.current_profile_index = 0;
@ -817,21 +817,21 @@ static bool processInCommand(void)
}
break;
case MSP_RESET_CONF:
if (!f.ARMED) {
if (!ARMING_FLAG(ARMED)) {
resetEEPROM();
readEEPROM();
}
break;
case MSP_ACC_CALIBRATION:
if (!f.ARMED)
if (!ARMING_FLAG(ARMED))
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
break;
case MSP_MAG_CALIBRATION:
if (!f.ARMED)
f.CALIBRATE_MAG = 1;
if (!ARMING_FLAG(ARMED))
ENABLE_STATE(CALIBRATE_MAG);
break;
case MSP_EEPROM_WRITE:
if (f.ARMED) {
if (ARMING_FLAG(ARMED)) {
headSerialError(0);
return true;
}
@ -840,13 +840,17 @@ static bool processInCommand(void)
break;
#ifdef GPS
case MSP_SET_RAW_GPS:
f.GPS_FIX = read8();
if (read8()) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update |= 2; // New data signalisation to GPS functions
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break;
case MSP_SET_WP:
wp_no = read8(); //get the wp number
@ -859,8 +863,8 @@ static bool processInCommand(void)
if (wp_no == 0) {
GPS_home[LAT] = lat;
GPS_home[LON] = lon;
f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
f.GPS_FIX_HOME = 1;
DISABLE_FLIGHT_MODE(GPS_HOME_MODE); // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
ENABLE_STATE(GPS_FIX_HOME);
if (alt != 0)
AltHold = alt; // temporary implementation to test feature with apps
} else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
@ -890,7 +894,7 @@ static void mspProcessPort(void)
if (currentPort->c_state == IDLE) {
currentPort->c_state = (c == '$') ? HEADER_START : IDLE;
if (currentPort->c_state == IDLE && !f.ARMED)
if (currentPort->c_state == IDLE && !ARMING_FLAG(ARMED))
evaluateOtherData(c); // if not armed evaluate all other incoming serial data
} else if (currentPort->c_state == HEADER_START) {
currentPort->c_state = (c == 'M') ? HEADER_M : IDLE;

4
src/main/main.c
View File

@ -258,8 +258,8 @@ void init(void)
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
f.SMALL_ANGLE = 1;
f.PREVENT_ARMING = 0;
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly

94
src/main/mw.c
View File

@ -110,14 +110,14 @@ void updateAutotuneState(void)
static bool autoTuneWasUsed = false;
if (rcOptions[BOXAUTOTUNE]) {
if (!f.AUTOTUNE_MODE) {
if (f.ARMED) {
if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
if (ARMING_FLAG(ARMED)) {
if (isAutotuneIdle() || landedAfterAutoTuning) {
autotuneReset();
landedAfterAutoTuning = false;
}
autotuneBeginNextPhase(&currentProfile->pidProfile, currentProfile->pidController);
f.AUTOTUNE_MODE = 1;
ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
autoTuneWasUsed = true;
} else {
if (havePidsBeenUpdatedByAutotune()) {
@ -129,12 +129,12 @@ void updateAutotuneState(void)
return;
}
if (f.AUTOTUNE_MODE) {
if (FLIGHT_MODE(AUTOTUNE_MODE)) {
autotuneEndPhase();
f.AUTOTUNE_MODE = 0;
DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
}
if (!f.ARMED && autoTuneWasUsed) {
if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
landedAfterAutoTuning = true;
}
}
@ -214,7 +214,7 @@ void annexCode(void)
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (f.HEADFREE_MODE) {
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(heading - headFreeModeHold);
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
@ -241,25 +241,29 @@ void annexCode(void)
beepcodeUpdateState(batteryWarningEnabled);
if (f.ARMED) {
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
ENABLE_ARMING_FLAG(OK_TO_ARM);
if (isCalibrating()) {
LED0_TOGGLE;
f.OK_TO_ARM = 0;
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
f.OK_TO_ARM = 1;
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!f.SMALL_ANGLE) {
f.OK_TO_ARM = 0;
if (rcOptions[BOXARM] && !ARMING_FLAG(SEEN_BOXARM_OFF)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (rcOptions[BOXAUTOTUNE]) {
f.OK_TO_ARM = 0;
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (f.OK_TO_ARM) {
if (ARMING_FLAG(OK_TO_ARM)) {
disableWarningLed();
} else {
enableWarningLed(currentTime);
@ -287,24 +291,24 @@ void annexCode(void)
void mwDisarm(void)
{
if (f.ARMED)
f.ARMED = 0;
if (ARMING_FLAG(ARMED))
DISABLE_ARMING_FLAG(ARMED);
}
void mwArm(void)
{
if (f.OK_TO_ARM) {
if (f.ARMED) {
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (!f.PREVENT_ARMING) {
f.ARMED = 1;
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = heading;
return;
}
}
if (!f.ARMED) {
if (!ARMING_FLAG(ARMED)) {
blinkLedAndSoundBeeper(2, 255, 1);
}
}
@ -334,7 +338,7 @@ void handleInflightCalibrationStickPosition(void)
void updateInflightCalibrationState(void)
{
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = false;
}
@ -342,7 +346,7 @@ void updateInflightCalibrationState(void)
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
InflightcalibratingA = 50;
AccInflightCalibrationActive = true;
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
} else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
AccInflightCalibrationMeasurementDone = false;
AccInflightCalibrationSavetoEEProm = true;
}
@ -350,14 +354,14 @@ void updateInflightCalibrationState(void)
void updateMagHold(void)
{
if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
if (abs(rcCommand[YAW]) < 70 && FLIGHT_MODE(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)
if (STATE(SMALL_ANGLE))
rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;
@ -492,25 +496,25 @@ void processRx(void)
if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
if (!FLIGHT_MODE(ANGLE_MODE)) {
resetErrorAngle();
f.ANGLE_MODE = 1;
ENABLE_FLIGHT_MODE(ANGLE_MODE);
}
} else {
f.ANGLE_MODE = 0; // failsafe support
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
}
if (rcOptions[BOXHORIZON]) {
f.ANGLE_MODE = 0;
if (!f.HORIZON_MODE) {
DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) {
resetErrorAngle();
f.HORIZON_MODE = 1;
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else {
f.HORIZON_MODE = 0;
DISABLE_FLIGHT_MODE(HORIZON_MODE);
}
if (f.ANGLE_MODE || f.HORIZON_MODE) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
LED1_ON;
} else {
LED1_OFF;
@ -519,19 +523,19 @@ void processRx(void)
#ifdef MAG
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = heading;
}
} else {
f.MAG_MODE = 0;
DISABLE_FLIGHT_MODE(MAG_MODE);
}
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
f.HEADFREE_MODE = 0;
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
@ -546,13 +550,13 @@ void processRx(void)
#endif
if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1;
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else {
f.PASSTHRU_MODE = 0;
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
}
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
f.HEADFREE_MODE = 0;
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
}
@ -609,20 +613,20 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
if (FLIGHT_MODE(BARO_MODE)) {
updateAltHold();
}
}
#endif
if (currentProfile->throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
}
#ifdef GPS
if (sensors(SENSOR_GPS)) {
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
updateGpsStateForHomeAndHoldMode();
}
}

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

@ -67,14 +67,14 @@ void updateCompass(flightDynamicsTrims_t *magZero)
hmc5883lRead(magADC);
alignSensors(magADC, magADC, magAlign);
if (f.CALIBRATE_MAG) {
if (STATE(CALIBRATE_MAG)) {
tCal = nextUpdateAt;
for (axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = magADC[axis];
magZeroTempMax.raw[axis] = magADC[axis];
}
f.CALIBRATE_MAG = 0;
DISABLE_STATE(CALIBRATE_MAG);
}
if (magInit) { // we apply offset only once mag calibration is done

2
src/main/telemetry/hott.c
View File

@ -175,7 +175,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{
hottGPSMessage->gps_satelites = GPS_numSat;
if (!f.GPS_FIX) {
if (!STATE(GPS_FIX)) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return;
}

2
src/main/telemetry/telemetry.c
View File

@ -105,7 +105,7 @@ bool determineNewTelemetryEnabledState(void)
if (telemetryConfig->telemetry_switch)
enabled = rcOptions[BOXTELEMETRY];
else
enabled = f.ARMED;
enabled = ARMING_FLAG(ARMED);
}
return enabled;