diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 7a1b547e65..9ceb4498a2 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -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 - 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 + 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,11 +1401,13 @@ void gpsUpdate(timeUs_t currentTimeUs) case GPS_STATE_RECEIVING_DATA: #ifdef USE_GPS_UBLOX - 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)) { - gpsData.ubloxUsingFlightModel = true; - ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model); + 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)) { + gpsData.ubloxUsingFlightModel = true; + ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model); + } } } #endif @@ -1434,14 +1455,14 @@ 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)); } static void gpsNewData(uint16_t c) { DEBUG_SET(DEBUG_GPS_CONNECTION, 1, gpsSol.navIntervalMs); - if (!gpsNewFrame(c)) { + if (!gpsNewFrame(c)) { // no new nav solution data return; } diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index ceed8ab02e..33759885cf 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -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); 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