Altitude hold for 4.6 (#13816)

src/test/Makefile

@@ -458,6 +458,14 @@ rx_spi_expresslrs_telemetry_unittest_DEFINES := \
 		USE_GPS= \
 		USE_MSP_OVER_TELEMETRY= \
 
+althold_unittest_SRC := \
+		$(USER_DIR)/flight/alt_hold.c \
+		$(USER_DIR)/common/maths.c \
+		$(USER_DIR)/pg/rx.c
+
+althold_unittest_DEFINES := \
+		USE_ALT_HOLD_MODE= \
+
 vtx_msp_unittest_SRC := \
         $(USER_DIR)/common/crc.c \
 		$(USER_DIR)/common/streambuf.c \

src/test/unit/althold_unittest.cc

@@ -0,0 +1,134 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <limits.h>
+
+extern "C" {
+
+    #include "platform.h"
+    #include "build/debug.h"
+    #include "pg/pg_ids.h"
+
+    #include "fc/core.h"
+    #include "fc/rc_controls.h"
+    #include "fc/runtime_config.h"
+
+    #include "flight/alt_hold.h"
+    #include "flight/failsafe.h"
+    #include "flight/imu.h"
+    #include "flight/position.h"
+
+    #include "rx/rx.h"
+
+    #include "sensors/acceleration.h"
+
+    PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
+    PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
+    PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
+
+    extern altHoldState_t altHoldState;
+    void altHoldReset(void);
+    void altHoldProcessTransitions(void);
+    void altHoldInit(void);
+    void updateAltHoldState(timeUs_t);
+    bool failsafeIsActive(void) { return false; }
+    timeUs_t currentTimeUs = 0;
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+uint32_t millisRW;
+uint32_t millis() {
+    return millisRW;
+}
+
+TEST(AltholdUnittest, altHoldTransitionsTest)
+{
+    updateAltHoldState(currentTimeUs);
+    EXPECT_EQ(altHoldState.isAltHoldActive, false);
+
+    flightModeFlags |= ALT_HOLD_MODE;
+    millisRW = 42;
+    updateAltHoldState(currentTimeUs);
+    EXPECT_EQ(altHoldState.isAltHoldActive, true);
+
+    flightModeFlags ^= ALT_HOLD_MODE;
+    millisRW = 56;
+    updateAltHoldState(currentTimeUs);
+    EXPECT_EQ(altHoldState.isAltHoldActive, false);
+
+    flightModeFlags |= ALT_HOLD_MODE;
+    millisRW = 64;
+    updateAltHoldState(currentTimeUs);
+    EXPECT_EQ(altHoldState.isAltHoldActive, true);
+}
+
+TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter)
+{
+    altHoldInit();
+    EXPECT_EQ(altHoldState.isAltHoldActive, false);
+
+    flightModeFlags |= ALT_HOLD_MODE;
+    millisRW = 42;
+    updateAltHoldState(currentTimeUs);
+    EXPECT_EQ(altHoldState.isAltHoldActive, true);
+}
+
+// STUBS
+
+extern "C" {
+    acc_t acc;
+
+    void pt2FilterInit(pt2Filter_t *altHoldDeltaLpf, float) {
+        UNUSED(altHoldDeltaLpf);
+    }
+    float pt2FilterGain(float, float) {
+        return 0.0f;
+    }
+    float pt2FilterApply(pt2Filter_t *altHoldDeltaLpf, float) {
+        UNUSED(altHoldDeltaLpf);
+        return 0.0f;
+    }
+
+    bool isAltitudeAvailable(void) { return true; }
+    float getAltitude(void) { return 0.0f; }
+    bool isAltitudeLow(void) { return true; }
+    float getCosTiltAngle(void) { return 0.0f; }
+    float rcCommand[4];
+
+    float getRcDeflection(int)
+    {
+        return 0;
+    }
+
+    void parseRcChannels(const char *input, rxConfig_t *rxConfig)
+    {
+        UNUSED(input);
+        UNUSED(rxConfig);
+    }
+
+    int16_t debug[DEBUG16_VALUE_COUNT];
+    uint8_t debugMode;
+
+    uint8_t armingFlags = 0;
+    uint8_t stateFlags = 0;
+    uint16_t flightModeFlags = 0;
+}

src/test/unit/arming_prevention_unittest.cc

@@ -1160,5 +1160,6 @@ extern "C" {
         return 0.0f;
     }
     void getRcDeflectionAbs(void) {}
-    uint32_t getCpuPercentageLate(void) { return 0; };
+    uint32_t getCpuPercentageLate(void) { return 0; }
+    bool isAltitudeLow(void) {return false ;};
 }

src/test/unit/flight_failsafe_unittest.cc

@@ -81,7 +81,7 @@ void configureFailsafe(void)
     rxConfigMutable()->mincheck = TEST_MIN_CHECK;
 
     failsafeConfigMutable()->failsafe_delay = 10; // 1 second
-    failsafeConfigMutable()->failsafe_off_delay = 15; // 1.5 seconds
+    failsafeConfigMutable()->failsafe_landing_time = 1; // 1.0 seconds
     failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1;
     failsafeConfigMutable()->failsafe_throttle = 1200;
     failsafeConfigMutable()->failsafe_throttle_low_delay = 100; // 10 seconds
@@ -233,7 +233,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
 // note this test follows on from the previous test
 {
     // exceed the stage 2 landing time
-    sysTickUptime += (failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND);
+    sysTickUptime += (failsafeConfig()->failsafe_landing_time * MILLIS_PER_SECOND);
     failsafeOnValidDataFailed();                    // confirm that we still have no valid data
 
     // when
@@ -572,8 +572,8 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
     
-    // should stay in landing for failsafe_off_delay (stage 2 period) of 1s
-    sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
+    // should stay in landing for failsafe_landing_time (stage 2 landing period) of 1s
+    sysTickUptime += failsafeConfig()->failsafe_landing_time * MILLIS_PER_SECOND;
 
     // when
     failsafeUpdateState();

src/test/unit/pid_unittest.cc

@@ -65,7 +65,8 @@ extern "C" {
     #include "flight/mixer.h"
     #include "flight/pid.h"
     #include "flight/pid_init.h"
-
+    #include "flight/position.h"
+    
     #include "io/gps.h"
 
     #include "pg/pg.h"
@@ -85,6 +86,7 @@ extern "C" {
 
     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);
+    PG_REGISTER(positionConfig_t, positionConfig, PG_SYSTEM_CONFIG, 4);
 
     bool unitLaunchControlActive = false;
     launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
@@ -94,10 +96,13 @@ extern "C" {
     bool isAirmodeActivated(void) { return simulatedAirmodeEnabled; }
     float getRcDeflectionAbs(int axis) { return fabsf(simulatedRcDeflection[axis]); }
 
-    // used by ezDisarm auto-disarm code
+    // used by auto-disarm code
     float getMaxRcDeflectionAbs() { return fabsf(simulatedMaxRcDeflectionAbs); }
     float mixerGetRcThrottle() { return fabsf(simulatedMixerGetRcThrottle); }
 
+
+    bool isAltitudeLow(void) { return false; }
+
     void systemBeep(bool) { }
     bool gyroOverflowDetected(void) { return false; }
     float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }