Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Add support for MSP2_SENSOR_GPS message from RemoteID module (#13062)

Browse source
This commit is contained in:
Steve Evans 2023-09-09 00:02:09 +01:00 • committed by GitHub
parent 5cd2ab50e4
commit a72ad8b2b6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 63 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

28
src/main/msp/msp.c
View file

@ -3604,6 +3604,34 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#endif
#ifdef USE_GPS
case MSP2_SENSOR_GPS:
(void)sbufReadU8(src); // instance
(void)sbufReadU16(src); // gps_week
gpsSol.time = sbufReadU32(src); // ms_tow
gpsSetFixState(sbufReadU8(src) != 0); // fix_type
gpsSol.numSat = sbufReadU8(src); // satellites_in_view
gpsSol.acc.hAcc = sbufReadU16(src) * 10; // horizontal_pos_accuracy - convert cm to mm
gpsSol.acc.vAcc = sbufReadU16(src) * 10; // vertical_pos_accuracy - convert cm to mm
gpsSol.acc.sAcc = sbufReadU16(src) * 10; // horizontal_vel_accuracy - convert cm to mm
gpsSol.dop.hdop = sbufReadU16(src); // hdop
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.altCm = sbufReadU32(src); // alt
int32_t ned_vel_north = (int32_t)sbufReadU32(src); // ned_vel_north
int32_t ned_vel_east = (int32_t)sbufReadU32(src); // ned_vel_east
gpsSol.groundSpeed = (uint16_t)sqrtf((ned_vel_north * ned_vel_north) + (ned_vel_east * ned_vel_east));
(void)sbufReadU32(src); // ned_vel_down
gpsSol.groundCourse = ((uint16_t)sbufReadU16(src) % 360); // ground_course
(void)sbufReadU16(src); // true_yaw
(void)sbufReadU16(src); // year
(void)sbufReadU8(src); // month
(void)sbufReadU8(src); // day
(void)sbufReadU8(src); // hour
(void)sbufReadU8(src); // min
(void)sbufReadU8(src); // sec
GPS_update |= GPS_MSP_UPDATE; // MSP data signalisation to GPS functions
break;
case MSP_SET_RAW_GPS:
gpsSetFixState(sbufReadU8(src));
gpsSol.numSat = sbufReadU8(src);