1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00

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()
This commit is contained in:
AirBreak69 2018-06-24 12:52:22 +02:00 committed by mikeller
parent bbdd717505
commit 0e52e21524
23 changed files with 105 additions and 105 deletions

View file

@ -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);

View file

@ -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

View file

@ -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;

View file

@ -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.

View file

@ -24,5 +24,5 @@
bool isAltitudeOffset(void);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
int32_t getEstimatedAltitude(void);
int32_t getEstimatedAltitudeCm(void);
int16_t getEstimatedVario(void);

View file

@ -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;

View file

@ -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) },

View file

@ -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 {

View file

@ -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 {

View file

@ -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) {

View file

@ -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);

View file

@ -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);
}

View file

@ -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

View file

@ -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;

View file

@ -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;

View file

@ -236,7 +236,7 @@ int32_t getSensorValue(uint8_t sensor)
break;
case EX_ALTITUDE:
return getEstimatedAltitude();
return getEstimatedAltitudeCm() / 100;
break;
case EX_CAPACITY:

View file

@ -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);

View file

@ -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

View file

@ -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

View file

@ -1005,7 +1005,7 @@ extern "C" {
return simulationMahDrawn;
}
int32_t getEstimatedAltitude() {
int32_t getEstimatedAltitudeCm() {
return simulationAltitude;
}

View file

@ -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;}

View file

@ -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;
}

View file

@ -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;