From 7ad65031e2838f711f130c383823e43ed6d7f16a Mon Sep 17 00:00:00 2001 From: s0up Date: Sun, 20 May 2018 19:14:09 -0700 Subject: [PATCH] revert failsafe code, re-implement commented unit tests, add degrees suffix for min/max angle in rescue mode --- src/main/flight/gps_rescue.c | 18 +++++++++--------- src/main/flight/gps_rescue.h | 4 ++-- src/test/unit/flight_failsafe_unittest.cc | 19 ++++++++----------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 2b18e88954..e78d0473c6 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -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)); diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 66879a61a7..dc9a6c7749 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -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; diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index a4f09e7e4d..96bc03abfe 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -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()); }