rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix CRSF telmetry GPS speed calculation 

Calculation was assuming that the GPS groundspeed was in 1/10 m/s when in reality it's in cm/s units.  This was causing the value sent in telemetry to be 10 times too large.
This commit is contained in:
Bruce Luckcuck 2018-09-18 07:07:22 -04:00
parent 27cbf0515d
commit 0d7f8f5feb
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/telemetry/crsf.c
View File

@ -180,7 +180,7 @@ void crsfFrameGps(sbuf_t *dst)
sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s
sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10
const uint16_t altitude = (constrain(getEstimatedAltitudeCm(), 0 * 100, 5000 * 100) / 100) + 1000; // constrain altitude from 0 to 5,000m
sbufWriteU16BigEndian(dst, altitude);

2
src/test/unit/telemetry_crsf_unittest.cc
View File

@ -125,7 +125,7 @@ TEST(TelemetryCrsfTest, TestGPS)
gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
ENABLE_STATE(GPS_FIX);
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.groundSpeed = 1630; // speed in cm/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
frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS);