mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
OP-1464 tested version with debug still in place
This commit is contained in:
parent
92295559b0
commit
de521c9004
@ -121,6 +121,7 @@ static bool gpsEnabled = false;
|
||||
static xTaskHandle gpsTaskHandle;
|
||||
|
||||
static char *gps_rx_buffer;
|
||||
static uint16_t gps_rx_buffer_size;
|
||||
|
||||
static uint32_t timeOfLastCommandMs;
|
||||
static uint32_t timeOfLastUpdateMs;
|
||||
@ -220,13 +221,16 @@ int32_t GPSInitialize(void)
|
||||
// if (gpsPort && gpsEnabled) {
|
||||
if (gpsEnabled) {
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
|
||||
gps_rx_buffer_size = (sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH;
|
||||
gps_rx_buffer = pios_malloc(gps_rx_buffer_size);
|
||||
#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
|
||||
gps_rx_buffer_size = sizeof(struct UBXPacket);
|
||||
gps_rx_buffer = pios_malloc(gps_rx_buffer_size);
|
||||
#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
|
||||
gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
|
||||
gps_rx_buffer_size = NMEA_MAX_PACKET_LENGTH;
|
||||
gps_rx_buffer = pios_malloc(gps_rx_buffer_size);
|
||||
#else
|
||||
gps_rx_buffer = NULL;
|
||||
gps_rx_buffer = NULL;
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
PIOS_Assert(gps_rx_buffer);
|
||||
@ -250,7 +254,14 @@ MODULE_INITCALL(GPSInitialize, GPSStart);
|
||||
|
||||
static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
portTickType xDelay = 100 / portTICK_RATE_MS;
|
||||
// 57600 baud = 5760 bytes per second
|
||||
// PIOS serial buffers appear to be 32 bytes long
|
||||
// that is a maximum of 5760/32 = 180 buffers per second
|
||||
// that is 1/180 = 0.005555556 seconds per packet
|
||||
// we must never wait more than 5ms since packet was last drained or it may overflow
|
||||
// 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible
|
||||
// portTickType xDelay = 100 / portTICK_RATE_MS;
|
||||
portTickType xDelay = 5 / portTICK_RATE_MS;
|
||||
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
|
||||
#ifdef PIOS_GPS_SETS_HOMELOCATION
|
||||
@ -274,12 +285,12 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
PERF_INIT_COUNTER(counterParse, 0x97510003);
|
||||
// uint8_t c[GPS_READ_BUFFER];
|
||||
uint8_t c[GPS_READ_BUFFER+8];
|
||||
uint16_t bytes_remaining = 0;
|
||||
|
||||
// Loop forever
|
||||
while (1) {
|
||||
if (gpsPort) {
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
// do autoconfig stuff for UBX GPS's
|
||||
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
|
||||
char *buffer = 0;
|
||||
uint16_t count = 0;
|
||||
@ -297,7 +308,7 @@ if (gpsPort) {
|
||||
}
|
||||
}
|
||||
|
||||
// need to do this whether there is received data or not, or the status (e.g. gcs) is not always correct
|
||||
// need to do this whether there is received data or not, or the status (e.g. gcs) will not always be correct
|
||||
int32_t ac_status = ubx_autoconfig_get_status();
|
||||
static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
|
||||
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
|
||||
@ -314,11 +325,9 @@ if (gpsPort) {
|
||||
}
|
||||
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
|
||||
|
||||
// This blocks the task until there is something on the buffer (or 100ms? passes)
|
||||
uint16_t cnt;
|
||||
uint16_t bytes_used;
|
||||
cnt = PIOS_COM_ReceiveBuffer(gpsPort, &c[bytes_remaining], GPS_READ_BUFFER-bytes_remaining, xDelay);
|
||||
// cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
|
||||
// This blocks the task until there is something on the buffer (or 100ms? passes)
|
||||
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
|
||||
if (cnt > 0) {
|
||||
PERF_TIMED_SECTION_START(counterParse);
|
||||
PERF_TRACK_VALUE(counterBytesIn, cnt);
|
||||
@ -332,21 +341,15 @@ if (gpsPort) {
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
case GPSSETTINGS_DATAPROTOCOL_UBX:
|
||||
cnt += bytes_remaining;
|
||||
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats, &bytes_used);
|
||||
// res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats, &bytes_used);
|
||||
#if 1
|
||||
bytes_remaining = cnt - bytes_used;
|
||||
memmove(c, &c[bytes_used], bytes_remaining);
|
||||
#endif
|
||||
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
res = NO_PARSER; // this should not happen
|
||||
break;
|
||||
}
|
||||
|
||||
PERF_TIMED_SECTION_END(counterParse);
|
||||
|
||||
if (res == PARSER_COMPLETE) {
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
timeOfLastUpdateMs = timeNowMs;
|
||||
@ -504,12 +507,19 @@ void debugindex(int index, uint8_t c)
|
||||
// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
|
||||
void gps_set_fc_baud_from_arg(uint8_t baud, __attribute__((unused)) uint8_t addit)
|
||||
{
|
||||
static uint8_t previous_baud=255;
|
||||
static uint8_t mutex; // = 0
|
||||
|
||||
// do we need this?
|
||||
if (__sync_fetch_and_add(&mutex, 1) == 0) {
|
||||
// don't bother doing the baud change if it is actually the same
|
||||
// might drop less data
|
||||
if (previous_baud != baud) {
|
||||
previous_baud = baud;
|
||||
// Set Revo port hwsettings_baud
|
||||
PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud));
|
||||
GPSPositionSensorCurrentBaudRateSet(&baud);
|
||||
}
|
||||
} else {
|
||||
static uint8_t c;
|
||||
debugindex(9, ++c);
|
||||
@ -517,6 +527,7 @@ void gps_set_fc_baud_from_arg(uint8_t baud, __attribute__((unused)) uint8_t addi
|
||||
}
|
||||
--mutex;
|
||||
|
||||
|
||||
debugindex(6, baud);
|
||||
{
|
||||
static uint8_t c;
|
||||
@ -583,15 +594,15 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
|
||||
// - the user can use the same GPS with both an older release of OP (e.g. GPS settings at 38400) and the current release (autoconfig-nostore)
|
||||
// - the 57600 could be fixed instead of the 9600 as part of autoconfig-store/nostore and the HwSettings.GPSSpeed be the initial baud rate?
|
||||
|
||||
// About using UBlox GPS's with default settings (9600 baud and NEMA data):
|
||||
// - the default uBlox settings (different than OP settings) are NEMA and 9600 baud
|
||||
// About using UBlox GPS's with default settings (9600 baud and NMEA data):
|
||||
// - the default uBlox settings (different than OP settings) are NMEA and 9600 baud
|
||||
// - - and that is OK and you use autoconfig-nostore
|
||||
// - - I personally feel that this is the best way to set it up because if OP dev changes GPS settings,
|
||||
// - - you get them automatically changed to match whatever version of OP code you are running
|
||||
// - - but 9600 is only OK for this autoconfig startup
|
||||
// - by my testing, the 9600 initial to 57600 final baud startup takes:
|
||||
// - - 0:10 if the GPS has been configured to send OP data at 9600
|
||||
// - - 0:06 if the GPS has default data settings (NEMA) at 9600
|
||||
// - - 0:06 if the GPS has default data settings (NMEA) at 9600
|
||||
// - - reminder that even 0:10 isn't that bad. You need to wait for the GPS to acquire satellites,
|
||||
|
||||
// Some things you want to think about if you want to play around with this:
|
||||
@ -608,10 +619,10 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
|
||||
// - - so you could use autoconfig.nostore with this high baud rate, but not autoconfig.store (once) followed by autoconfig.disabled
|
||||
// - - I haven't tested other GPS's in regard to this
|
||||
|
||||
// since 9600 baud and lower are not usable, and are best left at NEMA, I could have coded it to do a factory reset
|
||||
// since 9600 baud and lower are not usable, and are best left at NMEA, I could have coded it to do a factory reset
|
||||
// - if set to 9600 baud (or lower)
|
||||
|
||||
// if GPS (UBX or NEMA) is enabled at all
|
||||
// if GPS (UBX or NMEA) is enabled at all
|
||||
static uint32_t previousGpsPort=0xf0f0f0f0; // = 0 means deconfigured and at power up it is deconfigured
|
||||
if (gpsPort && gpsEnabled) {
|
||||
//we didn't have a gpsPort when we booted, and disabled did not find GPS's
|
||||
@ -817,11 +828,6 @@ the ubx parser doesn't handle partially received buffers
|
||||
HwSettingsGPSSpeedGet(&speed);
|
||||
// set fc baud
|
||||
gps_set_fc_baud_from_arg(speed, 1);
|
||||
debugindex(2, speed);
|
||||
{
|
||||
static uint8_t c;
|
||||
debugindex(3, ++c);
|
||||
}
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
// even changing the baud rate will make it re-verify the sensor type
|
||||
@ -863,9 +869,9 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
|
||||
newconfig.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED;
|
||||
}
|
||||
#else
|
||||
// it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running NEMA
|
||||
// it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running NMEA
|
||||
// because ubx auto config never gets called
|
||||
// setting it up completely means that if we switch from initial NEMA to UBX or disabled to enabled, that it will start normally
|
||||
// setting it up completely means that if we switch from initial NMEA to UBX or disabled to enabled, that it will start normally
|
||||
newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig;
|
||||
#endif
|
||||
|
||||
|
@ -111,74 +111,114 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition
|
||||
static uint8_t rx_count = 0;
|
||||
static bool start_flag = false;
|
||||
static bool found_cr = false;
|
||||
bool goodParse = false;
|
||||
uint8_t c;
|
||||
int i = 0;
|
||||
|
||||
for (int i = 0; i < len; i++) {
|
||||
c = rx[i];
|
||||
while (i < len) {
|
||||
c = rx[i++];
|
||||
// detect start while acquiring stream
|
||||
if (!start_flag && (c == '$')) { // NMEA identifier found
|
||||
start_flag = true;
|
||||
found_cr = false;
|
||||
rx_count = 0;
|
||||
} else if (!start_flag) {
|
||||
return PARSER_ERROR;
|
||||
// if we find a $ in the middle it was a bad packet (e.g. maybe UBX binary),
|
||||
// and this may be the start of another packet
|
||||
// silently cancel the current sentence
|
||||
if (c == '$') { // NMEA identifier found
|
||||
start_flag = false;
|
||||
}
|
||||
if (!start_flag) { // if no NMEA identifier ('$') found yet
|
||||
if (c == '$') { // NMEA identifier found
|
||||
start_flag = true;
|
||||
found_cr = false;
|
||||
rx_count = 0;
|
||||
} else {
|
||||
// find a likely candidate for a NMEA string
|
||||
// skip over some e.g. uBlox packets
|
||||
uint8_t *p;
|
||||
p = memchr(&rx[i], '$', len-i);
|
||||
if (p) {
|
||||
i += p - &rx[i];
|
||||
} else {
|
||||
i = len;
|
||||
}
|
||||
// if no more data, we can return an error
|
||||
ret = PARSER_ERROR;
|
||||
// loop to restart at the $ if there is one
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (rx_count >= NMEA_MAX_PACKET_LENGTH) {
|
||||
// The buffer is already full and we haven't found a valid NMEA sentence.
|
||||
// Flush the buffer and note the overflow event.
|
||||
gpsRxStats->gpsRxOverflow++;
|
||||
start_flag = false;
|
||||
found_cr = false;
|
||||
rx_count = 0;
|
||||
ret = PARSER_OVERRUN;
|
||||
continue;
|
||||
} else {
|
||||
gps_rx_buffer[rx_count] = c;
|
||||
rx_count++;
|
||||
#if 1
|
||||
gps_rx_buffer[rx_count++] = c;
|
||||
#else
|
||||
uint8_t *p;
|
||||
i-=1;
|
||||
p = memchr(&rx[i], '\r', len-i);
|
||||
if (p) {
|
||||
copylen = p - &rx[i];
|
||||
if (rx_count+copylen >= NMEA_MAX_PACKET_LENGTH) {
|
||||
memcpy(&gps_rx_buffer[rx_count], &rx[i], len-i);
|
||||
}
|
||||
found_cr = true;
|
||||
#endif
|
||||
}
|
||||
|
||||
// look for ending '\r\n' sequence
|
||||
if (!found_cr && (c == '\r')) {
|
||||
found_cr = true;
|
||||
} else if (found_cr && (c != '\n')) {
|
||||
found_cr = false; // false end flag
|
||||
} else if (found_cr && (c == '\n')) {
|
||||
// The NMEA functions require a zero-terminated string
|
||||
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
|
||||
gps_rx_buffer[rx_count - 2] = 0;
|
||||
} else if (found_cr) {
|
||||
if (c != '\n') {
|
||||
found_cr = false; // false end flag
|
||||
} else {
|
||||
// The NMEA functions require a zero-terminated string
|
||||
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
|
||||
gps_rx_buffer[rx_count - 2] = 0;
|
||||
|
||||
// prepare to parse next sentence
|
||||
start_flag = false;
|
||||
found_cr = false;
|
||||
rx_count = 0;
|
||||
// Our rxBuffer must look like this now:
|
||||
// [0] = '$'
|
||||
// ... = zero or more bytes of sentence payload
|
||||
// [end_pos - 1] = '\r'
|
||||
// [end_pos] = '\n'
|
||||
//
|
||||
// Prepare to consume the sentence from the buffer
|
||||
// prepare to parse next sentence
|
||||
start_flag = false;
|
||||
// Our rxBuffer must look like this now:
|
||||
// [0] = '$'
|
||||
// ... = zero or more bytes of sentence payload
|
||||
// [end_pos - 1] = '\r'
|
||||
// [end_pos] = '\n'
|
||||
//
|
||||
// Prepare to consume the sentence from the buffer
|
||||
|
||||
// Validate the checksum over the sentence
|
||||
if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx.
|
||||
// PIOS_DEBUG_PinHigh(2);
|
||||
gpsRxStats->gpsRxChkSumError++;
|
||||
// PIOS_DEBUG_PinLow(2);
|
||||
ret = PARSER_ERROR;
|
||||
} else { // Valid checksum, use this packet to update the GPS position
|
||||
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
|
||||
// Validate the checksum over the sentence
|
||||
if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx.
|
||||
// PIOS_DEBUG_PinHigh(2);
|
||||
gpsRxStats->gpsRxParserError++;
|
||||
gpsRxStats->gpsRxChkSumError++;
|
||||
// PIOS_DEBUG_PinLow(2);
|
||||
} else {
|
||||
gpsRxStats->gpsRxReceived++;
|
||||
};
|
||||
|
||||
ret = PARSER_COMPLETE;
|
||||
ret = PARSER_ERROR;
|
||||
} else { // Valid checksum, use this packet to update the GPS position
|
||||
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
|
||||
// PIOS_DEBUG_PinHigh(2);
|
||||
gpsRxStats->gpsRxParserError++;
|
||||
// PIOS_DEBUG_PinLow(2);
|
||||
ret = PARSER_ERROR;
|
||||
} else {
|
||||
gpsRxStats->gpsRxReceived++;
|
||||
goodParse = true;
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
|
||||
if (goodParse) {
|
||||
// if so much as one good sentence we return a good status so the connection status says "alive"
|
||||
// if we didn't do this, a lot of garbage (e.g. UBX protocol) mixed in with enough NMEA to fly
|
||||
// might think the GPS was offline
|
||||
return PARSER_COMPLETE;
|
||||
} else {
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
|
||||
@ -429,6 +469,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
|
||||
DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
|
||||
#endif
|
||||
if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
|
||||
// leave my new field alone!
|
||||
GPSPositionSensorCurrentBaudRateGet(&GpsData->CurrentBaudRate);
|
||||
GPSPositionSensorSet(GpsData);
|
||||
}
|
||||
return false;
|
||||
@ -440,6 +482,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
|
||||
#ifdef DEBUG_MSGID_IN
|
||||
DEBUG_MSG("U");
|
||||
#endif
|
||||
// leave my new field alone!
|
||||
GPSPositionSensorCurrentBaudRateGet(&GpsData->CurrentBaudRate);
|
||||
GPSPositionSensorSet(GpsData);
|
||||
}
|
||||
|
||||
|
@ -107,7 +107,7 @@ struct UBX_ACK_NAK ubxLastNak;
|
||||
#define UBX_PVT_TIMEOUT (1000)
|
||||
// parse incoming character stream for messages in UBX binary format
|
||||
|
||||
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats, __attribute__((unused)) uint16_t *bytes_used)
|
||||
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
|
||||
{
|
||||
int ret = PARSER_INCOMPLETE; // message not (yet) complete
|
||||
enum proto_states {
|
||||
@ -126,15 +126,10 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
|
||||
static enum proto_states proto_state = START;
|
||||
static uint16_t rx_count = 0;
|
||||
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
|
||||
|
||||
#if 0
|
||||
for (int i = 0; i < len; i++) {
|
||||
c = rx[i];
|
||||
#else
|
||||
int i = 0;
|
||||
|
||||
while (i < len) {
|
||||
c = rx[i++];
|
||||
#endif
|
||||
switch (proto_state) {
|
||||
case START: // detect protocol
|
||||
if (c == UBX_SYNC1) { // first UBX sync char found
|
||||
@ -177,12 +172,6 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
|
||||
proto_state = UBX_CHK1;
|
||||
}
|
||||
}
|
||||
#if 0 // will never happen
|
||||
else {
|
||||
gpsRxStats->gpsRxOverflow++;
|
||||
proto_state = START;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case UBX_CHK1:
|
||||
ubx->header.ck_a = c;
|
||||
@ -198,7 +187,8 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
|
||||
proto_state = START;
|
||||
}
|
||||
break;
|
||||
default: break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (proto_state == START) {
|
||||
@ -207,10 +197,9 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
|
||||
gpsRxStats->gpsRxReceived++;
|
||||
proto_state = START;
|
||||
ret = PARSER_COMPLETE; // message complete & processed
|
||||
break;
|
||||
}
|
||||
}
|
||||
*bytes_used = i;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -539,6 +528,8 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
|
||||
GpsPosition->SensorType = sensorType;
|
||||
|
||||
if (msgtracker.msg_received == ALL_RECEIVED) {
|
||||
// leave my new field alone!
|
||||
GPSPositionSensorCurrentBaudRateGet(&GpsPosition->CurrentBaudRate);
|
||||
GPSPositionSensorSet(GpsPosition);
|
||||
msgtracker.msg_received = NONE_RECEIVED;
|
||||
id = GPSPOSITIONSENSOR_OBJID;
|
||||
|
@ -619,7 +619,7 @@ extern struct UBX_ACK_NAK ubxLastNak;
|
||||
bool checksum_ubx_message(struct UBXPacket *);
|
||||
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
|
||||
|
||||
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *, uint16_t *bytes_used);
|
||||
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
|
||||
void load_mag_settings();
|
||||
|
||||
#endif /* UBX_H */
|
||||
|
@ -49,6 +49,26 @@
|
||||
// later versions dropped this and drop data when the send buffer is full and that could be even longer
|
||||
// rather than have long timeouts, we will let timeouts * retries handle that if it happens
|
||||
|
||||
// known to work
|
||||
#if 0
|
||||
// timeout for ack reception
|
||||
#define UBX_REPLY_TIMEOUT (400 * 1000) 50 has 1 retry 100 has 1 retry 0 retries at 400, 4@200-200@19200 1@400-200@115200-onetime 1@400@19200
|
||||
// timeout for a settings save, in case it has to erase flash
|
||||
#define UBX_SAVE_WAIT_TIME (1000 * 1000)
|
||||
// max retries in case of timeout
|
||||
#define UBX_MAX_RETRIES 5
|
||||
// pause between each verifiably correct configuration step
|
||||
#define UBX_VERIFIED_STEP_WAIT_TIME (20 * 1000)
|
||||
// pause between each unverifiably correct configuration step
|
||||
#define UBX_UNVERIFIED_STEP_WAIT_TIME (400 * 1000) was 200
|
||||
#endif
|
||||
|
||||
// need about 60 + 8 char times for the largest config packet to be sent and acked
|
||||
// that is about 35ms at 19200 baud, figure 40ms
|
||||
// but then there is the hairy problem of the dropped packets that occur
|
||||
// when there is more data to send than there is bandwidth
|
||||
// we generally avoid turning on the OP data packets for configurations of 9600 baud or lower
|
||||
|
||||
#if 0
|
||||
// timeout for ack reception
|
||||
#define UBX_REPLY_TIMEOUT (500 * 1000)
|
||||
@ -61,16 +81,18 @@
|
||||
// pause between each unverifiably correct configuration step
|
||||
#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000)
|
||||
#else
|
||||
// timeout for ack reception
|
||||
#define UBX_REPLY_TIMEOUT (2000 * 1000)
|
||||
// timeout for a settings save, in case it has to erase flash
|
||||
#define UBX_REPLY_TO_SAVE_TIMEOUT (3000 * 1000)
|
||||
// timeouts for ack reception
|
||||
// 1@265-265@19200
|
||||
// 0@272-272
|
||||
#define UBX_REPLY_TIMEOUT (280 * 1000)
|
||||
// timeout for a settings save, in case it has to erase flash?
|
||||
#define UBX_SAVE_WAIT_TIME (1000 * 1000)
|
||||
// max retries in case of timeout
|
||||
#define UBX_MAX_RETRIES 5
|
||||
// pause between each verifiably correct configuration step
|
||||
#define UBX_VERIFIED_STEP_WAIT_TIME (1000 * 1000)
|
||||
// pause between each unverifiably correct configuration step
|
||||
#define UBX_UNVERIFIED_STEP_WAIT_TIME (2000 * 1000)
|
||||
// max time for ubx parser to respond to MON_VER
|
||||
#define UBX_PARSER_TIMEOUT (950 * 1000)
|
||||
// pause between each unverifiable (no ack/nak) configuration step
|
||||
#define UBX_UNVERIFIED_STEP_WAIT_TIME (280 * 1000)
|
||||
#endif
|
||||
#define UBX_CFG_CFG_OP_STORE_SETTINGS \
|
||||
(UBX_CFG_CFG_SETTINGS_IOPORT | \
|
||||
|
@ -51,7 +51,9 @@ typedef enum {
|
||||
INIT_STEP_CONFIGURE_WAIT_ACK,
|
||||
INIT_STEP_SAVE,
|
||||
INIT_STEP_SAVE_WAIT_ACK,
|
||||
INIT_STEP_PRE_DONE,
|
||||
INIT_STEP_DONE,
|
||||
INIT_STEP_PRE_ERROR,
|
||||
INIT_STEP_ERROR
|
||||
} initSteps_t;
|
||||
|
||||
@ -61,6 +63,8 @@ typedef struct {
|
||||
uint32_t lastStepTimestampRaw; // timestamp of last operation
|
||||
uint32_t lastConnectedRaw; // timestamp of last time gps was connected
|
||||
struct {
|
||||
union {
|
||||
struct {
|
||||
UBXSentPacket_t working_packet; // outbound "buffer"
|
||||
// bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
|
||||
// failure mode otherwise:
|
||||
@ -68,7 +72,17 @@ typedef struct {
|
||||
// - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly)
|
||||
// - change Revo port baud rate
|
||||
// sometimes fails (much worse for lowest baud rates)
|
||||
// FIXME: remove this and retest
|
||||
// FIXME: remove this and retest
|
||||
// FIXME: remove this and retest
|
||||
// FIXME: remove this and retest
|
||||
// FIXME: remove this and retest
|
||||
// FIXME: remove this and retest
|
||||
// FIXME: remove this and retest
|
||||
uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
|
||||
} __attribute__((packed));
|
||||
GPSSettingsData gpsSettings;
|
||||
} __attribute__((packed));
|
||||
} __attribute__((packed));
|
||||
volatile ubx_autoconfig_settings_t currentSettings;
|
||||
int8_t lastConfigSent; // index of last configuration string sent
|
||||
@ -219,11 +233,14 @@ void gps_ubx_reset_sensor_type()
|
||||
{
|
||||
static uint8_t mutex; // = 0
|
||||
|
||||
// is this needed?
|
||||
if (__sync_fetch_and_add(&mutex, 1) == 0) {
|
||||
ubxHwVersion = -1;
|
||||
baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful
|
||||
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
|
||||
GPSPositionSensorSensorTypeSet(&sensorType);
|
||||
// make the sensor type / autobaud code time out immediately to send the request immediately
|
||||
status->lastStepTimestampRaw += 0x8000000UL;
|
||||
} else {
|
||||
static uint8_t c;
|
||||
debugindex(8, ++c);
|
||||
@ -259,7 +276,7 @@ static void config_gps_baud(uint16_t *bytes_to_send)
|
||||
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_prt_t));
|
||||
status->working_packet.message.payload.cfg_prt.mode = UBX_CFG_PRT_MODE_DEFAULT; // 8databits, 1stopbit, noparity, and non-zero reserved
|
||||
status->working_packet.message.payload.cfg_prt.portID = 1; // 1 = UART1, 2 = UART2
|
||||
// for protocol masks, bit 0 is UBX enable, bit 1 is NEMA enable
|
||||
// for protocol masks, bit 0 is UBX enable, bit 1 is NMEA enable
|
||||
status->working_packet.message.payload.cfg_prt.inProtoMask = 1; // 1 = UBX only (bit 0)
|
||||
// disable current UBX messages for low baud rates
|
||||
// status->working_packet.message.payload.cfg_prt.outProtoMask = (hwsettings_baud<=HWSETTINGS_GPSSPEED_9600) ? 0 : 1; // 1 = UBX only (bit 0)
|
||||
@ -445,6 +462,19 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
|
||||
}
|
||||
|
||||
|
||||
// permanently store our version of GPSSettings.UbxAutoConfig
|
||||
// we use this to disable after ConfigStoreAndDisable is complete
|
||||
static void setGpsSettings()
|
||||
{
|
||||
// try casting it correctly and it says:
|
||||
// expected 'struct GPSSettingsData *' but argument is of type 'struct GPSSettingsData *'
|
||||
// probably a volatile or align issue
|
||||
GPSSettingsGet((void *) &status->gpsSettings);
|
||||
status->gpsSettings.UbxAutoConfig = status->currentSettings.UbxAutoConfig;
|
||||
GPSSettingsSet((void *) &status->gpsSettings);
|
||||
}
|
||||
|
||||
|
||||
// End User Documentation
|
||||
|
||||
// There are two baud rates of interest
|
||||
@ -553,11 +583,11 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
|
||||
// it will use HwSettings.GPSSpeed for the baud rate and not do any configuration changes
|
||||
// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE it will
|
||||
// 1 Reset the permanent configuration back to factory default
|
||||
// 2 Disable NEMA message settings
|
||||
// 2 Disable NMEA message settings
|
||||
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
|
||||
// 4 Save the current volatile settings to non-volatile storage
|
||||
// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGURE it will
|
||||
// 2 Disable NEMA message settings
|
||||
// 2 Disable NMEA message settings
|
||||
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
|
||||
// If the requested baud rate is 9600 or less it skips the step (3) of adding some volatile UBX settings
|
||||
|
||||
@ -592,10 +622,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
if (!status) {
|
||||
return;
|
||||
}
|
||||
// smallest delay between each step
|
||||
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_VERIFIED_STEP_WAIT_TIME) {
|
||||
return;
|
||||
}
|
||||
|
||||
// get UBX version whether autoconfig is enabled or not
|
||||
// this allows the user to try some baud rates and visibly see when it works
|
||||
// ubxHwVersion is a global set externally by the caller of this function
|
||||
@ -610,7 +637,11 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
// send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped
|
||||
|
||||
// wait for the normal reply timeout before sending it over and over
|
||||
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT) {
|
||||
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_PARSER_TIMEOUT) {
|
||||
// 1 sec seems to work, except for just 230400 baud rates and then it takes 4 times
|
||||
// retries for various timeouts
|
||||
// 925-280-280 -> 2@2400
|
||||
// 937-280-280 -> 1@2400
|
||||
return;
|
||||
}
|
||||
|
||||
@ -640,65 +671,79 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
// if (status->currentSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED)
|
||||
// this is the same thing
|
||||
// if (enabled)
|
||||
{
|
||||
uint8_t baud_to_try;
|
||||
//HwSettingsGPSSpeedOptions baud_to_try;
|
||||
#if 0
|
||||
static uint8_t baud_array[] = {
|
||||
HWSETTINGS_GPSSPEED_57600,
|
||||
HWSETTINGS_GPSSPEED_9600,
|
||||
HWSETTINGS_GPSSPEED_115200,
|
||||
HWSETTINGS_GPSSPEED_38400,
|
||||
HWSETTINGS_GPSSPEED_19200,
|
||||
HWSETTINGS_GPSSPEED_230400,
|
||||
HWSETTINGS_GPSSPEED_4800,
|
||||
HWSETTINGS_GPSSPEED_2400
|
||||
};
|
||||
|
||||
do {
|
||||
if (baud_to_try_index >= sizeof(baud_array)/sizeof(baud_array[0])) {
|
||||
HwSettingsGPSSpeedGet(&hwsettings_baud);
|
||||
baud_to_try = hwsettings_baud;
|
||||
baud_to_try_index = 0;
|
||||
break;
|
||||
} else {
|
||||
baud_to_try = baud_array[baud_to_try_index++];
|
||||
}
|
||||
} while (baud_to_try == hwsettings_baud);
|
||||
// if not Disabled, do AutoBaud
|
||||
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) {
|
||||
uint8_t baud_to_try;
|
||||
//HwSettingsGPSSpeedOptions baud_to_try;
|
||||
#if 1
|
||||
static uint8_t baud_array[] = {
|
||||
HWSETTINGS_GPSSPEED_57600,
|
||||
HWSETTINGS_GPSSPEED_9600,
|
||||
HWSETTINGS_GPSSPEED_115200,
|
||||
HWSETTINGS_GPSSPEED_38400,
|
||||
HWSETTINGS_GPSSPEED_19200,
|
||||
HWSETTINGS_GPSSPEED_230400,
|
||||
HWSETTINGS_GPSSPEED_4800,
|
||||
HWSETTINGS_GPSSPEED_2400
|
||||
};
|
||||
|
||||
do {
|
||||
// index is inited to be out of bounds, which is interpreted as "currently defined baud rate" (= HwSettigns.GPSSpeed)
|
||||
if (baud_to_try_index >= sizeof(baud_array)/sizeof(baud_array[0])) {
|
||||
HwSettingsGPSSpeedGet(&hwsettings_baud);
|
||||
baud_to_try = hwsettings_baud;
|
||||
baud_to_try_index = 0;
|
||||
break;
|
||||
} else {
|
||||
baud_to_try = baud_array[baud_to_try_index++];
|
||||
}
|
||||
// skip HwSettings.GPSSpeed when you run across it in the list
|
||||
} while (baud_to_try == hwsettings_baud);
|
||||
#else
|
||||
HwSettingsGPSSpeedGet(&hwsettings_baud);
|
||||
baud_to_try = hwsettings_baud;
|
||||
// no autobaud, just use current setting
|
||||
HwSettingsGPSSpeedGet(&hwsettings_baud);
|
||||
baud_to_try = hwsettings_baud;
|
||||
#if 0
|
||||
debugindex(0, hwsettings_baud);
|
||||
{
|
||||
static uint8_t c;
|
||||
debugindex(1, ++c);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
// set the FC (Revo) baud rate
|
||||
gps_set_fc_baud_from_arg(baud_to_try, 2);
|
||||
}
|
||||
|
||||
// set the Revo baud rate
|
||||
gps_set_fc_baud_from_arg(baud_to_try, 2);
|
||||
{
|
||||
static uint8_t c;
|
||||
debugindex(4, ++c);
|
||||
}
|
||||
|
||||
// this code is executed even if ubxautoconfig is disabled
|
||||
// it detects the "sensor type" = type of GPS
|
||||
// the user can use this to manually determine if the baud rate is correct
|
||||
build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
|
||||
// keep timeouts running properly, we (will have) just sent a packet that generates a reply
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
return;
|
||||
}
|
||||
|
||||
#if 0
|
||||
{
|
||||
static uint8_t c;
|
||||
uint8_t baud;
|
||||
GPSPositionSensorCurrentBaudRateOptions trythis;
|
||||
trythis=GPSPOSITIONSENSOR_CURRENTBAUDRATE_9600;
|
||||
GPSPositionSensorCurrentBaudRateGet(&baud);
|
||||
debugindex(8, baud);
|
||||
if (baud != hwsettings_baud) {
|
||||
debugindex(9, ++c);
|
||||
// big hack to keep the malfunctioning GPSPositionSensor.CurrentBaudRate correct
|
||||
// for some reason it keeps going back to boot value (or default if xml has a default)
|
||||
GPSPositionSensorCurrentBaudRateSet(&trythis);
|
||||
GPSPositionSensorCurrentBaudRateSet(&baud);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (!enabled) {
|
||||
// keep resetting the timeouts here if we are not actually going to run the configure code
|
||||
@ -707,15 +752,7 @@ gps_set_fc_baud_from_arg(baud_to_try, 2);
|
||||
return; // autoconfig not enabled
|
||||
}
|
||||
|
||||
// replaying constantly could wear the settings memory out
|
||||
// don't allow constant reconfiging when offline
|
||||
// don't even allow program bugs that could constantly toggle between connected and disconnected to cause configuring
|
||||
if (status->currentStep == INIT_STEP_DONE || status->currentStep == INIT_STEP_ERROR) {
|
||||
//do we want this here
|
||||
// status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
return;
|
||||
}
|
||||
|
||||
debugindex(5, status->currentStep);
|
||||
switch (status->currentStep) {
|
||||
#if 0
|
||||
// if here, we have just verified that the baud rates are in sync
|
||||
@ -744,7 +781,8 @@ gps_set_fc_baud_from_arg(baud_to_try, 2);
|
||||
case INIT_STEP_START:
|
||||
// we should look for the GPS version again (user may plug in a different GPS and then do autoconfig again)
|
||||
// zero retries for the next state that needs it (INIT_STEP_SAVE)
|
||||
status->retryCount = 0;
|
||||
// not needed if MON_VER ack is a nop
|
||||
// status->retryCount = 0;
|
||||
set_current_step_if_untouched(INIT_STEP_SEND_MON_VER);
|
||||
// fall through to next state
|
||||
// we can do that if we choose because we haven't sent any data in this state
|
||||
@ -754,6 +792,10 @@ gps_set_fc_baud_from_arg(baud_to_try, 2);
|
||||
build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
|
||||
// keep timeouts running properly, we (will have) just sent a packet that generates a reply
|
||||
set_current_step_if_untouched(INIT_STEP_WAIT_MON_VER_ACK);
|
||||
// just for test, remove
|
||||
// it messes configure up, config has baud synced, and HwSettings.GPSSpeed is already set with new baud,
|
||||
// so it configures to new baud before GPS has new baud
|
||||
// gps_ubx_reset_sensor_type();
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
break;
|
||||
|
||||
@ -780,7 +822,7 @@ debugindex(5, ubxLastNak.msgID);
|
||||
status->retryCount++;
|
||||
if (status->retryCount > UBX_MAX_RETRIES) {
|
||||
// give up on the retries
|
||||
set_current_step_if_untouched(INIT_STEP_ERROR);
|
||||
set_current_step_if_untouched(INIT_STEP_PRE_ERROR);
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
} else {
|
||||
// retry a few times
|
||||
@ -823,16 +865,16 @@ debugindex(3, ++c);
|
||||
#if !defined(ALWAYS_RESET)
|
||||
// ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore
|
||||
// but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore
|
||||
// if user requests a low baud rate then we just reset and leave it set to NEMA
|
||||
// if user requests a low baud rate then we just reset and leave it set to NMEA
|
||||
// because low baud and high OP data rate doesn't play nice
|
||||
// if user requests that settings be saved, we will reset here too
|
||||
// that makes sure that all strange settings are reset to factory default
|
||||
// else these strange settings may persist because we don't reset all settings by table
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE
|
||||
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE)
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_ABCONFIGANDSTORE
|
||||
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_ABCONFIGSTOREANDDISABLE)
|
||||
#endif
|
||||
{
|
||||
// reset all GPS parameters to factory default (configure low rate NEMA for low baud rates)
|
||||
// reset all GPS parameters to factory default (configure low rate NMEA for low baud rates)
|
||||
// this is not usable by OP code for either baud rate or types of messages sent
|
||||
// but it starts up very quickly for use with autoconfig-nostore (which sets a high baud and enables all the necessary messages)
|
||||
config_reset(bytes_to_send);
|
||||
@ -845,13 +887,13 @@ debugindex(3, ++c);
|
||||
// GPS was just reset, so GPS is running 9600 baud, and Revo is running whatever baud it was before
|
||||
case INIT_STEP_REVO_9600_BAUD:
|
||||
#if !defined(ALWAYS_RESET)
|
||||
// if user requests a low baud rate then we just reset and leave it set to NEMA
|
||||
// if user requests a low baud rate then we just reset and leave it set to NMEA
|
||||
// because low baud and high OP data rate doesn't play nice
|
||||
// if user requests that settings be saved, we will reset here too
|
||||
// that makes sure that all strange settings are reset to factory default
|
||||
// else these strange settings may persist because we don't reset all settings by hand
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE
|
||||
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE)
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_ABCONFIGANDSTORE
|
||||
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_ABCONFIGSTOREANDDISABLE)
|
||||
#endif
|
||||
{
|
||||
// wait for previous step
|
||||
@ -907,13 +949,16 @@ debugindex(9, hwsettings_baud);
|
||||
return;
|
||||
}
|
||||
// set the Revo GPS port baud rate to the (same) user configured value
|
||||
#if 0
|
||||
debugindex(4, hwsettings_baud);
|
||||
{
|
||||
static uint8_t c;
|
||||
debugindex(5, ++c);
|
||||
}
|
||||
#endif
|
||||
gps_set_fc_baud_from_arg(hwsettings_baud, 3);
|
||||
status->lastConfigSent = LAST_CONFIG_SENT_START;
|
||||
// zero the retries for the first "enable sentence"
|
||||
status->retryCount = 0;
|
||||
// skip enabling UBX sentences for low baud rates
|
||||
// low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration
|
||||
@ -969,8 +1014,11 @@ debugindex(5, ++c);
|
||||
} else {
|
||||
// timeout or NAK, resend the message or abort
|
||||
status->retryCount++;
|
||||
debugindex(0, status->currentStep);
|
||||
debugindex(1, status->lastConfigSent);
|
||||
debugindex(2, status->retryCount);
|
||||
if (status->retryCount > UBX_MAX_RETRIES) {
|
||||
set_current_step_if_untouched(INIT_STEP_ERROR);
|
||||
set_current_step_if_untouched(INIT_STEP_PRE_ERROR);
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
break;
|
||||
}
|
||||
@ -987,63 +1035,53 @@ debugindex(5, ++c);
|
||||
// all configurations have been made
|
||||
case INIT_STEP_SAVE:
|
||||
// now decide whether to save them permanently into the GPS
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE
|
||||
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE) {
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_ABCONFIGANDSTORE
|
||||
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_ABCONFIGSTOREANDDISABLE) {
|
||||
config_save(bytes_to_send);
|
||||
set_current_step_if_untouched(INIT_STEP_SAVE_WAIT_ACK);
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
} else {
|
||||
set_current_step_if_untouched(INIT_STEP_DONE);
|
||||
// allow it enter INIT_STEP_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
set_current_step_if_untouched(INIT_STEP_PRE_DONE);
|
||||
// allow it enter INIT_STEP_PRE_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
break;
|
||||
|
||||
// we could remove this state
|
||||
// if we retry, it writes to settings storage a few more times
|
||||
// and it is probably the ack that was dropped, with the save actually performed correctly
|
||||
// because e.g. OP sentences at 19200 baud don't fit and stuff (like acks) gets dropped
|
||||
// command to save configuration has already been issued
|
||||
case INIT_STEP_SAVE_WAIT_ACK:
|
||||
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
|
||||
// Continue with next configuration option
|
||||
set_current_step_if_untouched(INIT_STEP_DONE);
|
||||
// note that we increase the reply timeout in case the GPS must do a flash erase
|
||||
} else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TO_SAVE_TIMEOUT &&
|
||||
(ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
|
||||
// allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
break;
|
||||
} else {
|
||||
// timeout or NAK, resend the message or abort
|
||||
status->retryCount++;
|
||||
if (status->retryCount > UBX_MAX_RETRIES / 2) {
|
||||
// give up on the retries
|
||||
set_current_step_if_untouched(INIT_STEP_ERROR);
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
} else {
|
||||
// retry a few times
|
||||
set_current_step_if_untouched(INIT_STEP_SAVE);
|
||||
}
|
||||
// save doesn't appear to respond, even in 24 seconds
|
||||
// just delay a while, in case there it is busy with a flash write, etc.
|
||||
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_SAVE_WAIT_TIME) {
|
||||
return;
|
||||
}
|
||||
break;
|
||||
|
||||
// an error, such as retries exhausted, has occurred
|
||||
case INIT_STEP_ERROR:
|
||||
// on error we should get the GPS version immediately
|
||||
gps_ubx_reset_sensor_type();
|
||||
break;
|
||||
|
||||
// user has disable the autoconfig
|
||||
case INIT_STEP_DISABLED:
|
||||
break;
|
||||
// fall through to next state
|
||||
// we can do that if we choose because we haven't sent any data in this state
|
||||
// we actually must fall through because the FSM won't be entered if DONE, ERROR, or DISABLED
|
||||
set_current_step_if_untouched(INIT_STEP_PRE_DONE);
|
||||
// break;
|
||||
|
||||
// the autoconfig has completed normally
|
||||
case INIT_STEP_DONE:
|
||||
case INIT_STEP_PRE_DONE:
|
||||
// determine if we need to disable autoconfig via the autoconfig==CONFIGSTOREANDDISABLE setting
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE) {
|
||||
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_ABCONFIGSTOREANDDISABLE) {
|
||||
enabled = false;
|
||||
status->currentSettings.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED;
|
||||
// like it says
|
||||
GPSSettingsUbxAutoConfigSet((GPSSettingsUbxAutoConfigOptions *) &status->currentSettings.UbxAutoConfig);
|
||||
// GPSSettingsUbxAutoConfigSet((GPSSettingsUbxAutoConfigOptions *) &status->currentSettings.UbxAutoConfig);
|
||||
setGpsSettings();
|
||||
}
|
||||
set_current_step_if_untouched(INIT_STEP_DONE);
|
||||
break;
|
||||
|
||||
// an error, such as retries exhausted, has occurred
|
||||
case INIT_STEP_PRE_ERROR:
|
||||
// on error we should get the GPS version immediately
|
||||
gps_ubx_reset_sensor_type();
|
||||
set_current_step_if_untouched(INIT_STEP_ERROR);
|
||||
break;
|
||||
|
||||
case INIT_STEP_DONE:
|
||||
case INIT_STEP_ERROR:
|
||||
case INIT_STEP_DISABLED:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -1069,7 +1107,7 @@ void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
|
||||
if (config != NULL) {
|
||||
status->currentSettings = *config;
|
||||
}
|
||||
if (status->currentSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) {
|
||||
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_ABANDCONFIGURE) {
|
||||
new_step = INIT_STEP_START;
|
||||
} else {
|
||||
new_step = INIT_STEP_DISABLED;
|
||||
@ -1085,11 +1123,16 @@ void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
|
||||
status->currentStep = new_step;
|
||||
status->currentStepSave = new_step;
|
||||
|
||||
if (status->currentSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) {
|
||||
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_ABANDCONFIGURE) {
|
||||
// enabled refers to autoconfigure
|
||||
// note that sensor type (gps type) detection happens even if completely disabled
|
||||
// also note that AutoBaud is less than Configure
|
||||
enabled = true;
|
||||
} else {
|
||||
// this forces the sensor type detection to occur outside the FSM
|
||||
// and also engages the autobaud detection that is outside the FSM
|
||||
// and _can_ also engage the autobaud detection that is outside the FSM
|
||||
// don't do it if FSM is enabled as FSM can change the baud itself
|
||||
// (don't do it because the baud rates are already in sync)
|
||||
gps_ubx_reset_sensor_type();
|
||||
}
|
||||
}
|
||||
|
@ -188,7 +188,7 @@ void VehicleConfigurationHelper::applyHardwareConfiguration()
|
||||
case VehicleConfigurationSource::GPS_PLATINUM:
|
||||
{
|
||||
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX;
|
||||
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_CONFIGURE;
|
||||
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_ABANDCONFIGURE;
|
||||
AuxMagSettings *magSettings = AuxMagSettings::GetInstance(m_uavoManager);
|
||||
Q_ASSERT(magSettings);
|
||||
AuxMagSettings::DataFields magsData = magSettings->getData();
|
||||
|
@ -14,7 +14,7 @@
|
||||
<field name="VDOP" units="" type="float" elements="1"/>
|
||||
<field name="SensorType" units="" type="enum" elements="1" options="Unknown,NMEA,UBX,UBX7,UBX8" defaultvalue="Unknown" />
|
||||
<field name="AutoConfigStatus" units="" type="enum" elements="1" options="DISABLED,RUNNING,DONE,ERROR" defaultvalue="DISABLED" />
|
||||
<field name="CurrentBaudRate" units="" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400"/>
|
||||
<field name="CurrentBaudRate" units="" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400,Unknown" defaultvalue="Unknown" />
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
@ -6,7 +6,7 @@
|
||||
<field name="MaxPDOP" units="" type="float" elements="1" defaultvalue="3.5"/>
|
||||
<!-- Ubx self configuration. Enable to allow the flight board to auto configure ubx GPS options,
|
||||
store does AutoConfig and save GPS settings (i.e. to prevent issues if gps is power cycled) -->
|
||||
<field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,Configure,ConfigureAndStore,ConfigStoreAndDisable" defaultvalue="ConfigStoreAndDisable"/>
|
||||
<field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,AutoBaud,AbAndConfigure,AbConfigAndStore,AbConfigStoreAndDisable" defaultvalue="AbAndConfigure"/>
|
||||
<!-- Ubx position update rate, -1 for auto -->
|
||||
<field name="UbxRate" units="Hz" type="int8" elements="1" defaultvalue="5" />
|
||||
<!-- Ubx dynamic model, see UBX datasheet for more details -->
|
||||
|
Loading…
x
Reference in New Issue
Block a user