From f25538f8920ff30b8d7e4f1cafb3da081146402e Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 1 Aug 2022 17:56:14 +0200 Subject: [PATCH] Add USER3 mode (#8276) --- src/main/common/log.h | 20 ++--- src/main/drivers/osd_symbols.h | 14 ++-- src/main/fc/settings.yaml | 3 + src/main/io/osd.c | 91 ++++++++++++---------- src/main/io/osd.h | 1 + src/main/navigation/navigation_fixedwing.c | 2 +- src/main/sensors/gyro.c | 4 +- src/main/target/common.h | 4 +- 8 files changed, 77 insertions(+), 62 deletions(-) diff --git a/src/main/common/log.h b/src/main/common/log.h index 55d8efaaf94..bc6795e5dd9 100644 --- a/src/main/common/log.h +++ b/src/main/common/log.h @@ -48,40 +48,40 @@ void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) __attribute__ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size); #if defined(USE_LOG) -#define LOG_E(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, fmt, ##__VA_ARGS__) -#define LOG_BUFFER_E(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, buf, size) +#define LOG_ERROR(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, fmt, ##__VA_ARGS__) +#define LOG_BUFFER_ERROR(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, buf, size) #else #define LOG_ERROR(...) #define LOG_BUFFER_ERROR(...) #endif #if defined(USE_LOG) -#define LOG_W(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, fmt, ##__VA_ARGS__) -#define LOG_BUF_W(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, buf, size) +#define LOG_WARNING(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, fmt, ##__VA_ARGS__) +#define LOG_BUF_WARNING(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, buf, size) #else #define LOG_WARNING(...) #define LOG_BUF_WARNING(...) #endif #if defined(USE_LOG) -#define LOG_I(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, fmt, ##__VA_ARGS__) -#define LOG_BUF_I(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, buf, size) +#define LOG_INFO(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, fmt, ##__VA_ARGS__) +#define LOG_BUF_INFO(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, buf, size) #else #define LOG_INFO(...) #define LOG_BUF_INFO(...) #endif #if defined(USE_LOG) -#define LOG_V(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, fmt, ##__VA_ARGS__) -#define LOG_BUF_V(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, buf, size) +#define LOG_VERBOSE(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, fmt, ##__VA_ARGS__) +#define LOG_BUF_VERBOSE(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, buf, size) #else #define LOG_VERBOSE(...) #define LOG_BUF_VERBOSE(...) #endif #if defined(USE_LOG) -#define LOG_D(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, fmt, ##__VA_ARGS__) -#define LOG_BUF_D(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, buf, size) +#define LOG_DEBUG(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, fmt, ##__VA_ARGS__) +#define LOG_BUF_DEBUG(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, buf, size) #else #define LOG_DEBUG(...) #define LOG_BUF_DEBUG(...) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index b267718f54a..b9881cf3cc3 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -165,14 +165,12 @@ #define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Mid #define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Low #define SYM_AH 0xD3 // 211 Amphours symbol -#define SYM_GLIDE_DIST 0xD0 // 208 Glide Distance -#define SYM_GLIDE_MINS 0xD1 // 209 Glide Minutes - -#define SYM_MAH_V_FT_0 0xD5 // 213 mAh/v-ft left -#define SYM_MAH_V_FT_1 0xD6 // 214 mAh/v-ft right -#define SYM_MAH_V_M_0 0xD7 // 215 mAh/v-m left -#define SYM_MAH_V_M_1 0xD8 // 216 mAh/v-m right - +#define SYM_GLIDE_DIST 0xD4 // 212 Glide Distance +#define SYM_GLIDE_MINS 0xD5 // 213 Glide Minutes +#define SYM_AH_V_FT_0 0xD6 // 214 mAh/v-ft left +#define SYM_AH_V_FT_1 0xD7 // 215 mAh/v-ft right +#define SYM_AH_V_M_0 0xD8 // 216 mAh/v-m left +#define SYM_AH_V_M_1 0xD9 // 217 mAh/v-m right #define SYM_LOGO_START 0x101 // 257 to 280, INAV logo #define SYM_LOGO_WIDTH 6 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 41acab0be5f..0af6fe63e77 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -190,6 +190,9 @@ tables: - name: dynamic_gyro_notch_mode values: ["2D", "3D_R", "3D_P", "3D_Y", "3D_RP", "3D_RY", "3D_PY", "3D"] enum: dynamicGyroNotchMode_e + - name: nav_fw_wp_turn_smoothing + values: ["OFF", "ON", "ON-CUT"] + enum: wpFwTurnSmoothing_e constants: RPYL_PID_MIN: 0 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index cac854222d2..f988652b6cc 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1097,13 +1097,21 @@ static inline int32_t osdGetAltitudeMsl(void) } uint16_t osdGetRemainingGlideTime(void) { - int32_t value = getEstimatedActualVelocity(Z); + float value = getEstimatedActualVelocity(Z); + static pt1Filter_t glideTimeFilterState; + const timeMs_t curTimeMs = millis(); + static timeMs_t glideTimeUpdatedMs; + + value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); + glideTimeUpdatedMs = curTimeMs; + if (value < 0) { - value = osdGetAltitude() / abs(value); + value = osdGetAltitude() / abs((int)value); } else { value = 0; } - return value; + + return (uint16_t)round(value); } static bool osdIsHeadingValid(void) @@ -2303,21 +2311,21 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_CLIMB_EFFICIENCY: { - // amperage is in centi amps, vertical speed is in cms/s. We want - // mah/dist only to show when vertical speed > 1m/s. - static pt1Filter_t eFilterState; - static timeUs_t efficiencyUpdated = 0; + // amperage is in centi amps (10mA), vertical speed is in cms/s. We want + // Ah/dist only to show when vertical speed > 1m/s. + static pt1Filter_t veFilterState; + static timeUs_t vEfficiencyUpdated = 0; int32_t value = 0; - bool moreThanAh = false; timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated); if (getEstimatedActualVelocity(Z) > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / getEstimatedActualVelocity(Z)) / 0.0036f, 1, US2S(efficiencyTimeDelta)); + if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD + value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); - efficiencyUpdated = currentTimeUs; + vEfficiencyUpdated = currentTimeUs; } else { - value = eFilterState.state; + value = veFilterState.state; } } bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100); @@ -2328,16 +2336,13 @@ static bool osdDrawSingleElement(uint8_t item) FALLTHROUGH; case OSD_UNIT_IMPERIAL: // mAh/foot - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_FOOT / 10, 1, 0, 2, 3); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_FT_0, SYM_MAH_V_FT_1); + if (efficiencyValid) { + osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_MI_0; - buff[4] = SYM_MAH_MI_1; + buff[3] = SYM_AH_V_FT_0; + buff[4] = SYM_AH_V_FT_1; buff[5] = '\0'; } break; @@ -2345,42 +2350,50 @@ static bool osdDrawSingleElement(uint8_t item) FALLTHROUGH; case OSD_UNIT_METRIC: // mAh/metre - moreThanAh = osdFormatCentiNumber(buff, value * 100, 1, 0, 2, 3); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_M_0, SYM_MAH_V_M_1); + if (efficiencyValid) { + osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; + buff[3] = SYM_AH_V_M_0; + buff[4] = SYM_AH_V_M_1; buff[5] = '\0'; } break; } break; } - case OSD_GLIDE_TIME_REMAINING: + case OSD_GLIDE_TIME_REMAINING: { - uint16_t glideSeconds = osdGetRemainingGlideTime(); + uint16_t glideTime = osdGetRemainingGlideTime(); buff[0] = SYM_GLIDE_MINS; - if (glideSeconds > 0) { - tfp_sprintf(buff + 1, "%i", (int)round(glideSeconds / 60)); + if (glideTime > 0) { + // Maximum value we can show in minutes is 99 minutes and 59 seconds. It is extremely unlikely that glide + // time will be longer than 99 minutes. If it is, it will show 99:^^ + if (glideTime > (99 * 60) + 59) { + tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60)); + buff[4] = SYM_DIRECTION; + buff[5] = SYM_DIRECTION; + } else { + tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); + } } else { - buff[1] = '-'; - buff[2] = '-'; - buff[3] = '-'; + tfp_sprintf(buff + 1, "%s", "--:--"); } - buff[4] = '\0'; + buff[6] = '\0'; break; } case OSD_GLIDE_RANGE: { uint16_t glideSeconds = osdGetRemainingGlideTime(); - uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; buff[0] = SYM_GLIDE_DIST; - osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + if (glideSeconds > 0) { + uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; + osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + } else { + tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); + buff[5] = '\0'; + } break; } #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 92cdfc5593a..cc482f82599 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -265,6 +265,7 @@ typedef enum { OSD_GLIDE_TIME_REMAINING, OSD_GLIDE_RANGE, OSD_CLIMB_EFFICIENCY, + OSD_NAV_WP_MULTI_MISSION_INDEX, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index bd2fd934e03..e0e0bd36f8b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -667,7 +667,7 @@ bool isFixedWingFlying(void) airspeed = getAirspeedEstimate(); #endif bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle; - bool velCondition = posControl.actualState.velXY > 250 || airspeed > 250; + bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f; bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING; return (isImuHeadingValid() && throttleCondition && velCondition) || launchCondition; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6b72c511879..872d189db43 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -461,9 +461,7 @@ void FAST_CODE NOINLINE gyroFilter() * In some cases, noise amplitude is high enough not to be filtered by the primary filter. * This happens on the first frequency with the biggest aplitude */ - DEBUG_SET(DEBUG_ALWAYS, axis, gyroADCf); - secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); - DEBUG_SET(DEBUG_ALWAYS, axis + 3, gyroADCf); + gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index ff11b4c4533..2365be96712 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -181,10 +181,12 @@ // Wind estimator #define USE_WIND_ESTIMATOR +#define USE_SIMULATOR +#define USE_PITOT_VIRTUAL + //Designed to free space of F722 and F411 MCUs #if (MCU_FLASH_SIZE > 512) #define USE_VTX_FFPV -#define USE_PITOT_VIRTUAL #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE