Add alti limiter For french Regulation

src/main/flight/mixer.c

@@ -48,6 +48,10 @@
 
 #include "io/motors.h"
 
+#ifdef USE_BARO
+#include "io/gps.h"
+#endif
+
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
@@ -62,9 +66,12 @@
 #include "flight/mixer.h"
 #include "flight/mixer_tricopter.h"
 #include "flight/pid.h"
+#include "flight/position.h"
 
 #include "rx/rx.h"
-
+#ifdef USE_BARO
+#include "sensors/barometer.h"
+#endif
 #include "sensors/battery.h"
 #include "sensors/gyro.h"
 
@@ -74,11 +81,14 @@ PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
 #define TARGET_DEFAULT_MIXER    MIXER_QUADX
 #endif
 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
-                  .mixerMode = TARGET_DEFAULT_MIXER,
-                  .yaw_motors_reversed = false,
-                  .crashflip_motor_percent = 0,
-                  .crashflip_power_percent = 70,
-                 );
+    .mixerMode = TARGET_DEFAULT_MIXER,
+    .yaw_motors_reversed = false,
+    .crashflip_motor_percent = 0,
+    .crashflip_power_percent = 70,
+    .alti_cutoff = 50,
+    .alti_start_lim = 40,
+    .altiLimiter = false
+);
 
 PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
 
@@ -129,6 +139,9 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS];
 mixerMode_e currentMixerMode;
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 
+static uint8_t altiLimStatus = 0;
+
+
 static FAST_RAM_ZERO_INIT int throttleAngleCorrection;
 
 
@@ -825,6 +838,30 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) {
     }
 #endif
     loggingThrottle = throttle;
+
+#if defined(USE_BARO) || defined(USE_GPS)
+  if(isAltiLimit()){
+      if(((gpsIsHealthy() && gpsSol.numSat > 7) || isBaroReady())
+        && (mixerConfig()->alti_cutoff > 0 && mixerConfig()->alti_start_lim > 0)){
+            if (getEstimatedAltitude() > (mixerConfig()->alti_cutoff*100)){
+                throttle = 0.0f;
+                altiLimStatus = 1;
+            }else if(getEstimatedAltitude() > (mixerConfig()->alti_start_lim*100)){
+                float limitingRatio = 0.4f * ((mixerConfig()->alti_cutoff*100) - getEstimatedAltitude()) / ((mixerConfig()->alti_cutoff*100) - (mixerConfig()->alti_start_lim*100));
+                limitingRatio = constrainf(limitingRatio, 0.0f, 1.0f);
+                throttle = constrainf(limitingRatio, 0.0f, throttle);
+                altiLimStatus = 1;
+            }else {
+                altiLimStatus = 0;
+            }
+      }else{
+        altiLimStatus = 2;
+      }
+  }else{
+        altiLimStatus = 0;
+  }
+#endif
+
     motorMixRange = motorMixMax - motorMixMin;
     if (motorMixRange > 1.0f && (hardwareMotorType != MOTOR_BRUSHED)) {
         for (int i = 0; i < motorCount; i++) {
@@ -903,3 +940,13 @@ void mixerSetThrottleAngleCorrection(int correctionValue) {
 float mixerGetLoggingThrottle(void) {
     return loggingThrottle;
 }
+
+uint8_t getThrottleLimitationStatus(void)
+{
+    return altiLimStatus;
+}
+
+bool isAltiLimit(void)
+{
+  return mixerConfig()->altiLimiter;
+}

src/main/flight/mixer.h

@@ -91,6 +91,9 @@ typedef struct mixerConfig_s {
     bool yaw_motors_reversed;
     uint8_t crashflip_motor_percent;
     uint8_t crashflip_power_percent;
+    uint16_t alti_cutoff;
+    uint16_t alti_start_lim;
+    bool altiLimiter;
 } mixerConfig_t;
 
 PG_DECLARE(mixerConfig_t, mixerConfig);
@@ -138,3 +141,5 @@ bool mixerIsTricopter(void);
 
 void mixerSetThrottleAngleCorrection(int correctionValue);
 float mixerGetLoggingThrottle(void);
+uint8_t getThrottleLimitationStatus(void);
+bool isAltiLimit(void);

src/main/interface/settings.c

@@ -721,6 +721,9 @@ const clivalue_t valueTable[] = {
     { "yaw_motors_reversed",        VAR_INT8  |  MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) },
     { "crashflip_motor_percent",    VAR_UINT8 |  MASTER_VALUE,  .config.minmax = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) },
     { "crashflip_power_percent",    VAR_UINT8 |  MASTER_VALUE,  .config.minmax = { 25, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_power_percent) },
+    { "alti_cutoff",                VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, alti_cutoff) },
+    { "alti_start_lim",             VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, alti_start_lim) },
+    { "altiLimiter",                VAR_INT8  |  MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, altiLimiter) },
 
 // PG_MOTOR_3D_CONFIG
     { "3d_deadband_low",            VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) },

src/main/io/gps.c

@@ -533,7 +533,11 @@ bool gpsNewFrame(uint8_t c) {
     return false;
 }
 
-
+// Check for healthy communications
+bool gpsIsHealthy()
+{
+    return (gpsData.state == GPS_RECEIVING_DATA);
+}
 /* This is a light implementation of a GPS frame decoding
    This should work with most of modern GPS devices configured to output 5 frames.
    It assumes there are some NMEA GGA frames to decode on the serial bus

src/main/io/gps.h

@@ -155,6 +155,7 @@ extern uint8_t GPS_svinfo_cno[16];         // Carrier to Noise Ratio (Signal Str
 void gpsInit(void);
 void gpsUpdate(timeUs_t currentTimeUs);
 bool gpsNewFrame(uint8_t c);
+bool gpsIsHealthy(void); // Check for healthy communications
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
 void onGpsNewData(void);

src/main/io/osd.c

@@ -72,9 +72,9 @@
 
 #include "flight/position.h"
 #include "flight/imu.h"
-#ifdef USE_ESC_SENSOR
+//#ifdef USE_ESC_SENSOR
 #include "flight/mixer.h"
-#endif
+//#endif
 #include "flight/pid.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
@@ -799,8 +799,23 @@ static bool osdDrawSingleElement(uint8_t item) {
     case OSD_WARNINGS: {
 #define OSD_WARNINGS_MAX_SIZE 11
 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
-        STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size);
-        const batteryState_e batteryState = getBatteryState();
+
+            STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size);
+
+            const batteryState_e batteryState = getBatteryState();
+
+            // Show warning if no altitude limitation applicable
+            if (getThrottleLimitationStatus() == 2) {
+                tfp_sprintf(buff,"NO ALTI LIM");
+                SET_BLINK(OSD_WARNINGS);
+            } else if(getThrottleLimitationStatus() == 1) {
+                tfp_sprintf(buff, "ALTI LIM");
+                SET_BLINK(OSD_WARNINGS);
+
+            } else {
+              CLR_BLINK(OSD_WARNINGS);
+            }
+
 #ifdef USE_DSHOT
         if (isTryingToArm() && !ARMING_FLAG(ARMED)) {
             int armingDelayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - micros()) / 1e5;