Merge pull request #4619 from martinbudden/bfa_use_alt_hold

Added USE_ALT_HOLD build flag

src/main/fc/fc_core.c

@@ -617,7 +617,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
     }
 #endif
 
-#if defined(USE_BARO) || defined(USE_SONAR)
+#if defined(USE_ALT_HOLD)
     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
     updateRcCommands();
     if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {

src/main/fc/fc_tasks.c

@@ -140,12 +140,13 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
     processRx(currentTimeUs);
     isRXDataNew = true;
 
-#if !defined(USE_BARO) && !defined(USE_SONAR)
+#if !defined(USE_ALT_HOLD)
     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
     updateRcCommands();
 #endif
     updateArmingStatus();
 
+#ifdef USE_ALT_HOLD
 #ifdef USE_BARO
     if (sensors(SENSOR_BARO)) {
         updateAltHoldState();
@@ -157,6 +158,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
         updateSonarAltHoldState();
     }
 #endif
+#endif // USE_ALT_HOLD
 }
 #endif
 
@@ -196,7 +198,7 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs)
         ) {
         calculateEstimatedAltitude(currentTimeUs);
     }}
-#endif
+#endif // USE_BARO || USE_SONAR
 
 #ifdef USE_TELEMETRY
 static void taskTelemetry(timeUs_t currentTimeUs)

src/main/flight/altitude.c

@@ -52,13 +52,16 @@ static int32_t estimatedVario = 0;                      // variometer in cm/s
 static int32_t estimatedAltitude = 0;                // in cm
 
 
-#if defined(USE_BARO) || defined(USE_SONAR)
 
 enum {
     DEBUG_ALTITUDE_ACC,
     DEBUG_ALTITUDE_VEL,
     DEBUG_ALTITUDE_HEIGHT
 };
+// 40hz update rate (20hz LPF on acc)
+#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
+
+#if defined(USE_ALT_HOLD)
 
 PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
 
@@ -72,8 +75,6 @@ static int32_t errorVelocityI = 0;
 static int32_t altHoldThrottleAdjustment = 0;
 static int16_t initialThrottleHold;
 
-// 40hz update rate (20hz LPF on acc)
-#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
 
 #define DEGREES_80_IN_DECIDEGREES 800
 
@@ -202,7 +203,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
 
     return result;
 }
+#endif // USE_ALT_HOLD
 
+#if defined(USE_BARO) || defined(USE_SONAR)
 void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 {
     static timeUs_t previousTimeUs = 0;
@@ -280,7 +283,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
         baroVel = constrain(baroVel, -1500, 1500);  // constrain baro velocity +/- 1500cm/s
         baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero
     }
-#endif // BARO
+#endif // USE_BARO
 
     // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
     // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
@@ -290,11 +293,15 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
     // set vario
     estimatedVario = applyDeadband(vel_tmp, 5);
 
+#ifdef USE_ALT_HOLD
     static float accZ_old = 0.0f;
     altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
     accZ_old = accZ_tmp;
+#else
+    UNUSED(accZ_tmp);
+#endif
 }
-#endif // defined(USE_BARO) || defined(USE_SONAR)
+#endif // USE_BARO || USE_SONAR
 
 int32_t getEstimatedAltitude(void)
 {

src/main/interface/settings.c

@@ -616,18 +616,19 @@ const clivalue_t valueTable[] = {
     { "i_yaw",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) },
     { "d_yaw",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
 
+#ifdef USE_ALT_HOLD
     { "p_alt",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].P) },
     { "i_alt",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].I) },
     { "d_alt",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].D) },
+    { "p_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) },
+    { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) },
+    { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) },
+#endif
 
     { "p_level",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
     { "i_level",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
     { "d_level",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
 
-    { "p_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) },
-    { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) },
-    { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) },
-
     { "level_limit",                VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) },
 
     { "horizon_tilt_effect",        VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0,  250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },

src/main/target/common_fc_post.h

@@ -61,6 +61,11 @@
 #endif
 #endif
 
+// undefine USE_ALT_HOLD if there is no baro or sonar to support it
+#if defined(USE_ALT_HOLD) && !defined(USE_BARO) && !defined(USE_SONAR)
+#undef USE_ALT_HOLD
+#endif
+
 /* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */
 #if !defined(VTX_COMMON) || !defined(VTX_CONTROL)
 #undef VTX_COMMON

src/main/target/common_fc_pre.h

@@ -148,6 +148,7 @@
 #define USE_GPS_UBLOX
 #define USE_GPS_NMEA
 #define USE_NAV
+#define USE_ALT_HOLD
 #define USE_UNCOMMON_MIXERS
 #define USE_OSD_ADJUSTMENTS
 #endif