fix(telemetry): Inconsistent/wrong GPS distance calculation (#4442)
This commit is contained in:
@@ -161,12 +161,12 @@ void TelemetryItem::setValue(const TelemetrySensor &sensor, int32_t val,
|
||||
#if defined(INTERNAL_GPS)
|
||||
if (gpsData.fix && gpsData.hdop < PILOTPOS_MIN_HDOP) {
|
||||
pilotLatitude = gpsData.latitude;
|
||||
distFromEarthAxis = getDistFromEarthAxis(pilotLatitude);
|
||||
gps.pilotDistFromEarthAxis = getDistFromEarthAxis(pilotLatitude);
|
||||
}
|
||||
#endif
|
||||
if (!pilotLatitude) {
|
||||
pilotLatitude = newVal;
|
||||
distFromEarthAxis = getDistFromEarthAxis(newVal);
|
||||
gps.pilotDistFromEarthAxis = getDistFromEarthAxis(newVal);
|
||||
}
|
||||
gps.latitude = newVal;
|
||||
setFresh();
|
||||
@@ -380,7 +380,7 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
|
||||
uint32_t result = dist * dist;
|
||||
|
||||
angle = abs(gpsItem.gps.longitude - gpsItem.pilotLongitude);
|
||||
dist = uint64_t(gpsItem.distFromEarthAxis) * angle / 1000000;
|
||||
dist = uint64_t(gpsItem.gps.pilotDistFromEarthAxis) * angle / 1000000;
|
||||
result += dist * dist;
|
||||
|
||||
// Length on ground (ignoring curvature of the earth)
|
||||
|
||||
@@ -32,10 +32,7 @@ constexpr uint8_t TELEMETRY_SENSOR_TEXT_LENGTH = 16;
|
||||
class TelemetryItem
|
||||
{
|
||||
public:
|
||||
union {
|
||||
int32_t value; // value, stored as uint32_t but interpreted accordingly to type
|
||||
uint32_t distFromEarthAxis;
|
||||
};
|
||||
int32_t value; // value, stored as uint32_t but interpreted accordingly to type
|
||||
|
||||
union {
|
||||
int32_t valueMin; // min store
|
||||
@@ -72,9 +69,9 @@ class TelemetryItem
|
||||
struct {
|
||||
int32_t latitude;
|
||||
int32_t longitude;
|
||||
// pilot longitude is stored in min
|
||||
// pilot latitude is stored in max
|
||||
// distFromEarthAxis is stored in value
|
||||
uint32_t pilotDistFromEarthAxis; // pilot distance from earth axis
|
||||
// pilot longitude is stored in min
|
||||
// pilot latitude is stored in max
|
||||
} gps;
|
||||
char text[TELEMETRY_SENSOR_TEXT_LENGTH];
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user