Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-14 03:49:58 +03:00
Code Issues Wiki Activity

added USE_GPS_FIX_ESTIMATION

Browse source 
fixed indentation
This commit is contained in:
Roman Lut 2023-08-09 09:48:46 +02:00
parent 2b9a5a6a8d
commit 599e45c48f
29 changed files with 626 additions and 397 deletions
Download patch file Download diff file Expand all files Collapse all files

174
src/main/fc/fc_msp.c
View file

@ -3492,8 +3492,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) { if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) {
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
#ifdef USE_BARO #ifdef USE_BARO
if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
@ -3508,15 +3508,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
// Review: Many states were affected. Reboot? // Review: Many states were affected. Reboot?
disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! disarm(DISARM_SWITCH); // Disarm to prevent motor output!!!
} }
} else { } else {
if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
#ifdef USE_BARO #ifdef USE_BARO
if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
sensorsSet(SENSOR_BARO); sensorsSet(SENSOR_BARO);
setTaskEnabled(TASK_BARO, true); setTaskEnabled(TASK_BARO, true);
DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
baroStartCalibration(); baroStartCalibration();
} }
#endif #endif
@ -3530,80 +3530,80 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
mag.magADC[Z] = 0; mag.magADC[Z] = 0;
} }
#endif #endif
ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
LOG_DEBUG(SYSTEM, "Simulator enabled"); LOG_DEBUG(SYSTEM, "Simulator enabled");
} }
if (dataSize >= 14) { if (dataSize >= 14) {
if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
gpsSolDRV.fixType = sbufReadU8(src); gpsSolDRV.fixType = sbufReadU8(src);
gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSolDRV.numSat = sbufReadU8(src); gpsSolDRV.numSat = sbufReadU8(src);
if (gpsSolDRV.fixType != GPS_NO_FIX) { if (gpsSolDRV.fixType != GPS_NO_FIX) {
gpsSolDRV.flags.validVelNE = true; gpsSolDRV.flags.validVelNE = true;
gpsSolDRV.flags.validVelD = true; gpsSolDRV.flags.validVelD = true;
gpsSolDRV.flags.validEPE = true; gpsSolDRV.flags.validEPE = true;
gpsSolDRV.flags.validTime = false; gpsSolDRV.flags.validTime = false;
gpsSolDRV.llh.lat = sbufReadU32(src); gpsSolDRV.llh.lat = sbufReadU32(src);
gpsSolDRV.llh.lon = sbufReadU32(src); gpsSolDRV.llh.lon = sbufReadU32(src);
gpsSolDRV.llh.alt = sbufReadU32(src); gpsSolDRV.llh.alt = sbufReadU32(src);
gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src);
gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src);
gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src);
gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src);
gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src);
gpsSolDRV.eph = 100; gpsSolDRV.eph = 100;
gpsSolDRV.epv = 100; gpsSolDRV.epv = 100;
} else { } else {
sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
} }
// Feed data to navigation // Feed data to navigation
gpsProcessNewDriverData(); gpsProcessNewDriverData();
gpsProcessNewSolutionData(false); gpsProcessNewSolutionData(false);
} else { } else {
sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
} }
if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) { if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
attitude.values.roll = (int16_t)sbufReadU16(src); attitude.values.roll = (int16_t)sbufReadU16(src);
attitude.values.pitch = (int16_t)sbufReadU16(src); attitude.values.pitch = (int16_t)sbufReadU16(src);
attitude.values.yaw = (int16_t)sbufReadU16(src); attitude.values.yaw = (int16_t)sbufReadU16(src);
} else { } else {
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
} }
// Get the acceleration in 1G units // Get the acceleration in 1G units
acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accVibeSq[X] = 0.0f; acc.accVibeSq[X] = 0.0f;
acc.accVibeSq[Y] = 0.0f; acc.accVibeSq[Y] = 0.0f;
acc.accVibeSq[Z] = 0.0f; acc.accVibeSq[Z] = 0.0f;
// Get the angular velocity in DPS // Get the angular velocity in DPS
gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
baro.baroPressure = (int32_t)sbufReadU32(src); baro.baroPressure = (int32_t)sbufReadU32(src);
baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
} else { } else {
sbufAdvance(src, sizeof(uint32_t)); sbufAdvance(src, sizeof(uint32_t));
} }
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT
mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero
mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
} else { } else {
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
} }
#if defined(USE_FAKE_BATT_SENSOR) #if defined(USE_FAKE_BATT_SENSOR)
if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) {
@ -3617,7 +3617,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
simulatorData.airSpeed = sbufReadU16(src); simulatorData.airSpeed = sbufReadU16(src);
} else { } else {
if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
sbufReadU16(src); sbufReadU16(src);
} }
@ -3626,35 +3626,35 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
} }
} else { } else {
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} }
} }
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]); sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]); sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]); sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500)); sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
simulatorData.debugIndex++; simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8) { if (simulatorData.debugIndex == 8) {
simulatorData.debugIndex = 0; simulatorData.debugIndex = 0;
} }
tmp_u8 = simulatorData.debugIndex | tmp_u8 = simulatorData.debugIndex |
((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
(ARMING_FLAG(ARMED) ? 64 : 0) | (ARMING_FLAG(ARMED) ? 64 : 0) |
(!feature(FEATURE_OSD) ? 32: 0) | (!feature(FEATURE_OSD) ? 32: 0) |
(!isOSDTypeSupportedBySimulator() ? 16 : 0); (!isOSDTypeSupportedBySimulator() ? 16 : 0);
sbufWriteU8(dst, tmp_u8); sbufWriteU8(dst, tmp_u8);
sbufWriteU32(dst, debug[simulatorData.debugIndex]); sbufWriteU32(dst, debug[simulatorData.debugIndex]);
sbufWriteU16(dst, attitude.values.roll); sbufWriteU16(dst, attitude.values.roll);
sbufWriteU16(dst, attitude.values.pitch); sbufWriteU16(dst, attitude.values.pitch);
sbufWriteU16(dst, attitude.values.yaw); sbufWriteU16(dst, attitude.values.yaw);
mspWriteSimulatorOSD(dst); mspWriteSimulatorOSD(dst);
*ret = MSP_RESULT_ACK; *ret = MSP_RESULT_ACK;
break; break;

20
src/main/fc/runtime_config.h
View file

@ -123,7 +123,9 @@ typedef enum {
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
COMPASS_CALIBRATED = (1 << 8), COMPASS_CALIBRATED = (1 << 8),
ACCELEROMETER_CALIBRATED = (1 << 9), ACCELEROMETER_CALIBRATED = (1 << 9),
#ifdef USE_GPS_FIX_ESTIMATION
GPS_ESTIMATED_FIX = (1 << 10), GPS_ESTIMATED_FIX = (1 << 10),
#endif
NAV_CRUISE_BRAKING = (1 << 11), NAV_CRUISE_BRAKING = (1 << 11),
NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_BOOST = (1 << 12),
NAV_CRUISE_BRAKING_LOCKED = (1 << 13), NAV_CRUISE_BRAKING_LOCKED = (1 << 13),
@ -176,12 +178,12 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void);
typedef enum { typedef enum {
HITL_RESET_FLAGS = (0 << 0), HITL_RESET_FLAGS = (0 << 0),
HITL_ENABLE = (1 << 0), HITL_ENABLE = (1 << 0),
HITL_SIMULATE_BATTERY = (1 << 1), HITL_SIMULATE_BATTERY = (1 << 1),
HITL_MUTE_BEEPER = (1 << 2), HITL_MUTE_BEEPER = (1 << 2),
HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV)
HITL_HAS_NEW_GPS_DATA = (1 << 4), HITL_HAS_NEW_GPS_DATA = (1 << 4),
HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2
HITL_AIRSPEED = (1 << 6), HITL_AIRSPEED = (1 << 6),
HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2
HITL_GPS_TIMEOUT = (1 << 8), HITL_GPS_TIMEOUT = (1 << 8),
@ -189,10 +191,10 @@ typedef enum {
} simulatorFlags_t; } simulatorFlags_t;
typedef struct { typedef struct {
simulatorFlags_t flags; simulatorFlags_t flags;
uint8_t debugIndex; uint8_t debugIndex;
uint8_t vbat; // 126 -> 12.6V uint8_t vbat; // 126 -> 12.6V
uint16_t airSpeed; // cm/s uint16_t airSpeed; // cm/s
int16_t input[4]; int16_t input[4];
} simulatorData_t; } simulatorData_t;

2
src/main/fc/settings.yaml
View file

@ -834,6 +834,7 @@ groups:
max: 600 max: 600
- name: failsafe_gps_fix_estimation_delay - name: failsafe_gps_fix_estimation_delay
description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation." description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation."
condition: USE_GPS_FIX_ESTIMATION
default_value: 7 default_value: 7
min: -1 min: -1
max: 600 max: 600
@ -2195,6 +2196,7 @@ groups:
type: bool type: bool
- name: inav_allow_gps_fix_estimation - name: inav_allow_gps_fix_estimation
description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay." description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay."
condition: USE_GPS_FIX_ESTIMATION
default_value: OFF default_value: OFF
field: allow_gps_fix_estimation field: allow_gps_fix_estimation
type: bool type: bool

23
src/main/flight/failsafe.c
View file

@ -82,7 +82,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default
.failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
.failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s) .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s)
#ifdef USE_GPS_FIX_ESTIMATION
.failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
#endif
); );
typedef enum { typedef enum {
@ -355,7 +357,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
// Craft is closer than minimum failsafe procedure distance (if set to non-zero) // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set // GPS must also be working, and home position set
if (failsafeConfig()->failsafe_min_distance > 0 && if (failsafeConfig()->failsafe_min_distance > 0 &&
((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME)) {
// get the distance to the original arming point // get the distance to the original arming point
uint32_t distance = calculateDistanceToDestination(&original_rth_home); uint32_t distance = calculateDistanceToDestination(&original_rth_home);
@ -368,7 +374,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
return failsafeConfig()->failsafe_procedure; return failsafeConfig()->failsafe_procedure;
} }
#ifdef USE_GPS_FIX_ESTIMATION
bool checkGPSFixFailsafe(void) bool checkGPSFixFailsafe(void)
{ {
if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) {
@ -387,6 +393,7 @@ bool checkGPSFixFailsafe(void)
} }
return false; return false;
} }
#endif
void failsafeUpdateState(void) void failsafeUpdateState(void)
@ -418,9 +425,12 @@ void failsafeUpdateState(void)
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
} }
#ifdef USE_GPS_FIX_ESTIMATION
if ( checkGPSFixFailsafe() ) { if ( checkGPSFixFailsafe() ) {
reprocessState = true; reprocessState = true;
} else if (!receivingRxDataAndNotFailsafeMode) { } else
#endif
if (!receivingRxDataAndNotFailsafeMode) {
if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
// Don't disarm at all if `failsafe_throttle_low_delay` is set to zero // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero
@ -432,7 +442,7 @@ void failsafeUpdateState(void)
failsafeState.wpModeDelayedFailsafeStart = 0; failsafeState.wpModeDelayedFailsafeStart = 0;
} }
reprocessState = true; reprocessState = true;
} }
} else { } else {
// When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
if (!receivingRxDataAndNotFailsafeMode) { if (!receivingRxDataAndNotFailsafeMode) {
@ -488,11 +498,14 @@ void failsafeUpdateState(void)
} else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true; reprocessState = true;
} else { }
#ifdef USE_GPS_FIX_ESTIMATION
else {
if ( checkGPSFixFailsafe() ) { if ( checkGPSFixFailsafe() ) {
reprocessState = true; reprocessState = true;
} }
} }
#endif
break; break;

4
src/main/flight/failsafe.h
View file

@ -43,7 +43,9 @@ typedef struct failsafeConfig_s {
uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s)
#ifdef USE_GPS_FIX_ESTIMATION
int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s)
#endif
} failsafeConfig_t; } failsafeConfig_t;
PG_DECLARE(failsafeConfig_t, failsafeConfig); PG_DECLARE(failsafeConfig_t, failsafeConfig);
@ -150,7 +152,9 @@ typedef struct failsafeState_s {
timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData
timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData
timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time
#ifdef USE_GPS_FIX_ESTIMATION
timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation
#endif
failsafeProcedure_e activeProcedure; failsafeProcedure_e activeProcedure;
failsafePhase_e phase; failsafePhase_e phase;
failsafeRxLinkState_e rxLinkState; failsafeRxLinkState_e rxLinkState;

6
src/main/flight/imu.c
View file

@ -323,7 +323,11 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c
bool isGPSTrustworthy(void) bool isGPSTrustworthy(void)
{ {
return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX); return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
;
} }
static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)

13
src/main/flight/wind_estimator.c
View file

@ -52,7 +52,11 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT];
bool isEstimatedWindSpeedValid(void) bool isEstimatedWindSpeedValid(void)
{ {
return hasValidWindEstimate || STATE(GPS_ESTIMATED_FIX); //use any wind estimate with GPS fix estimation. return hasValidWindEstimate
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX) //use any wind estimate with GPS fix estimation.
#endif
;
} }
float getEstimatedWindSpeed(int axis) float getEstimatedWindSpeed(int axis)
@ -90,8 +94,11 @@ void updateWindEstimator(timeUs_t currentTimeUs)
if (!STATE(FIXED_WING_LEGACY) || if (!STATE(FIXED_WING_LEGACY) ||
!isGPSHeadingValid() || !isGPSHeadingValid() ||
!gpsSol.flags.validVelNE || !gpsSol.flags.validVelNE ||
!gpsSol.flags.validVelD || !gpsSol.flags.validVelD
STATE(GPS_ESTIMATED_FIX)) { #ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
return; return;
} }

34
src/main/io/gps.c
View file

@ -223,7 +223,7 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs)
gpsState.timeoutMs = timeoutMs; gpsState.timeoutMs = timeoutMs;
} }
#ifdef USE_GPS_FIX_ESTIMATION
bool canEstimateGPSFix(void) bool canEstimateGPSFix(void)
{ {
#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) #if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO)
@ -238,10 +238,11 @@ bool canEstimateGPSFix(void)
#else #else
return false; return false;
#endif #endif
} }
#endif
#ifdef USE_GPS_FIX_ESTIMATION
void processDisableGPSFix(void) void processDisableGPSFix(void)
{ {
static int32_t last_lat = 0; static int32_t last_lat = 0;
@ -268,7 +269,9 @@ void processDisableGPSFix(void)
last_alt = gpsSol.llh.alt; last_alt = gpsSol.llh.alt;
} }
} }
#endif
#ifdef USE_GPS_FIX_ESTIMATION
//called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition" //called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition"
void updateEstimatedGPSFix(void) void updateEstimatedGPSFix(void)
{ {
@ -345,14 +348,17 @@ void updateEstimatedGPSFix(void)
gpsSol.velNED[Y] = (int16_t)(velY); gpsSol.velNED[Y] = (int16_t)(velY);
gpsSol.velNED[Z] = 0; gpsSol.velNED[Z] = 0;
} }
#endif
void gpsProcessNewDriverData(void) void gpsProcessNewDriverData(void)
{ {
gpsSol = gpsSolDRV; gpsSol = gpsSolDRV;
#ifdef USE_GPS_FIX_ESTIMATION
processDisableGPSFix(); processDisableGPSFix();
updateEstimatedGPSFix(); updateEstimatedGPSFix();
#endif
} }
//called after: //called after:
@ -362,11 +368,13 @@ void gpsProcessNewDriverData(void)
//On GPS sensor timeout - called after updateEstimatedGPSFix() //On GPS sensor timeout - called after updateEstimatedGPSFix()
void gpsProcessNewSolutionData(bool timeout) void gpsProcessNewSolutionData(bool timeout)
{ {
#ifdef USE_GPS_FIX_ESTIMATION
if ( gpsSol.numSat == 99 ) { if ( gpsSol.numSat == 99 ) {
ENABLE_STATE(GPS_ESTIMATED_FIX); ENABLE_STATE(GPS_ESTIMATED_FIX);
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} else { } else {
DISABLE_STATE(GPS_ESTIMATED_FIX); DISABLE_STATE(GPS_ESTIMATED_FIX);
#endif
// Set GPS fix flag only if we have 3D fix // Set GPS fix flag only if we have 3D fix
if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
@ -379,7 +387,9 @@ void gpsProcessNewSolutionData(bool timeout)
gpsSol.flags.validEPE = false; gpsSol.flags.validEPE = false;
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} }
#ifdef USE_GPS_FIX_ESTIMATION
} }
#endif
if (!timeout) { if (!timeout) {
// Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix) // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix)
@ -423,11 +433,15 @@ void gpsTryEstimateOnTimeout(void)
gpsResetSolution(&gpsSol); gpsResetSolution(&gpsSol);
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
updateEstimatedGPSFix(); #ifdef USE_GPS_FIX_ESTIMATION
if ( canEstimateGPSFix() ) {
updateEstimatedGPSFix();
if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in
gpsProcessNewSolutionData(true); gpsProcessNewSolutionData(true);
}
} }
#endif
} }
void gpsPreInit(void) void gpsPreInit(void)
@ -577,7 +591,7 @@ bool gpsUpdate(void)
break; break;
} }
if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) { if ( !sensors(SENSOR_GPS) ) {
gpsTryEstimateOnTimeout(); gpsTryEstimateOnTimeout();
} }
@ -630,7 +644,11 @@ bool isGPSHealthy(void)
bool isGPSHeadingValid(void) bool isGPSHeadingValid(void)
{ {
return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; return ((STATE(GPS_FIX) && gpsSol.numSat >= 6)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed >= 300;
} }
#endif #endif

52
src/main/io/gps_ublox.c
View file

@ -147,7 +147,7 @@ static union {
bool gpsUbloxHasGalileo(void) bool gpsUbloxHasGalileo(void)
{ {
return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK); return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK);
} }
bool gpsUbloxHasBeidou(void) bool gpsUbloxHasBeidou(void)
{ {
@ -622,8 +622,8 @@ static bool gpsParseFrameUBLOX(void)
gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion));
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
if (_buffer.ver.swVersion[9] > '2' || true) { if (_buffer.ver.swVersion[9] > '2' || true) {
// check extensions; // check extensions;
// after hw + sw vers; each is 30 bytes // after hw + sw vers; each is 30 bytes
bool found = false; bool found = false;
for (int j = 40; j < _payload_length && !found; j += 30) for (int j = 40; j < _payload_length && !found; j += 30)
{ {
@ -863,8 +863,8 @@ STATIC_PROTOTHREAD(gpsConfigure)
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
configureRATE(hz2rate(5)); // 5Hz configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK); ptWait(_ack_state == UBX_ACK_GOT_ACK);
} }
} }
else { else {
// u-Blox 5/6/7/8 or unknown // u-Blox 5/6/7/8 or unknown
@ -901,8 +901,8 @@ STATIC_PROTOTHREAD(gpsConfigure)
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
configureRATE(hz2rate(5)); // 5Hz configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK); ptWait(_ack_state == UBX_ACK_GOT_ACK);
} }
} }
// u-Blox 5/6 doesn't support PVT, use legacy config // u-Blox 5/6 doesn't support PVT, use legacy config
// UNKNOWN also falls here, use as a last resort // UNKNOWN also falls here, use as a last resort
@ -944,19 +944,19 @@ STATIC_PROTOTHREAD(gpsConfigure)
// Configure GNSS for M8N and later // Configure GNSS for M8N and later
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) {
configureGNSS10(); configureGNSS10();
} else { } else {
configureGNSS(); configureGNSS();
} }
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
if(_ack_state == UBX_ACK_GOT_NAK) { if(_ack_state == UBX_ACK_GOT_NAK) {
gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT;
gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT; gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT;
gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT; gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT;
} }
} }
ptEnd(0); ptEnd(0);
@ -1019,18 +1019,18 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
} }
// Reset protocol timeout // Reset protocol timeout
gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
// Attempt to detect GPS hw version // Attempt to detect GPS hw version
gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
gpsState.autoConfigStep = 0; gpsState.autoConfigStep = 0;
do { do {
pollVersion(); pollVersion();
gpsState.autoConfigStep++; gpsState.autoConfigStep++;
ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
} while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
gpsState.autoConfigStep = 0; gpsState.autoConfigStep = 0;
ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
@ -1062,7 +1062,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
{ {
pollVersion(); pollVersion();
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
} }
pollGnssCapabilities(); pollGnssCapabilities();
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);

408
src/main/io/osd.c
View file

@ -1080,7 +1080,7 @@ static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *at
if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) {
TEXT_ATTRIBUTES_ADD_BLINK(*attr); TEXT_ATTRIBUTES_ADD_BLINK(*attr);
} }
} }
void osdCrosshairPosition(uint8_t *x, uint8_t *y) void osdCrosshairPosition(uint8_t *x, uint8_t *y)
@ -1114,7 +1114,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes
if (useScaled) { if (useScaled) {
buff[0] = SYM_SCALE; buff[0] = SYM_SCALE;
} else { } else {
buff[0] = SYM_BLANK; buff[0] = SYM_BLANK;
} }
buff[1] = SYM_THR; buff[1] = SYM_THR;
if (navigationIsControllingThrottle()) { if (navigationIsControllingThrottle()) {
@ -1315,7 +1315,11 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen
} }
} }
if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
float poiAngle = DEGREES_TO_RADIANS(directionToPoi); float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
@ -1429,7 +1433,11 @@ static void osdDisplayTelemetry(void)
static uint16_t trk_bearing = 0; static uint16_t trk_bearing = 0;
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)){ if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
){
if (GPS_distanceToHome > 5) { if (GPS_distanceToHome > 5) {
trk_bearing = GPS_directionToHome; trk_bearing = GPS_directionToHome;
trk_bearing += 360 + 180; trk_bearing += 360 + 180;
@ -1729,7 +1737,7 @@ static bool osdDrawSingleElement(uint8_t item)
// Shown in Ah // Shown in Ah
buff[mah_digits] = SYM_AH; buff[mah_digits] = SYM_AH;
} else { } else {
// Shown in mAh // Shown in mAh
buff[mah_digits] = SYM_MAH; buff[mah_digits] = SYM_MAH;
} }
buff[mah_digits + 1] = '\0'; buff[mah_digits + 1] = '\0';
@ -1786,12 +1794,16 @@ static bool osdDrawSingleElement(uint8_t item)
buff[0] = SYM_SAT_L; buff[0] = SYM_SAT_L;
buff[1] = SYM_SAT_R; buff[1] = SYM_SAT_R;
tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_ESTIMATED_FIX)) {
strcpy(buff + 2, "ES"); strcpy(buff + 2, "ES");
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} } else
else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { #endif
strcpy(buff + 2, "X!"); if (!STATE(GPS_FIX)) {
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
strcpy(buff + 2, "X!");
}
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} }
break; break;
@ -1843,7 +1855,11 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_HOME_DIR: case OSD_HOME_DIR:
{ {
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
} }
@ -2331,11 +2347,19 @@ static bool osdDrawSingleElement(uint8_t item)
osdCrosshairPosition(&elemPosX, &elemPosY); osdCrosshairPosition(&elemPosX, &elemPosY);
osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY);
if (osdConfig()->hud_homing && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (osdConfig()->hud_homing && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
osdHudDrawHoming(elemPosX, elemPosY); osdHudDrawHoming(elemPosX, elemPosY);
} }
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && isImuHeadingValid()) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && isImuHeadingValid()) {
if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
osdHudClear(); osdHudClear();
@ -2959,8 +2983,12 @@ static bool osdDrawSingleElement(uint8_t item)
digits = 4U; digits = 4U;
} }
#endif #endif
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if ((STATE(GPS_FIX)
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { #ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta)); 1, US2S(efficiencyTimeDelta));
@ -3030,7 +3058,11 @@ static bool osdDrawSingleElement(uint8_t item)
int32_t value = 0; int32_t value = 0;
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))&& gpsSol.groundSpeed > 0) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta)); 1, US2S(efficiencyTimeDelta));
@ -3182,7 +3214,11 @@ static bool osdDrawSingleElement(uint8_t item)
STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier);
int digits = osdConfig()->plus_code_digits; int digits = osdConfig()->plus_code_digits;
int digitsRemoved = osdConfig()->plus_code_short * 2; int digitsRemoved = osdConfig()->plus_code_short * 2;
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)) {
olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff));
} else { } else {
// +codes with > 8 digits have a + at the 9th digit // +codes with > 8 digits have a + at the 9th digit
@ -4112,13 +4148,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
if (isSinglePageStatsCompatible) { if (isSinglePageStatsCompatible) {
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---");
} else if (page == 0) { } else if (page == 0) {
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->");
} else if (page == 1) { } else if (page == 1) {
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2");
} }
if (isSinglePageStatsCompatible || page == 0) { if (isSinglePageStatsCompatible || page == 0) {
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
if (isSinglePageStatsCompatible) { if (isSinglePageStatsCompatible) {
displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :");
osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); osdFormatVelocityStr(buff, stats.max_3D_speed, true, false);
@ -4130,32 +4166,32 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
osdLeftAlignString(buff); osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
} else { } else {
displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :");
osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); osdFormatVelocityStr(buff, stats.max_3D_speed, true, false);
osdLeftAlignString(buff); osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :");
osdGenerateAverageVelocityStr(buff); osdGenerateAverageVelocityStr(buff);
osdLeftAlignString(buff); osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
} }
displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :");
osdFormatDistanceStr(buff, stats.max_distance*100); osdFormatDistanceStr(buff, stats.max_distance*100);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:");
osdFormatDistanceStr(buff, getTotalTravelDistance());
displayWrite(osdDisplayPort, statValuesX, top++, buff);
}
displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :");
osdFormatAltitudeStr(buff, stats.max_altitude);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); switch (rxConfig()->serialrx_provider) {
osdFormatDistanceStr(buff, getTotalTravelDistance()); case SERIALRX_CRSF:
displayWrite(osdDisplayPort, statValuesX, top++, buff);
}
displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :");
osdFormatAltitudeStr(buff, stats.max_altitude);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
switch (rxConfig()->serialrx_provider) {
case SERIALRX_CRSF:
if (isSinglePageStatsCompatible) { if (isSinglePageStatsCompatible) {
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :");
itoa(stats.min_rssi, buff, 10); itoa(stats.min_rssi, buff, 10);
@ -4168,172 +4204,172 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
osdLeftAlignString(buff); osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
} else { } else {
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :");
itoa(stats.min_rssi, buff, 10); itoa(stats.min_rssi, buff, 10);
strcat(buff, "%"); strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :");
itoa(stats.min_rssi_dbm, buff, 10); itoa(stats.min_rssi_dbm, buff, 10);
tfp_sprintf(buff, "%s%c", buff, SYM_DBM); tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
} }
displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :");
itoa(stats.min_lq, buff, 10); itoa(stats.min_lq, buff, 10);
strcat(buff, "%"); strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
break; break;
default: default:
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
itoa(stats.min_rssi, buff, 10); itoa(stats.min_rssi, buff, 10);
strcat(buff, "%"); strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
} }
displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :");
uint16_t flySeconds = getFlightTime(); uint16_t flySeconds = getFlightTime();
uint16_t flyMinutes = flySeconds / 60; uint16_t flyMinutes = flySeconds / 60;
flySeconds %= 60; flySeconds %= 60;
uint16_t flyHours = flyMinutes / 60; uint16_t flyHours = flyMinutes / 60;
flyMinutes %= 60; flyMinutes %= 60;
tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :");
displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
} }
if (isSinglePageStatsCompatible || page == 1) { if (isSinglePageStatsCompatible || page == 1) {
if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) {
displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
} else {
displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :");
osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3);
}
tfp_sprintf(buff, "%s%c", buff, SYM_VOLT);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
if (feature(FEATURE_CURRENT_METER)) {
displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :");
osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3);
tfp_sprintf(buff, "%s%c", buff, SYM_AMP);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :");
bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3);
buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
buff[4] = '\0';
displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :");
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH);
} else { } else {
osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :");
tfp_sprintf(buff, "%s%c", buff, SYM_WH); osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3);
} }
tfp_sprintf(buff, "%s%c", buff, SYM_VOLT);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
int32_t totalDistance = getTotalTravelDistance(); if (feature(FEATURE_CURRENT_METER)) {
bool moreThanAh = false; displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :");
bool efficiencyValid = totalDistance >= 10000; osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3);
if (feature(FEATURE_GPS)) { tfp_sprintf(buff, "%s%c", buff, SYM_AMP);
displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :");
uint8_t digits = 3U; // Total number of digits (including decimal point)
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
// Add one digit so no switch to scaled decimal occurs above 99
digits = 4U;
}
#endif
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_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[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
}
}
break;
case OSD_UNIT_GA:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1);
} else {
tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM);
}
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
buff[3] = SYM_MAH_NM_0;
buff[4] = SYM_MAH_NM_1;
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
}
}
break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_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[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
}
}
break;
}
osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :");
bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3);
buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
buff[4] = '\0';
displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :");
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH);
} else {
osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3);
tfp_sprintf(buff, "%s%c", buff, SYM_WH);
}
displayWrite(osdDisplayPort, statValuesX, top++, buff);
int32_t totalDistance = getTotalTravelDistance();
bool moreThanAh = false;
bool efficiencyValid = totalDistance >= 10000;
if (feature(FEATURE_GPS)) {
displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :");
uint8_t digits = 3U; // Total number of digits (including decimal point)
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
// Add one digit so no switch to scaled decimal occurs above 99
digits = 4U;
}
#endif
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_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[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
}
}
break;
case OSD_UNIT_GA:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1);
} else {
tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM);
}
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
buff[3] = SYM_MAH_NM_0;
buff[4] = SYM_MAH_NM_1;
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
}
}
break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_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[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
}
}
break;
}
osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
}
} }
}
const float max_gforce = accGetMeasuredMaxG(); const float max_gforce = accGetMeasuredMaxG();
displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :");
osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_min = acc_extremes[Z].min;
const float acc_extremes_max = acc_extremes[Z].max; const float acc_extremes_max = acc_extremes[Z].max;
displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4);
osdLeftAlignString(buff); osdLeftAlignString(buff);
strcat(osdFormatTrimWhiteSpace(buff),"/"); strcat(osdFormatTrimWhiteSpace(buff),"/");

6
src/main/io/osd_dji_hd.c
View file

@ -786,7 +786,11 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta)); 1, US2S(efficiencyTimeDelta));

8
src/main/navigation/navigation.c
View file

@ -2378,6 +2378,7 @@ bool validateRTHSanityChecker(void)
return true; return true;
} }
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode //disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump //when estimated GPS fix is replaced with real fix, coordinates may jump
@ -2386,6 +2387,7 @@ bool validateRTHSanityChecker(void)
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true; return true;
} }
#endif
// Check at 10Hz rate // Check at 10Hz rate
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
@ -3993,7 +3995,11 @@ void updateWpMissionPlanner(void)
{ {
static timeMs_t resetTimerStart = 0; static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) { if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)); const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
);
posControl.flags.wpMissionPlannerActive = true; posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) { if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {

2
src/main/navigation/navigation.h
View file

@ -210,7 +210,9 @@ typedef struct positionEstimationConfig_s {
uint8_t use_gps_no_baro; uint8_t use_gps_no_baro;
#ifdef USE_GPS_FIX_ESTIMATION
uint8_t allow_gps_fix_estimation; uint8_t allow_gps_fix_estimation;
#endif
} positionEstimationConfig_t; } positionEstimationConfig_t;
PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);

10
src/main/navigation/navigation_fixedwing.c
View file

@ -313,13 +313,13 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
// turn direction to next waypoint // turn direction to next waypoint
loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right
needToCalculateCircularLoiter = true; needToCalculateCircularLoiter = true;
} }
posControl.flags.wpTurnSmoothingActive = true; posControl.flags.wpTurnSmoothingActive = true;
} }

35
src/main/navigation/navigation_pos_estimator.c
View file

@ -92,8 +92,9 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
.baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,
#ifdef USE_GPS_FIX_ESTIMATION
.allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
#endif
); );
#define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
@ -152,12 +153,14 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
bool isGlitching = false; bool isGlitching = false;
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode //disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump //when estimated GPS fix is replaced with real fix, coordinates may jump
previousTime = 0; previousTime = 0;
return true; return true;
} }
#endif
if (previousTime == 0) { if (previousTime == 0) {
isGlitching = false; isGlitching = false;
@ -210,8 +213,16 @@ void onNewGPSData(void)
newLLH.lon = gpsSol.llh.lon; newLLH.lon = gpsSol.llh.lon;
newLLH.alt = gpsSol.llh.alt; newLLH.alt = gpsSol.llh.alt;
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { #ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
if (!(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)) {
isFirstGPSUpdate = true; isFirstGPSUpdate = true;
return; return;
} }
@ -492,7 +503,11 @@ static bool navIsAccelerationUsable(void)
static bool navIsHeadingUsable(void) static bool navIsHeadingUsable(void)
{ {
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
// If we have GPS - we need true IMU north (valid heading) // If we have GPS - we need true IMU north (valid heading)
return isImuHeadingValid(); return isImuHeadingValid();
} }
@ -507,7 +522,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
/* Figure out if we have valid position data from our data sources */ /* Figure out if we have valid position data from our data sources */
uint32_t newFlags = 0; uint32_t newFlags = 0;
if ((sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) && posControl.gpsOrigin.valid && if ((sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && posControl.gpsOrigin.valid &&
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
@ -704,7 +723,11 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
{ {
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && navIsHeadingUsable()) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && navIsHeadingUsable()) {
static timeUs_t lastUpdateTimeUs = 0; static timeUs_t lastUpdateTimeUs = 0;
if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate

13
src/main/programming/logic_condition.c
View file

@ -471,8 +471,9 @@ static int logicConditionCompute(
} else { } else {
return false; return false;
} }
break; break;
#ifdef USE_GPS_FIX_ESTIMATION
case LOGIC_CONDITION_DISABLE_GPS_FIX: case LOGIC_CONDITION_DISABLE_GPS_FIX:
if (operandA > 0) { if (operandA > 0) {
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
@ -480,8 +481,9 @@ static int logicConditionCompute(
LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
} }
return true; return true;
break; break;
#endif
default: default:
return false; return false;
break; break;
@ -663,9 +665,12 @@ static int logicConditionGetFlightOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
#ifdef USE_GPS_FIX_ESTIMATION
if ( STATE(GPS_ESTIMATED_FIX) ){ if ( STATE(GPS_ESTIMATED_FIX) ){
return gpsSol.numSat; //99 return gpsSol.numSat; //99
} else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { } else
#endif
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
return 0; return 0;
} else { } else {
return gpsSol.numSat; return gpsSol.numSat;

4
src/main/programming/logic_condition.h
View file

@ -81,7 +81,9 @@ typedef enum {
LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_TIMER = 49,
LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_DELTA = 50,
LOGIC_CONDITION_APPROX_EQUAL = 51, LOGIC_CONDITION_APPROX_EQUAL = 51,
#ifdef USE_GPS_FIX_ESTIMATION
LOGIC_CONDITION_DISABLE_GPS_FIX = 52, LOGIC_CONDITION_DISABLE_GPS_FIX = 52,
#endif
LOGIC_CONDITION_LAST = 53, LOGIC_CONDITION_LAST = 53,
} logicOperation_e; } logicOperation_e;
@ -186,7 +188,9 @@ typedef enum {
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10),
#ifdef USE_GPS_FIX_ESTIMATION
LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11),
#endif
} logicConditionsGlobalFlags_t; } logicConditionsGlobalFlags_t;
typedef enum { typedef enum {

38
src/main/sensors/diagnostics.c
View file

@ -43,12 +43,10 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
if (accIsHealthy()) { if (accIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} } else {
else {
if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
@ -64,13 +62,13 @@ hardwareSensorStatus_e getHwCompassStatus(void)
{ {
#if defined(USE_MAG) #if defined(USE_MAG)
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) {
if (compassIsHealthy()) { if (compassIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} else { } else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
#endif #endif
if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
if (compassIsHealthy()) { if (compassIsHealthy()) {
@ -96,7 +94,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void)
{ {
#if defined(USE_BARO) #if defined(USE_BARO)
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) { if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) {
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} else if (baroIsHealthy()) { } else if (baroIsHealthy()) {
@ -104,7 +102,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void)
} else { } else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
#endif #endif
if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) { if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) {
if (baroIsHealthy()) { if (baroIsHealthy()) {
@ -135,8 +133,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
if (rangefinderIsHealthy()) { if (rangefinderIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -144,8 +141,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }
@ -161,8 +157,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
if (pitotIsHealthy()) { if (pitotIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -170,8 +165,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }
@ -187,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if (isGPSHealthy()) { if (isGPSHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -196,8 +189,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) { if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }
@ -213,8 +205,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
if (opflowIsHealthy()) { if (opflowIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -222,8 +213,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }

8
src/main/sensors/pitotmeter.c
View file

@ -215,14 +215,14 @@ STATIC_PROTOTHREAD(pitotThread)
pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL); pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
} }
#endif #endif
#if defined(USE_PITOT_FAKE) #if defined(USE_PITOT_FAKE)
if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
} }
#endif #endif
ptYield(); ptYield();
@ -248,14 +248,14 @@ STATIC_PROTOTHREAD(pitotThread)
pitot.airSpeed = 0.0f; pitot.airSpeed = 0.0f;
} }
#ifdef USE_SIMULATOR #ifdef USE_SIMULATOR
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
pitot.airSpeed = simulatorData.airSpeed; pitot.airSpeed = simulatorData.airSpeed;
} }
#endif #endif
#if defined(USE_PITOT_FAKE) #if defined(USE_PITOT_FAKE)
if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
pitot.airSpeed = fakePitotGetAirspeed(); pitot.airSpeed = fakePitotGetAirspeed();
} }
#endif #endif
} }

1
src/main/target/common.h
View file

@ -56,6 +56,7 @@
#define USE_GPS_PROTO_MSP #define USE_GPS_PROTO_MSP
#define USE_TELEMETRY #define USE_TELEMETRY
#define USE_TELEMETRY_LTM #define USE_TELEMETRY_LTM
#define USE_GPS_FIX_ESTIMATION
// This is the shortest period in microseconds that the scheduler will allow // This is the shortest period in microseconds that the scheduler will allow
#define SCHEDULER_DELAY_LIMIT 10 #define SCHEDULER_DELAY_LIMIT 10

6
src/main/telemetry/ghst.c
View file

@ -147,7 +147,11 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
sbufWriteU16(dst, GPS_directionToHome); sbufWriteU16(dst, GPS_directionToHome);
uint8_t gpsFlags = 0; uint8_t gpsFlags = 0;
if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
gpsFlags |= GPS_FLAGS_FIX; gpsFlags |= GPS_FLAGS_FIX;
} }
if (STATE(GPS_FIX_HOME)) { if (STATE(GPS_FIX_HOME)) {

12
src/main/telemetry/hott.c
View file

@ -234,7 +234,11 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120);
hottGPSMessage->climbrate3s = climbrate3s & 0xFF; hottGPSMessage->climbrate3s = climbrate3s & 0xFF;
if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { if (!(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX))
#endif
) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return; return;
} }
@ -476,7 +480,11 @@ static bool processBinaryModeRequest(uint8_t address)
switch (address) { switch (address) {
#ifdef USE_GPS #ifdef USE_GPS
case 0x8A: case 0x8A:
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
hottPrepareGPSResponse(&hottGPSMessage); hottPrepareGPSResponse(&hottGPSMessage);
hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage)); hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
return true; return true;

96
src/main/telemetry/ibus_shared.c
View file

@ -136,7 +136,11 @@ static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8
static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
#if defined(USE_GPS) #if defined(USE_GPS)
uint8_t fix = 0; uint8_t fix = 0;
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
if (gpsSol.fixType == GPS_NO_FIX) fix = 1; if (gpsSol.fixType == GPS_NO_FIX) fix = 1;
else if (gpsSol.fixType == GPS_FIX_2D) fix = 2; else if (gpsSol.fixType == GPS_FIX_2D) fix = 2;
else if (gpsSol.fixType == GPS_FIX_3D) fix = 3; else if (gpsSol.fixType == GPS_FIX_3D) fix = 3;
@ -196,7 +200,11 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0
uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()]; uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()];
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
status += gpsSol.numSat * 1000; status += gpsSol.numSat * 1000;
status += fix * 100; status += fix * 100;
if (STATE(GPS_FIX_HOME)) status += 500; if (STATE(GPS_FIX_HOME)) status += 500;
@ -206,72 +214,128 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
return sendIbusMeasurement2(address, status); return sendIbusMeasurement2(address, status);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north) } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north)
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t
#endif #endif
return sendIbusMeasurement4(address, 0); return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t
#endif #endif
return sendIbusMeasurement4(address, 0); return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10));
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t
#endif #endif
return sendIbusMeasurement4(address, 0); return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} }

6
src/main/telemetry/jetiexbus.c
View file

@ -575,7 +575,11 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
if (!allSensorsActive) { if (!allSensorsActive) {
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
enableGpsTelemetry(true); enableGpsTelemetry(true);
allSensorsActive = true; allSensorsActive = true;
} }

6
src/main/telemetry/ltm.c
View file

@ -117,7 +117,11 @@ void ltm_gframe(sbuf_t *dst)
uint8_t gps_fix_type = 0; uint8_t gps_fix_type = 0;
int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0; int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0;
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
if (gpsSol.fixType == GPS_NO_FIX) if (gpsSol.fixType == GPS_NO_FIX)
gps_fix_type = 1; gps_fix_type = 1;
else if (gpsSol.fixType == GPS_FIX_2D) else if (gpsSol.fixType == GPS_FIX_2D)

12
src/main/telemetry/mavlink.c
View file

@ -523,7 +523,11 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
{ {
uint8_t gpsFixType = 0; uint8_t gpsFixType = 0;
if (!(sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) if (!(sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
))
return; return;
if (gpsSol.fixType == GPS_NO_FIX) if (gpsSol.fixType == GPS_NO_FIX)
@ -638,7 +642,11 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(USE_GPS) #if defined(USE_GPS)
// use ground speed if source available // use ground speed if source available
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
mavGroundSpeed = gpsSol.groundSpeed / 100.0f; mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
} }
#endif #endif

6
src/main/telemetry/sim.c
View file

@ -348,7 +348,11 @@ static void sendSMS(void)
ZERO_FARRAY(pluscode_url); ZERO_FARRAY(pluscode_url);
if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) { if ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
groundSpeed = gpsSol.groundSpeed / 100; groundSpeed = gpsSol.groundSpeed / 100;
char buf[20]; char buf[20];

6
src/main/telemetry/smartport.c
View file

@ -414,7 +414,11 @@ static bool smartPortShouldSendGPSData(void)
// or the craft has never been armed yet. This way if GPS stops working // or the craft has never been armed yet. This way if GPS stops working
// while in flight, the user will easily notice because the sensor will stop // while in flight, the user will easily notice because the sensor will stop
// updating. // updating.
return feature(FEATURE_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); return feature(FEATURE_GPS) && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
|| !ARMING_FLAG(WAS_EVER_ARMED));
} }
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)

12
src/main/telemetry/srxl.c
View file

@ -303,7 +303,11 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint16_t altitudeLoBcd, groundCourseBcd, hdop;
uint8_t hdopBcd, gpsFlags; uint8_t hdopBcd, gpsFlags;
if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) || gpsSol.numSat < 6) { if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) || gpsSol.numSat < 6) {
return false; return false;
} }
@ -371,7 +375,11 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
uint8_t numSatBcd, altitudeHighBcd; uint8_t numSatBcd, altitudeHighBcd;
bool timeProvided = false; bool timeProvided = false;
if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))|| gpsSol.numSat < 6) { if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)|| gpsSol.numSat < 6) {
return false; return false;
} }