mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
Fix Galileo toggling (#13384)
* Fix Galileo toggling * try something else * cleanup * Fix per review from ledvinap * Prevent buffer overflow on invalid numConfigBlocks
This commit is contained in:
parent
79d031b8b6
commit
37d27ad3e3
1 changed files with 25 additions and 28 deletions
|
@ -173,6 +173,14 @@ typedef enum {
|
|||
#define UBLOX_GNSS_ENABLE 0x1
|
||||
#define UBLOX_GNSS_DEFAULT_SIGCFGMASK 0x10000
|
||||
|
||||
#define UBLOX_GNSS_GPS 0x00
|
||||
#define UBLOX_GNSS_SBAS 0x01
|
||||
#define UBLOX_GNSS_GALILEO 0x02
|
||||
#define UBLOX_GNSS_BEIDOU 0x03
|
||||
#define UBLOX_GNSS_IMES 0x04
|
||||
#define UBLOX_GNSS_QZSS 0x05
|
||||
#define UBLOX_GNSS_GLONASS 0x06
|
||||
|
||||
typedef struct ubxHeader_s {
|
||||
uint8_t preamble1;
|
||||
uint8_t preamble2;
|
||||
|
@ -2283,38 +2291,27 @@ static bool UBLOX_parse_gps(void)
|
|||
break;
|
||||
case CLSMSG(CLASS_CFG, MSG_CFG_GNSS):
|
||||
{
|
||||
bool isSBASenabled = false;
|
||||
bool isM8NwithDefaultConfig = false;
|
||||
|
||||
if ((ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks >= 2) &&
|
||||
(ubxRcvMsgPayload.ubxCfgGnss.configblocks[1].gnssId == 1) && //SBAS
|
||||
(ubxRcvMsgPayload.ubxCfgGnss.configblocks[1].flags & UBLOX_GNSS_ENABLE)) { //enabled
|
||||
|
||||
isSBASenabled = true;
|
||||
}
|
||||
|
||||
if ((ubxRcvMsgPayload.ubxCfgGnss.numTrkChHw == 32) && //M8N
|
||||
(ubxRcvMsgPayload.ubxCfgGnss.numTrkChUse == 32) &&
|
||||
(ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks == 7) &&
|
||||
(ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].gnssId == 2) && //Galileo
|
||||
(ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].resTrkCh == 4) && //min channels
|
||||
(ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].maxTrkCh == 8) && //max channels
|
||||
!(ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].flags & UBLOX_GNSS_ENABLE)) { //disabled
|
||||
|
||||
isM8NwithDefaultConfig = true;
|
||||
}
|
||||
|
||||
const uint16_t messageSize = 4 + (ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks * sizeof(ubxConfigblock_t));
|
||||
|
||||
ubxMessage_t tx_buffer;
|
||||
memcpy(&tx_buffer.payload, &ubxRcvMsgPayload, messageSize);
|
||||
|
||||
if (isSBASenabled && (gpsConfig()->sbasMode == SBAS_NONE)) {
|
||||
tx_buffer.payload.cfg_gnss.configblocks[1].flags &= ~UBLOX_GNSS_ENABLE; //Disable SBAS
|
||||
}
|
||||
// prevent buffer overflow on invalid numConfigBlocks
|
||||
const int size = MIN(messageSize, sizeof(tx_buffer.payload));
|
||||
memcpy(&tx_buffer.payload, &ubxRcvMsgPayload, size);
|
||||
|
||||
if (isM8NwithDefaultConfig && gpsConfig()->gps_ublox_use_galileo) {
|
||||
tx_buffer.payload.cfg_gnss.configblocks[2].flags |= UBLOX_GNSS_ENABLE; //Enable Galileo
|
||||
for (int i = 0; i < ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks; i++) {
|
||||
if (ubxRcvMsgPayload.ubxCfgGnss.configblocks[i].gnssId == UBLOX_GNSS_SBAS) {
|
||||
if (gpsConfig()->sbasMode == SBAS_NONE) {
|
||||
tx_buffer.payload.cfg_gnss.configblocks[i].flags &= ~UBLOX_GNSS_ENABLE; // Disable SBAS
|
||||
}
|
||||
}
|
||||
|
||||
if (ubxRcvMsgPayload.ubxCfgGnss.configblocks[i].gnssId == UBLOX_GNSS_GALILEO) {
|
||||
if (gpsConfig()->gps_ublox_use_galileo) {
|
||||
tx_buffer.payload.cfg_gnss.configblocks[i].flags |= UBLOX_GNSS_ENABLE; // Enable Galileo
|
||||
} else {
|
||||
tx_buffer.payload.cfg_gnss.configblocks[i].flags &= ~UBLOX_GNSS_ENABLE; // Disable Galileo
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ubloxSendConfigMessage(&tx_buffer, MSG_CFG_GNSS, messageSize, false);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue