Skip to content

Commit

Permalink
Add USER3 mode (iNavFlight#8276)
Browse files Browse the repository at this point in the history
  • Loading branch information
@shellixyz @sensei-hacker
shellixyz authored and sensei-hacker committed Nov 7, 2022
1 parent 9341697 commit f25538f
Show file tree
Hide file tree
Showing 8 changed files with 77 additions and 62 deletions.
20 changes: 10 additions & 10 deletions src/main/common/log.h
View file
Expand Up @@ -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(...)
Expand Down
14 changes: 6 additions & 8 deletions src/main/drivers/osd_symbols.h
View file
Expand Up @@ -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
Expand Down
3 changes: 3 additions & 0 deletions src/main/fc/settings.yaml
View file
Expand Up @@ -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
Expand Down
91 changes: 52 additions & 39 deletions src/main/io/osd.c
View file
Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand All @@ -2328,59 +2336,64 @@ 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;
case OSD_UNIT_METRIC_MPH:
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
Expand Down
1 change: 1 addition & 0 deletions src/main/io/osd.h
View file
Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion src/main/navigation/navigation_fixedwing.c
View file
Expand Up @@ -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;
Expand Down
4 changes: 1 addition & 3 deletions src/main/sensors/gyro.c
View file
Expand Up @@ -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

Expand Down
4 changes: 3 additions & 1 deletion src/main/target/common.h
View file
Expand Up @@ -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
Expand Down

0 comments on commit f25538f

Please sign in to comment.