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.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 10;
rescueState.intent.maxAngle = 15;
rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 15;
break;
case RESCUE_CROSSTRACK:
if (rescueState.sensor.distanceToHome < gpsRescue()->descentDistance) {
@ -113,8 +113,8 @@ void updateGPSRescueState(void)
rescueState.intent.targetGroundspeed = gpsRescue()->rescueGroundspeed;
rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 15;
rescueState.intent.maxAngle = gpsRescue()->angle;
rescueState.intent.minAngleDeg = 15;
rescueState.intent.maxAngleDeg = gpsRescue()->angle;
break;
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
@ -129,8 +129,8 @@ void updateGPSRescueState(void)
rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude);
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 10;
rescueState.intent.maxAngle = 20;
rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 20;
break;
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.
@ -145,8 +145,8 @@ void updateGPSRescueState(void)
rescueState.intent.targetGroundspeed = 0;
rescueState.intent.targetAltitude = 0;
rescueState.intent.crosstrack = true;
rescueState.intent.minAngle = 0;
rescueState.intent.maxAngle = 15;
rescueState.intent.minAngleDeg = 0;
rescueState.intent.maxAngleDeg = 15;
break;
case RESCUE_COMPLETE:
rescueStop();
@ -319,7 +319,7 @@ void rescueAttainPosition()
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));

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

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

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

@ -266,10 +266,9 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
// then
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(1, CALL_COUNTER(COUNTER_MW_DISARM));
//EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
@ -280,11 +279,10 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
failsafeUpdateState();
// 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(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
//EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
//EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// given
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
EXPECT_EQ(false, failsafeIsActive());
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());
}