Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[gps] fix parsing of PVT heading (#2939)

Browse Source 
the flag headVehValid only concerns the headVeh (vehicle heading,
computed from dual antenna configuration), not the headMot, which is the
heading of motion, aka the ground course, always "valid"
This commit is contained in:
Gautier Hattenberger
2022-10-26 11:46:15 +02:00
committed by GitHub
parent ad01121a17
commit 82d899ae0c
1 changed files with 7 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/gps/gps_ubx.c
+7 -11
View File
@@ -184,17 +184,13 @@ static void gps_ubx_parse_nav_pvt(void)
gps_ubx.state.lla_pos.alt = UBX_NAV_PVT_height(gps_ubx.msg_buf);
SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_LLA_BIT);
// Copy heading if valid
uint8_t headVehValid = bit_is_set(flags, 5);
if (headVehValid) {
// Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
// I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
// solution: First to radians, and then scale to 1e-7 radians
// First x 10 for loosing less resolution, then to radians, then multiply x 10 again
gps_ubx.state.course = (RadOfDeg(UBX_NAV_PVT_headMot(gps_ubx.msg_buf) * 10)) * 10;
SetBit(gps_ubx.state.valid_fields, GPS_VALID_COURSE_BIT);
gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_PVT_headAcc(gps_ubx.msg_buf) * 10)) * 10;
}
// Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
// I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
// solution: First to radians, and then scale to 1e-7 radians
// First x 10 for loosing less resolution, then to radians, then multiply x 10 again
gps_ubx.state.course = (RadOfDeg(UBX_NAV_PVT_headMot(gps_ubx.msg_buf) * 10)) * 10;
SetBit(gps_ubx.state.valid_fields, GPS_VALID_COURSE_BIT);
gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_PVT_headAcc(gps_ubx.msg_buf) * 10)) * 10;
// Copy HMSL and ground speed
gps_ubx.state.hmsl = UBX_NAV_PVT_hMSL(gps_ubx.msg_buf);