mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 19:40:31 +03:00
minimal NMEA support for 4.5 (#13044)
* minimal NMEA * fix a typo thanks Karate
This commit is contained in:
ctzsnooze
GitHub
parent
a35a5af16c
commit
37f119cf4f
1 changed files with 36 additions and 95 deletions
|
|
@ -101,17 +101,15 @@ typedef struct gpsInitData_s {
|
||||||
uint8_t index;
|
uint8_t index;
|
||||||
uint8_t baudrateIndex; // see baudRate_e
|
uint8_t baudrateIndex; // see baudRate_e
|
||||||
const char *ubx;
|
const char *ubx;
|
||||||
const char *mtk;
|
|
||||||
} gpsInitData_t;
|
} gpsInitData_t;
|
||||||
|
|
||||||
// NMEA will cycle through these until valid data is received
|
// UBX will cycle through these until valid data is received
|
||||||
static const gpsInitData_t gpsInitData[] = {
|
static const gpsInitData_t gpsInitData[] = {
|
||||||
{ GPS_BAUDRATE_115200, BAUD_115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
|
{ GPS_BAUDRATE_115200, BAUD_115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n" },
|
||||||
{ GPS_BAUDRATE_57600, BAUD_57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
|
{ GPS_BAUDRATE_57600, BAUD_57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n" },
|
||||||
{ GPS_BAUDRATE_38400, BAUD_38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
|
{ GPS_BAUDRATE_38400, BAUD_38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n" },
|
||||||
{ GPS_BAUDRATE_19200, BAUD_19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
|
{ GPS_BAUDRATE_19200, BAUD_19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n" },
|
||||||
// 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
|
{ GPS_BAUDRATE_9600, BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n" }
|
||||||
{ GPS_BAUDRATE_9600, BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#define GPS_INIT_DATA_ENTRY_COUNT ARRAYLEN(gpsInitData)
|
#define GPS_INIT_DATA_ENTRY_COUNT ARRAYLEN(gpsInitData)
|
||||||
|
|
@ -339,8 +337,6 @@ typedef enum {
|
||||||
|
|
||||||
baudRate_e initBaudRateIndex;
|
baudRate_e initBaudRateIndex;
|
||||||
size_t initBaudRateCycleCount;
|
size_t initBaudRateCycleCount;
|
||||||
static void ubloxSendClassMessage(ubxProtocolBytes_e class_id, ubxProtocolBytes_e msg_id, uint16_t length);
|
|
||||||
|
|
||||||
#endif // USE_GPS_UBLOX
|
#endif // USE_GPS_UBLOX
|
||||||
|
|
||||||
gpsData_t gpsData;
|
gpsData_t gpsData;
|
||||||
|
|
@ -953,111 +949,57 @@ void setSatInfoMessageRate(uint8_t divisor)
|
||||||
#ifdef USE_GPS_NMEA
|
#ifdef USE_GPS_NMEA
|
||||||
void gpsConfigureNmea(void)
|
void gpsConfigureNmea(void)
|
||||||
{
|
{
|
||||||
static bool atgmRestartDone = false;
|
// minimal support for NMEA, we only:
|
||||||
#if !defined(GPS_NMEA_TX_ONLY)
|
// - set the FC's GPS port to the user's configured rate, and
|
||||||
#endif
|
// - send any NMEA custom commands to the GPS Module
|
||||||
|
// the user must configure the power-up baud rate of the module to be fast enough for their data rate
|
||||||
|
// Note: we always parse all incoming NMEA messages
|
||||||
DEBUG_SET(DEBUG_GPS_CONNECTION, 4, (gpsData.state * 100 + gpsData.state_position));
|
DEBUG_SET(DEBUG_GPS_CONNECTION, 4, (gpsData.state * 100 + gpsData.state_position));
|
||||||
|
|
||||||
|
// wait 500ms between changes
|
||||||
|
if (cmp32(gpsData.now, gpsData.state_ts) < 500) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
gpsData.state_ts = gpsData.now;
|
||||||
|
|
||||||
|
// Check that the GPS transmit buffer is empty
|
||||||
|
if (!isSerialTransmitBufferEmpty(gpsPort)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
switch (gpsData.state) {
|
switch (gpsData.state) {
|
||||||
|
|
||||||
case GPS_STATE_DETECT_BAUD:
|
case GPS_STATE_DETECT_BAUD:
|
||||||
#if !defined(GPS_NMEA_TX_ONLY)
|
// no attempt to read the baud rate of the GPS module, or change it
|
||||||
if (cmp32(gpsData.now, gpsData.state_ts) < 1000) {
|
gpsSetState(GPS_STATE_CHANGE_BAUD);
|
||||||
return;
|
|
||||||
}
|
|
||||||
gpsData.state_ts = gpsData.now;
|
|
||||||
switch (gpsData.state_position) {
|
|
||||||
case 0: // first run after bootup
|
|
||||||
serialSetBaudRate(gpsPort, 4800);
|
|
||||||
gpsData.state_position++;
|
|
||||||
break;
|
|
||||||
case 1: // second run
|
|
||||||
// print the init string for the baudrate we want to be at
|
|
||||||
serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
|
|
||||||
gpsData.state_position++;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
// we're now (hopefully) at the correct rate, next state should switch to it
|
|
||||||
// TO DO: Check that this actually works, it seems broken at present
|
|
||||||
gpsData.state_position = 0;
|
|
||||||
gpsSetState(GPS_STATE_CHANGE_BAUD);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
#endif
|
|
||||||
case GPS_STATE_CHANGE_BAUD:
|
case GPS_STATE_CHANGE_BAUD:
|
||||||
#if !defined(GPS_NMEA_TX_ONLY)
|
#if !defined(GPS_NMEA_TX_ONLY)
|
||||||
// wait a short time between sending commands
|
if (gpsData.state_position < 1) {
|
||||||
// note that no commands are sent to request the packets we need
|
// set the FC's baud rate to the user's configured baud rate
|
||||||
if (cmp32(gpsData.now, gpsData.state_ts) < 500) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
gpsData.state_ts = gpsData.now;
|
|
||||||
|
|
||||||
if (gpsData.state_position < 1) { // first run after boot up
|
|
||||||
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]);
|
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]);
|
||||||
gpsData.state_position++;
|
gpsData.state_position++;
|
||||||
} else if (gpsData.state_position < 2) {
|
} else if (gpsData.state_position < 2) {
|
||||||
// *** this message also appears to fail ***//
|
// send NMEA custom commands to select which messages being sent, data rate etc
|
||||||
// NMEA reports back at whatever speed the module is configured to send at, not 5Hz
|
// use PUBX, MTK, SiRF or GTK format commands, depending on module type
|
||||||
serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n"); // set GGA rate to 5Hz
|
|
||||||
gpsData.state_position++;
|
|
||||||
} else if (gpsData.state_position < 3) {
|
|
||||||
// special initialization for NMEA ATGM336 and similar GPS recivers - should be done only once
|
|
||||||
if (!atgmRestartDone) {
|
|
||||||
atgmRestartDone = true;
|
|
||||||
serialPrint(gpsPort, "$PCAS02,100*1E\r\n"); // 10Hz refresh rate
|
|
||||||
serialPrint(gpsPort, "$PCAS10,0*1C\r\n"); // hot restart
|
|
||||||
}
|
|
||||||
gpsData.state_position++;
|
|
||||||
} else if (gpsData.state_position < 4) {
|
|
||||||
#ifdef USE_GPS_UBLOX
|
|
||||||
// try to disable UBlox NAV-PVT and NAV-SAT, using ValSet, to which M9 or above should respond
|
|
||||||
ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_PVT_UART1, 0);
|
|
||||||
ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_UART1, 0);
|
|
||||||
#endif
|
|
||||||
gpsData.state_position++;
|
|
||||||
} else if (gpsData.state_position < 5) {
|
|
||||||
#ifdef USE_GPS_UBLOX
|
|
||||||
// try to disable UBX NAV-PVT, NAV-SAT and NAV_SVINFO, to which M7 and M8 should respond
|
|
||||||
ubloxSetMessageRate(CLASS_NAV, MSG_NAV_PVT, 0);
|
|
||||||
ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SAT, 0);
|
|
||||||
ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SVINFO, 0);
|
|
||||||
#endif
|
|
||||||
gpsData.state_position++;
|
|
||||||
} else if (gpsData.state_position < 6) {
|
|
||||||
if (!(isConfiguratorConnected())) {
|
|
||||||
// disable GSV MESSAGES
|
|
||||||
// *** THIS COMMAND FAILS TO DISABLE GSV MESSAGES WHEN SENT ****
|
|
||||||
// bug?? why??
|
|
||||||
serialPrint(gpsPort, "$PSRF103,03,00,00,01*27\r\n"); // disable GSV (Sat info) messages
|
|
||||||
// *** BUT THE UBLOX EQUIVALENT WORKS ****
|
|
||||||
// so I'll send both for now...
|
|
||||||
ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSV, 0);
|
|
||||||
}
|
|
||||||
gpsData.state_position++;
|
|
||||||
} else if (gpsData.state_position < 7) {
|
|
||||||
// NMEA custom commands after ATGM336 initialization
|
|
||||||
static int commandOffset = 0;
|
static int commandOffset = 0;
|
||||||
const char *commands = gpsConfig()->nmeaCustomCommands;
|
const char *commands = gpsConfig()->nmeaCustomCommands;
|
||||||
const char *cmd = commands + commandOffset;
|
const char *cmd = commands + commandOffset;
|
||||||
|
|
||||||
// skip leading whitespaces and get first command length
|
// skip leading whitespaces and get first command length
|
||||||
int commandLen;
|
int commandLen;
|
||||||
while (*cmd && (commandLen = strcspn(cmd, " \0")) == 0) {
|
while (*cmd && (commandLen = strcspn(cmd, " \0")) == 0) {
|
||||||
cmd++; // skip separators
|
cmd++; // skip separators
|
||||||
}
|
}
|
||||||
|
|
||||||
if (*cmd) {
|
if (*cmd) {
|
||||||
// Send the current command to the GPS
|
// Send the current command to the GPS
|
||||||
serialWriteBuf(gpsPort, (uint8_t *)cmd, commandLen);
|
serialWriteBuf(gpsPort, (uint8_t *)cmd, commandLen);
|
||||||
serialWriteBuf(gpsPort, (uint8_t *)"\r\n", 2);
|
serialWriteBuf(gpsPort, (uint8_t *)"\r\n", 2);
|
||||||
|
|
||||||
// Move to the next command
|
// Move to the next command
|
||||||
cmd += commandLen;
|
cmd += commandLen;
|
||||||
}
|
}
|
||||||
|
|
||||||
// skip trailing whitespaces
|
// skip trailing whitespaces
|
||||||
while (*cmd && strcspn(cmd, " \0") == 0) cmd++;
|
while (*cmd && strcspn(cmd, " \0") == 0) cmd++;
|
||||||
|
|
||||||
if (*cmd) {
|
if (*cmd) {
|
||||||
// more commands to send
|
// more commands to send
|
||||||
commandOffset = cmd - commands;
|
commandOffset = cmd - commands;
|
||||||
|
|
@ -1065,14 +1007,13 @@ void gpsConfigureNmea(void)
|
||||||
gpsData.state_position++;
|
gpsData.state_position++;
|
||||||
commandOffset = 0;
|
commandOffset = 0;
|
||||||
}
|
}
|
||||||
} else
|
gpsData.state_position++;
|
||||||
#else
|
gpsSetState(GPS_STATE_RECEIVING_DATA);
|
||||||
{
|
|
||||||
serialSetBaudRate (gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]);
|
|
||||||
}
|
}
|
||||||
#endif
|
#else // !GPS_NMEA_TX_ONLY
|
||||||
gpsSetState(GPS_STATE_RECEIVING_DATA);
|
gpsSetState(GPS_STATE_RECEIVING_DATA);
|
||||||
break;
|
#endif // !GPS_NMEA_TX_ONLY
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif // USE_GPS_NMEA
|
#endif // USE_GPS_NMEA
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue