Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +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

25
src/main/io/gps.c
View file

@ -1355,10 +1355,29 @@ void gpsUpdate(timeUs_t currentTimeUs)
isFast = false;
rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE));
}
} else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
} else if (gpsConfig()->provider == GPS_MSP) {
if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
if (gpsData.state == GPS_STATE_INITIALIZED) {
gpsSetState(GPS_STATE_RECEIVING_DATA);
}
// Data is available
DEBUG_SET(DEBUG_GPS_CONNECTION, 3, gpsData.now - gpsData.lastNavMessage); // interval since last Nav data was received
gpsData.lastNavMessage = gpsData.now;
sensorsSet(SENSOR_GPS);
GPS_update ^= GPS_DIRECT_TICK;
onGpsNewData();
GPS_update &= ~GPS_MSP_UPDATE;
} else {
DEBUG_SET(DEBUG_GPS_CONNECTION, 2, gpsData.now - gpsData.lastNavMessage); // time since last Nav data, updated each GPS task interval
// check for no data/gps timeout/cable disconnection etc
if (cmp32(gpsData.now, gpsData.lastNavMessage) > GPS_TIMEOUT_MS) {
gpsSetState(GPS_STATE_LOST_COMMUNICATION);
}
}
}
switch (gpsData.state) {
@ -1382,6 +1401,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
case GPS_STATE_RECEIVING_DATA:
#ifdef USE_GPS_UBLOX
if (gpsConfig()->provider != GPS_MSP) {
if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) {
// when we are connected up, and get a 3D fix, enable the 'flight' fix model
if (!gpsData.ubloxUsingFlightModel && STATE(GPS_FIX)) {
@ -1389,6 +1409,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model);
}
}
}
#endif
DEBUG_SET(DEBUG_GPS_CONNECTION, 2, gpsData.now - gpsData.lastNavMessage); // time since last Nav data, updated each GPS task interval
// check for no data/gps timeout/cable disconnection etc
@ -1434,7 +1455,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
schedulerSetNextStateTime(gpsStateDurationFractionUs[gpsCurrentState] >> GPS_TASK_DECAY_SHIFT);
DEBUG_SET(DEBUG_GPS_CONNECTION, 5, executeTimeUs);
// keeping temporarily, to be used when debugging the sheduler stuff
// keeping temporarily, to be used when debugging the scheduler stuff
// DEBUG_SET(DEBUG_GPS_CONNECTION, 6, (gpsStateDurationFractionUs[gpsCurrentState] >> GPS_TASK_DECAY_SHIFT));
}

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);

3
src/main/msp/msp_protocol_v2_common.h
View file

@ -20,3 +20,6 @@
#define MSP2_COMMON_SERIAL_CONFIG 0x1009
#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A
// Sensors
#define MSP2_SENSOR_GPS 0x1F03