diff --git a/docs/OSD.md b/docs/OSD.md index a03f02a9eb6..c3e270ce8a1 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -199,6 +199,7 @@ Here are the OSD Elements provided by INAV. | 164 | OSD_H_DIST_TO_FENCE | 8.0.0 | | | 165 | OSD_V_DIST_TO_FENCE | 8.0.0 | | | 166 | OSD_NAV_FW_ALT_CONTROL_RESPONSE | 8.0.0 | | +| 167 | OSD_NAV_MIN_GROUND_SPEED | 9.0.0 | | # Pilot Logos diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index c5970be0a43..8fc01178c93 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -102,6 +102,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 53 | Disable GPS Sensor Fix | Disables the GNSS sensor fix. For testing GNSS failure. | | 54 | Mag calibration | Trigger a magnetometer calibration. | | 55 | Set Gimbal Sensitivity | Scales `Operand A` from [`-16` : `15`] +| 56 | Override Minimum Ground Speed | When active, sets the minimum ground speed to the value specified in `Operand A` [m/s]. Minimum allowed value is set in `nav_min_ground_speed`. Maximum value is `150` | ### Operands @@ -164,6 +165,11 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 41 | FW Land Sate | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare | | 42 | Current battery profile | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` | | 43 | Flown Loiter Radius [m] | The actual loiter radius flown by a fixed wing during hold modes, in `meters` | +| 44 | Downlink Link Quality | | +| 45 | Uplink RSSI [dBm] | | +| 46 | Minimum Ground Speed [m/s] | The current minimum ground speed allowed in navigation flight modes | +| 47 | Horizontal Wind Speed [cm/s] | Estimated wind speed. If the wind estimator is unavailble or the wind estimation is invalid, -1 is returned | +| 48 | Wind Direction [deg] | Estimated wind direction. If the wind estimator is unavailble or the wind estimation is invalid, -1 is returned | #### FLIGHT_MODE diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 2dbb53c0a1d..675ff56c2ca 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -35,49 +35,51 @@ #define RAD (M_PIf / 180.0f) -#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) -#define CENTIDEGREES_TO_DEGREES(angle) ((angle) * 0.01f) +#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) +#define CENTIDEGREES_TO_DEGREES(angle) ((angle) * 0.01f) -#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f) -#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10) +#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f) +#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10) -#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) -#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10.0f) +#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) +#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10.0f) -#define DEGREES_PER_DEKADEGREE 10 -#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE) -#define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE) +#define DEGREES_PER_DEKADEGREE 10 +#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE) +#define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE) -#define DEGREES_TO_RADIANS(angle) ((angle) * RAD) -#define RADIANS_TO_DEGREES(angle) ((angle) / RAD) -#define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD) -#define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD) +#define DEGREES_TO_RADIANS(angle) ((angle) * RAD) +#define RADIANS_TO_DEGREES(angle) ((angle) / RAD) +#define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD) +#define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD) -#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) -#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD) +#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) +#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD) -#define MILLIMETERS_TO_CENTIMETERS(mm) (mm / 10.0f) +#define MILLIMETERS_TO_CENTIMETERS(mm) (mm / 10.0f) -#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) -#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) -#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f) +#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) +#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) +#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f) -#define METERS_TO_CENTIMETERS(m) (m * 100) -#define METERS_TO_KILOMETERS(m) (m / 1000.0f) -#define METERS_TO_MILES(m) (m / 1609.344f) -#define METERS_TO_NAUTICALMILES(m) (m / 1852.00f) +#define METERS_TO_CENTIMETERS(m) (m * 100) +#define METERS_TO_KILOMETERS(m) (m / 1000.0f) +#define METERS_TO_MILES(m) (m / 1609.344f) +#define METERS_TO_NAUTICALMILES(m) (m / 1852.00f) -#define MWH_TO_WH(mWh) (mWh / 1000.0f) +#define MWH_TO_WH(mWh) (mWh / 1000.0f) -#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) -#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) -#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) +#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) +#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) +#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) -#define CMSEC_TO_MPH(cms) (CMSEC_TO_CENTIMPH(cms) / 100.0f) -#define CMSEC_TO_KPH(cms) (CMSEC_TO_CENTIKPH(cms) / 100.0f) -#define CMSEC_TO_KNOTS(cms) (CMSEC_TO_CENTIKNOTS(cms) / 100.0f) +#define CMSEC_TO_MPH(cms) (CMSEC_TO_CENTIMPH(cms) / 100.0f) +#define CMSEC_TO_KPH(cms) (CMSEC_TO_CENTIKPH(cms) / 100.0f) +#define CMSEC_TO_KNOTS(cms) (CMSEC_TO_CENTIKNOTS(cms) / 100.0f) -#define C_TO_KELVIN(temp) (temp + 273.15f) +#define KMH_TO_MS(kmh) (kmh / 3.6f) + +#define C_TO_KELVIN(temp) (temp + 273.15f) // Standard Sea Level values // Ref:https://en.wikipedia.org/wiki/Standard_sea_level diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 35b9c61a424..88b7f92ee54 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -176,14 +176,13 @@ #define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining #define SYM_GROUND_COURSE 0xDC // 220 Ground course #define SYM_ALERT 0xDD // 221 General alert symbol +#define SYM_MIN_GROUND_SPEED 0xDE // 222 Minimum Ground Speed #define SYM_TERRAIN_FOLLOWING 0xFB // 251 Terrain following (also Alt adjust) #define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error -#define SYM_ADSB 0xFD // 253 ADBS +#define SYM_ADSB 0xFD // 253 ADSB #define SYM_BLACKBOX 0xFE // 254 Blackbox -#define SYM_ADSB 0xFD // 253 ADSB - #define SYM_LOGO_START 0x101 // 257 to 297, INAV logo #define SYM_LOGO_WIDTH 10 #define SYM_LOGO_HEIGHT 4 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 365e73b1143..86c2e787f43 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -390,38 +390,116 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) * @param vel Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) * @param _3D is a 3D velocity * @param _max is a maximum velocity + * @return length of the formatted string */ -int osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max) +int osdFormatVelocityStr(char* buff, int32_t vel, osd_SpeedTypes_e speedType, bool _max) { + int strLen = 0; + switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (_max) { - return tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } else { - return tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } - break; - case OSD_UNIT_METRIC: - if (_max) { - return tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } else { - return tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } - break; - case OSD_UNIT_GA: - if (_max) { - return tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } else { - return tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } - break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (_max) { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_MPH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + break; + } + } else { + switch (speedType){ + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_MPH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + } + } + break; + case OSD_UNIT_METRIC: + if (_max) { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_KMH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + break; + } + } else { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_KMH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + } + } + break; + case OSD_UNIT_GA: + if (_max) { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_KT); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + break; + } + } else { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_KT); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + } + } + break; } - return 0; + return strLen; } /** @@ -429,7 +507,7 @@ int osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max) */ static void osdGenerateAverageVelocityStr(char* buff) { uint32_t cmPerSec = getTotalTravelDistance() / getFlightTime(); - osdFormatVelocityStr(buff, cmPerSec, false, false); + osdFormatVelocityStr(buff, cmPerSec, OSD_SPEED_TYPE_GROUND, false); } /** @@ -1919,19 +1997,23 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_GPS_SPEED: - osdFormatVelocityStr(buff, gpsSol.groundSpeed, false, false); + osdFormatVelocityStr(buff, gpsSol.groundSpeed, OSD_SPEED_TYPE_GROUND, false); + break; + + case OSD_NAV_MIN_GROUND_SPEED: + osdFormatVelocityStr(buff, getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100, OSD_SPEED_TYPE_MIN_GROUND, false); break; case OSD_GPS_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_speed, false, true); + osdFormatVelocityStr(buff, stats.max_speed, OSD_SPEED_TYPE_GROUND, true); break; case OSD_3D_SPEED: - osdFormatVelocityStr(buff, osdGet3DSpeed(), true, false); + osdFormatVelocityStr(buff, osdGet3DSpeed(), OSD_SPEED_TYPE_3D, false); break; case OSD_3D_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_3D_speed, true, true); + osdFormatVelocityStr(buff, stats.max_3D_speed, OSD_SPEED_TYPE_3D, true); break; case OSD_GLIDESLOPE: @@ -2238,7 +2320,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%s", getAdsbEmitterTypeString(vehicle->vehicleValues.emitterType)); // Vehicle speed - adsbLengthForClearSecondLine = osdFormatVelocityStr(buff + 7, vehicle->vehicleValues.horVelocity, false, false); + adsbLengthForClearSecondLine = osdFormatVelocityStr(buff + 7, vehicle->vehicleValues.horVelocity, OSD_SPEED_TYPE_GROUND, false); // draw values buff[6] = SYM_BLANK; // space for direction arrow @@ -3326,12 +3408,10 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AIR_SPEED: { #ifdef USE_PITOT - buff[0] = SYM_AIR; - if (pitotIsHealthy()) { const float airspeed_estimate = getAirspeedEstimate(); - osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); + osdFormatVelocityStr(buff, airspeed_estimate, OSD_SPEED_TYPE_AIR, false); if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -3339,6 +3419,7 @@ static bool osdDrawSingleElement(uint8_t item) } else { + buff[0] = SYM_AIR; strcpy(buff + 1, " X!"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -3351,9 +3432,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AIR_MAX_SPEED: { #ifdef USE_PITOT - buff[0] = SYM_MAX; - buff[1] = SYM_AIR; - osdFormatVelocityStr(buff + 2, stats.max_air_speed, false, false); + osdFormatVelocityStr(buff, stats.max_air_speed, OSD_SPEED_TYPE_AIR, true); #else return false; #endif @@ -4919,7 +4998,7 @@ uint8_t drawStat_Speed(uint8_t col, uint8_t row, uint8_t statValX) displayWrite(osdDisplayPort, col, row, "MAX/AVG SPEED"); - osdFormatVelocityStr(buff2, stats.max_3D_speed, true, false); + osdFormatVelocityStr(buff2, stats.max_3D_speed, OSD_SPEED_TYPE_3D, false); tfp_sprintf(buff, ": %s/", osdFormatTrimWhiteSpace(buff2)); multiValueXOffset = strlen(buff); displayWrite(osdDisplayPort, statValX, row, buff); @@ -6220,7 +6299,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { char buf[6]; - osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false); + osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, OSD_SPEED_TYPE_GROUND, false); tfp_sprintf(messageBuf, "(SPD %s)", buf); } else { strcpy(messageBuf, "(HOLD)"); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 5e78e8ff406..7c60016d46c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -338,6 +338,7 @@ typedef enum { OSD_H_DIST_TO_FENCE, OSD_V_DIST_TO_FENCE, OSD_NAV_FW_ALT_CONTROL_RESPONSE, + OSD_NAV_MIN_GROUND_SPEED, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -396,6 +397,13 @@ typedef enum { OSD_CRSF_LQ_TYPE3 } osd_crsf_lq_format_e; +typedef enum { + OSD_SPEED_TYPE_GROUND, + OSD_SPEED_TYPE_AIR, + OSD_SPEED_TYPE_3D, + OSD_SPEED_TYPE_MIN_GROUND, +} osd_SpeedTypes_e; + typedef struct osdLayoutsConfig_s { // Layouts uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; @@ -568,7 +576,7 @@ void osdShowEEPROMSavedNotification(void); void osdCrosshairPosition(uint8_t *x, uint8_t *y); bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros); void osdFormatAltitudeSymbol(char *buff, int32_t alt); -int osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max); +int osdFormatVelocityStr(char* buff, int32_t vel, osd_SpeedTypes_e speedType, bool _max); // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4e0fda62dc9..6e070af1740 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -606,10 +606,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) previousTimePositionUpdate = currentTimeUs; if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - float velThrottleBoost = ((navConfig()->general.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); + float velThrottleBoost = ((getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost - if (fabsf(posControl.actualState.velXY - (navConfig()->general.min_ground_speed * 100.0f)) > 50) { + if (fabsf(posControl.actualState.velXY - (getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f)) > 50) { throttleSpeedAdjustment += velThrottleBoost; } diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index e95acf2c4b5..9140cea2e9e 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -47,6 +47,7 @@ #include "flight/imu.h" #include "flight/pid.h" #include "flight/mixer_profile.h" +#include "flight/wind_estimator.h" #include "drivers/io_port_expander.h" #include "drivers/gimbal_common.h" #include "io/osd_common.h" @@ -464,6 +465,12 @@ static int logicConditionCompute( return true; break; + case LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED: + logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED] = constrain(operandA, navConfig()->general.min_ground_speed, 150); + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED); + return true; + break; + case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE: if (operandA >= 0 && operandA <= 2) { @@ -722,6 +729,10 @@ static int logicConditionGetFlightOperandValue(int operand) { return gpsSol.groundSpeed; break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED: // m/s + return getMinGroundSpeed(navConfig()->general.min_ground_speed); + break; + //FIXME align with osdGet3DSpeed case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s return osdGet3DSpeed(); @@ -735,6 +746,42 @@ static int logicConditionGetFlightOperandValue(int operand) { #endif break; + case LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED: // cm/s +#ifdef USE_WIND_ESTIMATOR + { + if (isEstimatedWindSpeedValid()) { + uint16_t angle; + return getEstimatedHorizontalWindSpeed(&angle); + } else + return -1; + } +#else + return -1; +#endif + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION: // deg +#ifdef USE_WIND_ESTIMATOR + { + if (isEstimatedWindSpeedValid()) { + uint16_t angle; + getEstimatedHorizontalWindSpeed(&angle); + int32_t windAngle = (CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22); + while (windAngle < 0) { + windAngle += 360; + } + while (windAngle >= 360) { + windAngle -= 360; + } + return windAngle; + } else + return -1; + } +#else + return -1; +#endif + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm return constrain(getEstimatedActualPosition(Z), INT32_MIN, INT32_MAX); break; @@ -1118,6 +1165,18 @@ uint32_t getLoiterRadius(uint32_t loiterRadius) { #endif } +uint32_t getMinGroundSpeed(uint32_t minGroundSpeed) { +#ifdef USE_PROGRAMMING_FRAMEWORK + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED)) { + return constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED], minGroundSpeed, 150); + } else { + return minGroundSpeed; + } +#else + return minGroundSpeed; +#endif +} + float getFlightAxisAngleOverride(uint8_t axis, float angle) { if (flightAxisOverride[axis].angleTargetActive) { return flightAxisOverride[axis].angleTarget; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index ad35ae9ec6a..f961abf60e2 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -85,7 +85,8 @@ typedef enum { LOGIC_CONDITION_DISABLE_GPS_FIX = 53, LOGIC_CONDITION_RESET_MAG_CALIBRATION = 54, LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY = 55, - LOGIC_CONDITION_LAST = 56, + LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED = 56, + LOGIC_CONDITION_LAST } logicOperation_e; typedef enum logicOperandType_s { @@ -147,6 +148,9 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43 LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK, // 44 LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM, // 45 + LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED, // m/s // 46 + LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED, // cm/s // 47 + LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION, // deg // 48 } logicFlightOperands_e; typedef enum { @@ -201,6 +205,7 @@ typedef enum { #ifdef USE_GPS_FIX_ESTIMATION LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), #endif + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED = (1 << 12), } logicConditionsGlobalFlags_t; typedef enum { @@ -262,6 +267,7 @@ float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); uint32_t getLoiterRadius(uint32_t loiterRadius); +uint32_t getMinGroundSpeed(uint32_t minGroundSpeed); float getFlightAxisAngleOverride(uint8_t axis, float angle); float getFlightAxisRateOverride(uint8_t axis, float rate); bool isFlightAxisAngleOverrideActive(uint8_t axis);