rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

OP-1464 tested version with debug still in place

Browse Source
This commit is contained in:
Cliff Geerdes 2015-05-19 19:33:31 -04:00
parent 92295559b0
commit de521c9004
9 changed files with 312 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
flight/modules/GPS/GPS.c
View File

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

134
flight/modules/GPS/NMEA.c
View File

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

23
flight/modules/GPS/UBX.c
View File

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

2
flight/modules/GPS/inc/UBX.h
View File

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

38
flight/modules/GPS/inc/ubx_autoconfig.h
View File

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

247
flight/modules/GPS/ubx_autoconfig.c
View File

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

2
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

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

2
shared/uavobjectdefinition/gpspositionsensor.xml
View File

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

2
shared/uavobjectdefinition/gpssettings.xml
View File

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