Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

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

Browse source
This commit is contained in:
Steve Evans 2023-09-19 09:50:43 +01:00 • committed by GitHub
parent c51762dd96
commit 70f3d76f6c
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 83 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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

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

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

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