diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8c0bc06d3b..54224194cd 100644 --- a/src/main/flight/mixer.c +++ b/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; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 625b56d89b..e78506b8a7 100644 --- a/src/main/flight/mixer.h +++ b/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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 034739523c..98653e9c8b 100644 --- a/src/main/interface/settings.c +++ b/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) }, diff --git a/src/main/io/gps.c b/src/main/io/gps.c index c415b84e3b..77b28146c9 100644 --- a/src/main/io/gps.c +++ b/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 diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 841a570a40..18126aa5ac 100644 --- a/src/main/io/gps.h +++ b/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); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 907279a883..b0ffc46a6f 100644 --- a/src/main/io/osd.c +++ b/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;