Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

revert failsafe code, re-implement commented unit tests, add degrees suffix for min/max angle in rescue mode

Browse source
This commit is contained in:
s0up 2018-05-20 19:14:09 -07:00
parent cc1bb05a1e
commit 7ad65031e2
3 changed files with 19 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/flight/gps_rescue.c
View file

@ -100,8 +100,8 @@ void updateGPSRescueState(void)
rescueState.intent.targetGroundspeed = 500; rescueState.intent.targetGroundspeed = 500;
rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.crosstrack = true; rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 10; rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngle = 15; rescueState.intent.maxAngleDeg = 15;
break; break;
case RESCUE_CROSSTRACK: case RESCUE_CROSSTRACK:
if (rescueState.sensor.distanceToHome < gpsRescue()->descentDistance) { if (rescueState.sensor.distanceToHome < gpsRescue()->descentDistance) {
@ -113,8 +113,8 @@ void updateGPSRescueState(void)
rescueState.intent.targetGroundspeed = gpsRescue()->rescueGroundspeed; rescueState.intent.targetGroundspeed = gpsRescue()->rescueGroundspeed;
rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.crosstrack = true; rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 15; rescueState.intent.minAngleDeg = 15;
rescueState.intent.maxAngle = gpsRescue()->angle; rescueState.intent.maxAngleDeg = gpsRescue()->angle;
break; break;
case RESCUE_LANDING_APPROACH: case RESCUE_LANDING_APPROACH:
// We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase
@ -129,8 +129,8 @@ void updateGPSRescueState(void)
rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude); rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude);
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
rescueState.intent.crosstrack = true; rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 10; rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngle = 20; rescueState.intent.maxAngleDeg = 20;
break; break;
case RESCUE_LANDING: case RESCUE_LANDING:
// We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data. // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data.
@ -145,8 +145,8 @@ void updateGPSRescueState(void)
rescueState.intent.targetGroundspeed = 0; rescueState.intent.targetGroundspeed = 0;
rescueState.intent.targetAltitude = 0; rescueState.intent.targetAltitude = 0;
rescueState.intent.crosstrack = true; rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 0; rescueState.intent.minAngleDeg = 0;
rescueState.intent.maxAngle = 15; rescueState.intent.maxAngleDeg = 15;
break; break;
case RESCUE_COMPLETE: case RESCUE_COMPLETE:
rescueStop(); rescueStop();
@ -319,7 +319,7 @@ void rescueAttainPosition()
int16_t angleAdjustment = gpsRescue()->velP * speedError + (gpsRescue()->velI * speedIntegral) / 100 + gpsRescue()->velD * speedDerivative; int16_t angleAdjustment = gpsRescue()->velP * speedError + (gpsRescue()->velI * speedIntegral) / 100 + gpsRescue()->velD * speedDerivative;
gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngle * 100, rescueState.intent.maxAngle * 100); gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100);
float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10)); float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10));

4
src/main/flight/gps_rescue.h
View file

@ -41,8 +41,8 @@ typedef enum {
typedef struct { typedef struct {
int32_t targetAltitude; int32_t targetAltitude;
int32_t targetGroundspeed; int32_t targetGroundspeed;
uint8_t minAngle; //NOTE: ANGLES ARE IN DEGREES uint8_t minAngleDeg;
uint8_t maxAngle; //NOTE: ANGLES ARE IN DEGREES uint8_t maxAngleDeg;
bool crosstrack; bool crosstrack;
} rescueIntent_s; } rescueIntent_s;

19
src/test/unit/flight_failsafe_unittest.cc
View file

@ -266,10 +266,9 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
// then // then
EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(true, failsafeIsActive());
// These checks were removed as they broke with introduction of rescue mode. The failsafe code should be refactored. EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
//EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
//EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_TRUE(isArmingDisabled());
//EXPECT_TRUE(isArmingDisabled());
// given // given
failsafeOnValidDataFailed(); // set last invalid sample at current time failsafeOnValidDataFailed(); // set last invalid sample at current time
@ -280,11 +279,10 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
failsafeUpdateState(); failsafeUpdateState();
// then // then
// These checks were removed as they broke with introduction of rescue mode. The failsafe code should be refactored. EXPECT_EQ(true, failsafeIsActive());
//EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
//EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
//EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_TRUE(isArmingDisabled());
//EXPECT_TRUE(isArmingDisabled());
// given // given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
@ -296,8 +294,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
// then // then
EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
// These checks were removed as they broke with introduction of rescue mode. The failsafe code should be refactored. EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
//EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(isArmingDisabled()); EXPECT_FALSE(isArmingDisabled());
} }