From 0e52e21524aa5de816f28530c5880af853338379 Mon Sep 17 00:00:00 2001 From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com> Date: Sun, 24 Jun 2018 12:52:22 +0200 Subject: [PATCH] GPS altitude: cleanup all occurancies to assume source is in cm per lsb resolution Harmonized (and partly corrected) all occurancies of gpsSol.llh.alt and getEstimatedAltitude() to handle altiude sourced in cm resolution. This was introduced by GSP_RESCUE/RTH. Introduced a naming convention that include the unit into the variable/function names: gpsSol.llh.alt -> gpsSol.llh.alt_cm getEstimatedAltitude() -> getEstimatedAltitude_cm() --- src/main/blackbox/blackbox.c | 2 +- src/main/flight/gps_rescue.c | 42 ++++++++++---------- src/main/flight/gps_rescue.h | 14 +++---- src/main/flight/position.c | 14 +++---- src/main/flight/position.h | 2 +- src/main/interface/msp.c | 6 +-- src/main/interface/settings.c | 4 +- src/main/io/gps.c | 10 ++--- src/main/io/gps.h | 2 +- src/main/io/osd.c | 14 +++---- src/main/target/COLIBRI_RACE/i2c_bst.c | 8 ++-- src/main/telemetry/crsf.c | 4 +- src/main/telemetry/frsky_hub.c | 35 ++++++---------- src/main/telemetry/hott.c | 9 +++-- src/main/telemetry/ibus_shared.c | 2 +- src/main/telemetry/jetiexbus.c | 2 +- src/main/telemetry/ltm.c | 4 +- src/main/telemetry/mavlink.c | 14 +++---- src/main/telemetry/smartport.c | 6 +-- src/test/unit/osd_unittest.cc | 2 +- src/test/unit/telemetry_crsf_msp_unittest.cc | 4 ++ src/test/unit/telemetry_crsf_unittest.cc | 6 ++- src/test/unit/telemetry_hott_unittest.cc | 4 +- 23 files changed, 105 insertions(+), 105 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 325aa5b297..46a1bb9135 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -977,7 +977,7 @@ static void writeGPSFrame(timeUs_t currentTimeUs) blackboxWriteUnsignedVB(gpsSol.numSat); blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]); blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]); - blackboxWriteUnsignedVB(gpsSol.llh.alt); + blackboxWriteUnsignedVB(gpsSol.llh.altCm / 10); // was originally designed to transport meters in int16, but +-3276.7m is a good compromise blackboxWriteUnsignedVB(gpsSol.groundSpeed); blackboxWriteUnsignedVB(gpsSol.groundCourse); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 76c972e987..74d145e9bf 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -58,8 +58,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCU PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .angle = 32, - .initialAltitude = 50, - .descentDistance = 200, + .initialAltitudeM = 50, + .descentDistanceM = 200, .rescueGroundspeed = 2000, .throttleP = 150, .throttleI = 20, @@ -125,8 +125,8 @@ void updateGPSRescueState(void) } // Minimum distance detection. Disarm regardless of sanity check configuration. Rescue too close is never a good idea. - if (rescueState.sensor.distanceToHome < gpsRescueConfig()->minRescueDth) { - // Never allow rescue mode to engage as a failsafe when too close or when disarmed. + if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) { + // Never allow rescue mode to engage as a failsafe when too close or when disarmed. if (rescueState.isFailsafe || !ARMING_FLAG(ARMED)) { rescueState.failure = RESCUE_TOO_CLOSE; setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); @@ -141,40 +141,40 @@ void updateGPSRescueState(void) FALLTHROUGH; case RESCUE_ATTAIN_ALT: // Get to a safe altitude at a low velocity ASAP - if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) { + if (ABS(rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) < 1000) { rescueState.phase = RESCUE_CROSSTRACK; } rescueState.intent.targetGroundspeed = 500; - rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); + rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500); rescueState.intent.crosstrack = true; rescueState.intent.minAngleDeg = 10; rescueState.intent.maxAngleDeg = 15; break; case RESCUE_CROSSTRACK: - if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) { + if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) { rescueState.phase = RESCUE_LANDING_APPROACH; } // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt // Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed; - rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500); + rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500); rescueState.intent.crosstrack = true; rescueState.intent.minAngleDeg = 15; rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; break; case RESCUE_LANDING_APPROACH: // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase - if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) { + if (rescueState.sensor.distanceToHomeM < 10 && rescueState.sensor.currentAltitudeCm <= 1000) { rescueState.phase = RESCUE_LANDING; } // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot) - int32_t newAlt = gpsRescueConfig()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; - int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance; + int32_t newAlt = gpsRescueConfig()->initialAltitudeM * 100 * rescueState.sensor.distanceToHomeM / gpsRescueConfig()->descentDistanceM; + int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / gpsRescueConfig()->descentDistanceM; - rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude); + rescueState.intent.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm); rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); rescueState.intent.crosstrack = true; rescueState.intent.minAngleDeg = 10; @@ -192,7 +192,7 @@ void updateGPSRescueState(void) } rescueState.intent.targetGroundspeed = 0; - rescueState.intent.targetAltitude = 0; + rescueState.intent.targetAltitudeCm = 0; rescueState.intent.crosstrack = true; rescueState.intent.minAngleDeg = 0; rescueState.intent.maxAngleDeg = 15; @@ -220,28 +220,28 @@ void updateGPSRescueState(void) void sensorUpdate() { - rescueState.sensor.currentAltitude = getEstimatedAltitude(); + rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm(); // Calculate altitude velocity static uint32_t previousTimeUs; - static int32_t previousAltitude; + static int32_t previousAltitudeCm; const uint32_t currentTimeUs = micros(); const float dTime = currentTimeUs - previousTimeUs; if (newGPSData) { // Calculate velocity at lowest common denominator - rescueState.sensor.distanceToHome = GPS_distanceToHome; + rescueState.sensor.distanceToHomeM = GPS_distanceToHome; rescueState.sensor.directionToHome = GPS_directionToHome; rescueState.sensor.numSat = gpsSol.numSat; rescueState.sensor.groundSpeed = gpsSol.groundSpeed; - rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitude - previousAltitude) * 1000000.0f / dTime; + rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime; rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f; rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec; rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f); - previousAltitude = rescueState.sensor.currentAltitude; + previousAltitudeCm = rescueState.sensor.currentAltitudeCm; previousTimeUs = currentTimeUs; } } @@ -327,9 +327,9 @@ void idleTasks() gpsRescueAngle[AI_ROLL] = 0; // Store the max altitude we see not during RTH so we know our fly-back minimum alt - rescueState.sensor.maxAltitude = MAX(rescueState.sensor.currentAltitude, rescueState.sensor.maxAltitude); + rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm); // Store the max distance to home during normal flight so we know if a flyaway is happening - rescueState.sensor.maxDistanceToHome = MAX(rescueState.sensor.distanceToHome, rescueState.sensor.maxDistanceToHome); + rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM); rescueThrottle = rcCommand[THROTTLE]; @@ -391,7 +391,7 @@ void rescueAttainPosition() static float previousAltitudeError = 0; static int16_t altitudeIntegral = 0; - const int16_t altitudeError = (rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) / 100; // Error in meters + const int16_t altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100; // Error in meters const int16_t altitudeDerivative = altitudeError - previousAltitudeError; // Only allow integral windup within +-15m absolute altitude error diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index ea7ba327b3..dba47f39f8 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -27,8 +27,8 @@ typedef enum { typedef struct gpsRescue_s { uint16_t angle; //degrees - uint16_t initialAltitude; //meters - uint16_t descentDistance; //meters + uint16_t initialAltitudeM; //meters + uint16_t descentDistanceM; //meters uint16_t rescueGroundspeed; // centimeters per second uint16_t throttleP, throttleI, throttleD; uint16_t yawP; @@ -62,7 +62,7 @@ typedef enum { } rescueFailureState_e; typedef struct { - int32_t targetAltitude; + int32_t targetAltitudeCm; int32_t targetGroundspeed; uint8_t minAngleDeg; uint8_t maxAngleDeg; @@ -70,10 +70,10 @@ typedef struct { } rescueIntent_s; typedef struct { - int32_t maxAltitude; - int32_t currentAltitude; - uint16_t distanceToHome; - uint16_t maxDistanceToHome; + int32_t maxAltitudeCm; + int32_t currentAltitudeCm; + uint16_t distanceToHomeM; + uint16_t maxDistanceToHomeM; int16_t directionToHome; uint16_t groundSpeed; uint8_t numSat; diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 771baa0f04..8b0c716ec5 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -38,7 +38,7 @@ #include "sensors/sensors.h" #include "sensors/barometer.h" -static int32_t estimatedAltitude = 0; // in cm +static int32_t estimatedAltitudeCm = 0; // in cm #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) @@ -77,7 +77,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) #ifdef USE_GPS if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - gpsAlt = gpsSol.llh.alt; + gpsAlt = gpsSol.llh.altCm; haveGpsAlt = true; if (gpsSol.hdop != 0) { @@ -99,11 +99,11 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { gpsAlt -= gpsAltOffset; if (haveGpsAlt && haveBaroAlt) { - estimatedAltitude = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust); + estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust); } else if (haveGpsAlt) { - estimatedAltitude = gpsAlt; + estimatedAltitudeCm = gpsAlt; } else if (haveBaroAlt) { - estimatedAltitude = baroAlt; + estimatedAltitudeCm = baroAlt; } DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust)); @@ -117,9 +117,9 @@ bool isAltitudeOffset(void) } #endif -int32_t getEstimatedAltitude(void) +int32_t getEstimatedAltitudeCm(void) { - return estimatedAltitude; + return estimatedAltitudeCm; } // This should be removed or fixed, but it would require changing a lot of other things to get rid of. diff --git a/src/main/flight/position.h b/src/main/flight/position.h index c7b98173b4..70a7bfc1c7 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -24,5 +24,5 @@ bool isAltitudeOffset(void); void calculateEstimatedAltitude(timeUs_t currentTimeUs); -int32_t getEstimatedAltitude(void); +int32_t getEstimatedAltitudeCm(void); int16_t getEstimatedVario(void); \ No newline at end of file diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index e2aecb14d5..ab195a6ba0 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -920,7 +920,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) case MSP_ALTITUDE: #if defined(USE_BARO) || defined(USE_RANGEFINDER) - sbufWriteU32(dst, getEstimatedAltitude()); + sbufWriteU32(dst, getEstimatedAltitudeCm()); #else sbufWriteU32(dst, 0); #endif @@ -1045,7 +1045,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, gpsSol.numSat); sbufWriteU32(dst, gpsSol.llh.lat); sbufWriteU32(dst, gpsSol.llh.lon); - sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.alt / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again. + sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again. sbufWriteU16(dst, gpsSol.groundSpeed); sbufWriteU16(dst, gpsSol.groundCourse); break; @@ -2109,7 +2109,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gpsSol.numSat = sbufReadU8(src); gpsSol.llh.lat = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src); - gpsSol.llh.alt = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled. + gpsSol.llh.altCm = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled. gpsSol.groundSpeed = sbufReadU16(src); GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers break; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index a0ed8a0990..2f1f444cb3 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -752,8 +752,8 @@ const clivalue_t valueTable[] = { #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE { "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) }, - { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitude) }, - { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistance) }, + { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) }, + { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) }, { "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) }, { "gps_rescue_throttle_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) }, { "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) }, diff --git a/src/main/io/gps.c b/src/main/io/gps.c index edd1853e10..19c0ad0447 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -673,7 +673,7 @@ typedef struct gpsDataNmea_s { int32_t latitude; int32_t longitude; uint8_t numSat; - int32_t altitude; + int32_t altitudeCm; uint16_t speed; uint16_t hdop; uint16_t ground_course; @@ -745,7 +745,7 @@ static bool gpsNewFrameNMEA(char c) gps_Msg.hdop = grab_fields(string, 1) * 100; // hdop break; case 9: - gps_Msg.altitude = grab_fields(string, 1) * 10; // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10 + gps_Msg.altitudeCm = grab_fields(string, 1) * 10; // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10 break; } break; @@ -836,7 +836,7 @@ static bool gpsNewFrameNMEA(char c) gpsSol.llh.lat = gps_Msg.latitude; gpsSol.llh.lon = gps_Msg.longitude; gpsSol.numSat = gps_Msg.numSat; - gpsSol.llh.alt = gps_Msg.altitude; + gpsSol.llh.altCm = gps_Msg.altitudeCm; gpsSol.hdop = gps_Msg.hdop; } break; @@ -891,7 +891,7 @@ typedef struct { int32_t longitude; int32_t latitude; int32_t altitude_ellipsoid; - int32_t altitude_msl; + int32_t altitudeMslMm; uint32_t horizontal_accuracy; uint32_t vertical_accuracy; } ubx_nav_posllh; @@ -1060,7 +1060,7 @@ static bool UBLOX_parse_gps(void) //i2c_dataset.time = _buffer.posllh.time; gpsSol.llh.lon = _buffer.posllh.longitude; gpsSol.llh.lat = _buffer.posllh.latitude; - gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm + gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm if (next_fix) { ENABLE_STATE(GPS_FIX); } else { diff --git a/src/main/io/gps.h b/src/main/io/gps.h index d9bbf9a655..cc35885f29 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -86,7 +86,7 @@ typedef struct gpsCoordinateDDDMMmmmm_s { typedef struct gpsLocation_s { int32_t lat; // latitude * 1e+7 int32_t lon; // longitude * 1e+7 - int32_t alt; // altitude in 0.01m + int32_t altCm; // altitude in 0.01m } gpsLocation_t; typedef struct gpsSolutionData_s { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2558627059..623fcbd44d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -284,9 +284,9 @@ static char osdGetTemperatureSymbolForSelectedUnit(void) } #endif -static void osdFormatAltitudeString(char * buff, int altitude) +static void osdFormatAltitudeString(char * buff, int32_t altitudeCm) { - const int alt = osdGetMetersToSelectedUnit(altitude) / 10; + const int alt = osdGetMetersToSelectedUnit(altitudeCm) / 10; tfp_sprintf(buff, "%5d %c", alt, osdGetMetersToSelectedUnitSymbol()); buff[5] = buff[4]; @@ -564,7 +564,7 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_ALTITUDE: - osdFormatAltitudeString(buff, getEstimatedAltitude()); + osdFormatAltitudeString(buff, getEstimatedAltitudeCm()); break; case OSD_ITEM_TIMER_1: @@ -1140,7 +1140,7 @@ void osdUpdateAlarms(void) { // This is overdone? - int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100; + int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100; if (getRssiPercent() < osdConfig()->rssi_alarm) { SET_BLINK(OSD_RSSI_VALUE); @@ -1273,9 +1273,9 @@ static void osdUpdateStats(void) stats.min_rssi = value; } - int altitude = getEstimatedAltitude(); - if (stats.max_altitude < altitude) { - stats.max_altitude = altitude; + int32_t altitudeCm = getEstimatedAltitudeCm(); + if (stats.max_altitude < altitudeCm) { + stats.max_altitude = altitudeCm; } if (stats.max_g_force < osdGForce) { diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 78b956a523..cf2f4f6058 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -796,23 +796,23 @@ static void bstMasterWrite32(uint32_t data) static int32_t lat = 0; static int32_t lon = 0; -static uint16_t alt = 0; +static uint16_t altM = 0; static uint8_t numOfSat = 0; #endif #ifdef USE_GPS bool writeGpsPositionPrameToBST(void) { - if ((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) { + if ((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != (gpsSol.llh.altCm / 100)) || (numOfSat != gpsSol.numSat)) { lat = gpsSol.llh.lat; lon = gpsSol.llh.lon; - alt = gpsSol.llh.alt; + altM = gpsSol.llh.altCm / 100; numOfSat = gpsSol.numSat; uint16_t speed = (gpsSol.groundSpeed * 9 / 25); uint16_t gpsHeading = 0; uint16_t altitude = 0; gpsHeading = gpsSol.groundCourse * 10; - altitude = alt * 10 + 1000; + altitude = altM + 1000; // To be verified: in m +1000m offset for neg. altitudes? bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterWrite8(GPS_POSITION_FRAME_ID); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 7b46487219..6f9fd09254 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -49,6 +49,7 @@ #include "fc/runtime_config.h" #include "flight/imu.h" +#include "flight/position.h" #include "interface/crsf_protocol.h" @@ -181,8 +182,7 @@ void crsfFrameGps(sbuf_t *dst) sbufWriteU32BigEndian(dst, gpsSol.llh.lon); sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10 - //Send real GPS altitude only if it's reliable (there's a GPS fix) - const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt / 100 : 0) + 1000; + const uint16_t altitude = (constrain(getEstimatedAltitudeCm(), 0 * 100, 5000 * 100) / 100) + 1000; // constrain altitude from 0 to 5,000m sbufWriteU16BigEndian(dst, altitude); sbufWriteU8(dst, gpsSol.numSat); } diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 7bbd05edcd..bc4ef15acc 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -269,14 +269,14 @@ static void sendLatLong(int32_t coord[2]) #if defined(USE_GPS) static void sendGpsAltitude(void) { - uint16_t altitude = gpsSol.llh.alt; + int32_t altitudeCm = gpsSol.llh.altCm; // Send real GPS altitude only if it's reliable (there's a GPS fix) if (!STATE(GPS_FIX)) { - altitude = 0; + altitudeCm = 0; } - frSkyHubWriteFrame(ID_GPS_ALTIDUTE_BP, altitude); - frSkyHubWriteFrame(ID_GPS_ALTIDUTE_AP, 0); + frSkyHubWriteFrame(ID_GPS_ALTIDUTE_BP, altitudeCm / 100); // meters: integer part, eg. 123 from 123.45m + frSkyHubWriteFrame(ID_GPS_ALTIDUTE_AP, altitudeCm % 100); // meters: fractional part, eg. 45 from 123.45m } static void sendSatalliteSignalQualityAsTemperature2(uint8_t cycleNum) @@ -340,17 +340,6 @@ static void sendGPSLatLong(void) #endif #endif -#if defined(USE_BARO) || defined(USE_RANGEFINDER) -/* - * Send vertical speed for opentx. ID_VERT_SPEED - * Unit is cm/s - */ -static void sendVario(void) -{ - frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario()); -} -#endif - /* * Send voltage via ID_VOLT * @@ -535,23 +524,25 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs) sendAccel(); } -#if defined(USE_BARO) || defined(USE_RANGEFINDER) - if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER)) { +#if defined(USE_BARO) || defined(USE_RANGEFINDER) || defined(USE_GPS) + if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER) | sensors(SENSOR_GPS)) { // Sent every 125ms - sendVario(); + // Send vertical speed for opentx. ID_VERT_SPEED + // Unit is cm/s + frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario()); // Sent every 500ms if ((cycleNum % 4) == 0) { - int16_t altitude = ABS(getEstimatedAltitude()); + int32_t altitudeCm = getEstimatedAltitudeCm(); /* Allow 5s to boot correctly othervise send zero to prevent OpenTX * sensor lost notifications after warm boot. */ if (frSkyHubLastCycleTime < DELAY_FOR_BARO_INITIALISATION_US) { - altitude = 0; + altitudeCm = 0; } - frSkyHubWriteFrame(ID_ALTITUDE_BP, altitude / 100); - frSkyHubWriteFrame(ID_ALTITUDE_AP, altitude % 100); + frSkyHubWriteFrame(ID_ALTITUDE_BP, altitudeCm / 100); // meters: integer part, eg. 123 from 123.45m + frSkyHubWriteFrame(ID_ALTITUDE_AP, altitudeCm % 100); // meters: fractional part, eg. 45 from 123.45m } } #endif diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 2aea088980..073e13aa45 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -69,6 +69,7 @@ #include "build/debug.h" #include "common/axis.h" +#include "common/maths.h" #include "common/time.h" #include "drivers/serial.h" @@ -236,12 +237,12 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF; hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8; - uint16_t altitude = gpsSol.llh.alt; + int32_t altitudeM = gpsSol.llh.altCm / 100; if (!STATE(GPS_FIX)) { - altitude = getEstimatedAltitude(); + altitudeM = getEstimatedAltitudeCm() / 100; } - const uint16_t hottGpsAltitude = (altitude / 100) + HOTT_GPS_ALTITUDE_OFFSET; // gpsSol.llh.alt in m ; offset = 500 -> O m + const uint16_t hottGpsAltitude = constrain(altitudeM + HOTT_GPS_ALTITUDE_OFFSET, 0 , UINT16_MAX); // gpsSol.llh.alt in m ; offset = 500 -> O m hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF; hottGPSMessage->altitude_H = hottGpsAltitude >> 8; @@ -296,7 +297,7 @@ static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMess static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage) { - const uint16_t hottEamAltitude = (getEstimatedAltitude() / 100) + HOTT_EAM_OFFSET_HEIGHT; + const uint16_t hottEamAltitude = (getEstimatedAltitudeCm() / 100) + HOTT_EAM_OFFSET_HEIGHT; hottEAMMessage->altitude_L = hottEamAltitude & 0x00FF; hottEAMMessage->altitude_H = hottEamAltitude >> 8; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 89998f6fe5..38481bc181 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -318,7 +318,7 @@ static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value) value->int32 = gpsSol.llh.lon; break; case IBUS_SENSOR_TYPE_GPS_ALT: - value->int32 = (int32_t)gpsSol.llh.alt; + value->int32 = (int32_t)gpsSol.llh.altCm; break; case IBUS_SENSOR_TYPE_GROUND_SPEED: value->uint16 = gpsSol.groundSpeed; diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 3f14cfe7bc..2c28c4a3c9 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -236,7 +236,7 @@ int32_t getSensorValue(uint8_t sensor) break; case EX_ALTITUDE: - return getEstimatedAltitude(); + return getEstimatedAltitudeCm() / 100; break; case EX_CAPACITY: diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 17412e13fb..9209a9c854 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -148,9 +148,9 @@ static void ltm_gframe(void) ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100)); #if defined(USE_BARO) || defined(USE_RANGEFINDER) - ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100; + ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitudeCm() : gpsSol.llh.altCm; #else - ltm_alt = gpsSol.llh.alt * 100; + ltm_alt = gpsSol.llh.altCm; #endif ltm_serialise_32(ltm_alt); ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 9b60bdc1c7..4475e2b42b 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -328,7 +328,7 @@ void mavlinkSendPosition(void) // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 1000, + gpsSol.llh.altCm * 10, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 @@ -351,12 +351,12 @@ void mavlinkSendPosition(void) // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 1000, + 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)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000, + (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitudeCm() * 10 : gpsSol.llh.altCm * 10, #else - gpsSol.llh.alt * 1000, + gpsSol.llh.altCm * 10, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, @@ -423,18 +423,18 @@ void mavlinkSendHUDAndHeartbeat(void) #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 = getEstimatedAltitude() / 100.0; + 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.alt; + 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.alt; + mavAltitude = gpsSol.llh.altCm / 100.0; } #endif diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index a3b8234558..1b7ceabb87 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -635,7 +635,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; #endif case FSSP_DATAID_ALTITUDE : - smartPortSendPackage(id, getEstimatedAltitude()); // unknown given unit, requested 100 = 1 meter + smartPortSendPackage(id, getEstimatedAltitudeCm()); // unknown given unit, requested 100 = 1 meter *clearToSend = false; break; case FSSP_DATAID_FUEL : @@ -794,7 +794,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; case FSSP_DATAID_GPS_ALT : if (STATE(GPS_FIX)) { - smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) + smartPortSendPackage(id, gpsSol.llh.altCm * 10); // given in 0.01m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) *clearToSend = false; } break; @@ -829,7 +829,7 @@ void handleSmartPortTelemetry(void) payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true); } - processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); + processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); } } #endif diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 2d36e1426d..8df7652730 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1005,7 +1005,7 @@ extern "C" { return simulationMahDrawn; } - int32_t getEstimatedAltitude() { + int32_t getEstimatedAltitudeCm() { return simulationAltitude; } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 40374e6209..51a5fbeb36 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -268,6 +268,10 @@ extern "C" { return 67; } + int32_t getEstimatedAltitudeCm(void) { + return 0; + } + bool feature(uint32_t) {return false;} bool isAirmodeActive(void) {return true;} diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index cd9e211069..ed0a29a642 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -124,7 +124,7 @@ TEST(TelemetryCrsfTest, TestGPS) gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER; gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER; ENABLE_STATE(GPS_FIX); - gpsSol.llh.alt = 2345 * 100; // altitude in cm + gpsSol.llh.altCm = 2345 * 100; // altitude in cm / 100 + 1000m offset, so CRSF value should be 3345 gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 gpsSol.numSat = 9; gpsSol.groundCourse = 1479; // degrees * 10 @@ -330,6 +330,10 @@ uint8_t calculateBatteryPercentageRemaining(void) { return 67; } +int32_t getEstimatedAltitudeCm(void) { + return gpsSol.llh.altCm; // function returns cm not m. +} + int32_t getMAhDrawn(void){ return testmAhDrawn; } diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 8a00bce26d..08829bc2c5 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -180,8 +180,8 @@ uint32_t fixedMillis = 0; baro_t baro; -uint32_t getEstimatedAltitude() { return 0; } -uint32_t getEstimatedVario() { return 0; } +int32_t getEstimatedAltitudeCm() { return 0; } +int16_t getEstimatedVario() { return 0; } uint32_t millis(void) { return fixedMillis;