From 70f3d76f6ca55df2dc9ebbfab8ea179f331778b2 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Tue, 19 Sep 2023 09:50:43 +0100 Subject: [PATCH] Add support for MSP2_SENSOR_GPS message from RemoteID module (#13081) --- src/main/io/gps.c | 85 ++++++++++++++++----------- src/main/msp/msp.c | 28 +++++++++ src/main/msp/msp_protocol_v2_common.h | 3 + 3 files changed, 83 insertions(+), 33 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 83ebb855fb..a89b6fb30a 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -782,10 +782,27 @@ void gpsUpdate(timeUs_t currentTimeUs) } // Restore default task rate rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE)); - } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP - gpsSetState(GPS_STATE_RECEIVING_DATA); - onGpsNewData(); - GPS_update &= ~GPS_MSP_UPDATE; + } 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 + gpsData.lastMessage = millis(); + sensorsSet(SENSOR_GPS); + + GPS_update ^= GPS_DIRECT_TICK; + + onGpsNewData(); + + GPS_update &= ~GPS_MSP_UPDATE; + } else { + // check for no data/gps timeout/cable disconnection etc + if (cmp32(millis(), gpsData.lastMessage) > GPS_TIMEOUT) { + gpsSetState(GPS_STATE_LOST_COMMUNICATION); + } + } } #if DEBUG_UBLOX_INIT @@ -823,37 +840,39 @@ void gpsUpdate(timeUs_t currentTimeUs) gpsSetState(GPS_STATE_LOST_COMMUNICATION); #ifdef USE_GPS_UBLOX } else { - if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled - switch (gpsData.state_position) { - case 0: - if (!isConfiguratorConnected()) { - if (gpsData.ubloxUseSAT) { - ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG + if (gpsConfig()->provider != GPS_MSP) { + if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled + switch (gpsData.state_position) { + case 0: + if (!isConfiguratorConnected()) { + if (gpsData.ubloxUseSAT) { + ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG + } + gpsData.state_position = 1; } - gpsData.state_position = 1; - } - break; - case 1: - if (STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) { - ubloxSendNAV5Message(true); - gpsData.state_position = 2; - } - if (isConfiguratorConnected()) { - gpsData.state_position = 2; - } - break; - case 2: - if (isConfiguratorConnected()) { - if (gpsData.ubloxUseSAT) { - ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles) - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles) + break; + case 1: + if (STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) { + ubloxSendNAV5Message(true); + gpsData.state_position = 2; } - gpsData.state_position = 0; - } - break; + if (isConfiguratorConnected()) { + gpsData.state_position = 2; + } + break; + case 2: + if (isConfiguratorConnected()) { + if (gpsData.ubloxUseSAT) { + ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles) + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles) + } + gpsData.state_position = 0; + } + break; + } } } #endif //USE_GPS_UBLOX diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index b730a221f2..5e78f1c300 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -3583,6 +3583,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 + (void)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); diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index 2031df2bbe..1f2f42a42c 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -20,3 +20,6 @@ #define MSP2_COMMON_SERIAL_CONFIG 0x1009 #define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A + +// Sensors +#define MSP2_SENSOR_GPS 0x1F03