diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 25118a361e..9720c9c067 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1189,11 +1189,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) break; case MSP_ALTITUDE: -#if defined(USE_BARO) || defined(USE_RANGEFINDER) sbufWriteU32(dst, getEstimatedAltitudeCm()); -#else - sbufWriteU32(dst, 0); -#endif #ifdef USE_VARIO sbufWriteU16(dst, getEstimatedVario()); #else diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index d4f5a21fa0..88c7c72fdf 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -237,10 +237,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF; hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8; - int32_t altitudeM = gpsSol.llh.altCm / 100; - if (!STATE(GPS_FIX)) { - altitudeM = getEstimatedAltitudeCm() / 100; - } + int32_t altitudeM = getEstimatedAltitudeCm() / 100; const uint16_t hottGpsAltitude = constrain(altitudeM + HOTT_GPS_ALTITUDE_OFFSET, 0 , UINT16_MAX); // gpsSol.llh.alt in m ; offset = 500 -> O m diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 5d6df83249..e174cd23c7 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -146,12 +146,7 @@ static void ltm_gframe(void) ltm_serialise_32(gpsSol.llh.lat); ltm_serialise_32(gpsSol.llh.lon); ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100)); - -#if defined(USE_BARO) || defined(USE_RANGEFINDER) - ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitudeCm() : gpsSol.llh.altCm; -#else - ltm_alt = gpsSol.llh.altCm; -#endif + ltm_alt = getEstimatedAltitudeCm(); ltm_serialise_32(ltm_alt); ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type); ltm_finalise(); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 8355687172..61a003c5f1 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -353,11 +353,7 @@ void mavlinkSendPosition(void) // alt Altitude in 1E3 meters (millimeters) above MSL gpsSol.llh.altCm * 10, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) -#if defined(USE_BARO) || defined(USE_RANGEFINDER) - (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitudeCm() * 10 : gpsSol.llh.altCm * 10, -#else - gpsSol.llh.altCm * 10, -#endif + getEstimatedAltitudeCm() * 10, // Ground X Speed (Latitude), expressed as m/s * 100 0, // Ground Y Speed (Longitude), expressed as m/s * 100 @@ -419,24 +415,7 @@ void mavlinkSendHUDAndHeartbeat(void) } #endif - // select best source for altitude -#if defined(USE_BARO) || defined(USE_RANGEFINDER) - if (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) { - // Baro or sonar generally is a better estimate of altitude than GPS MSL altitude - mavAltitude = getEstimatedAltitudeCm() / 100.0; - } -#if defined(USE_GPS) - else if (sensors(SENSOR_GPS)) { - // No sonar or baro, just display altitude above MLS - mavAltitude = gpsSol.llh.altCm / 100.0; - } -#endif -#elif defined(USE_GPS) - if (sensors(SENSOR_GPS)) { - // No sonar or baro, just display altitude above MLS - mavAltitude = gpsSol.llh.altCm / 100.0; - } -#endif + mavAltitude = getEstimatedAltitudeCm() / 100.0; mavlink_msg_vfr_hud_pack(0, 200, &mavMsg, // airspeed Current airspeed in m/s