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:
parent
bbdd717505
commit
0e52e21524
23 changed files with 105 additions and 105 deletions
|
@ -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);
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -24,5 +24,5 @@
|
|||
|
||||
bool isAltitudeOffset(void);
|
||||
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
|
||||
int32_t getEstimatedAltitude(void);
|
||||
int32_t getEstimatedAltitudeCm(void);
|
||||
int16_t getEstimatedVario(void);
|
|
@ -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;
|
||||
|
|
|
@ -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) },
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -236,7 +236,7 @@ int32_t getSensorValue(uint8_t sensor)
|
|||
break;
|
||||
|
||||
case EX_ALTITUDE:
|
||||
return getEstimatedAltitude();
|
||||
return getEstimatedAltitudeCm() / 100;
|
||||
break;
|
||||
|
||||
case EX_CAPACITY:
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -1005,7 +1005,7 @@ extern "C" {
|
|||
return simulationMahDrawn;
|
||||
}
|
||||
|
||||
int32_t getEstimatedAltitude() {
|
||||
int32_t getEstimatedAltitudeCm() {
|
||||
return simulationAltitude;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue