diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 28c586b1e..74166d6cf 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -50,7 +50,6 @@ #include "UBX.h" #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #include "inc/ubx_autoconfig.h" -// #include "../../libraries/inc/fifo_buffer.h" #endif #include @@ -60,8 +59,8 @@ PERF_DEFINE_COUNTER(counterParse); // **************** // Private functions -static void gpsTask(void *parameters); -static void updateHwSettings(UAVObjEvent *ev); +static void gpsTask(__attribute__((unused)) void *parameters); +static void updateHwSettings(__attribute__((unused)) UAVObjEvent *ev); #ifdef PIOS_GPS_SETS_HOMELOCATION static void setHomeLocation(GPSPositionSensorData *gpsData); @@ -69,9 +68,9 @@ static float GravityAccel(float latitude, float longitude, float altitude); #endif #ifdef PIOS_INCLUDE_GPS_UBX_PARSER -void AuxMagSettingsUpdatedCb(UAVObjEvent *ev); +void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); #ifndef PIOS_GPS_MINIMAL -void updateGpsSettings(UAVObjEvent *ev); +void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); #endif #endif @@ -114,7 +113,7 @@ void updateGpsSettings(UAVObjEvent *ev); static GPSSettingsData gpsSettings; -static uint32_t gpsPort; +#define gpsPort PIOS_COM_GPS static bool gpsEnabled = false; static xTaskHandle gpsTaskHandle; @@ -138,12 +137,10 @@ static struct GPS_RX_STATS gpsRxStats; int32_t GPSStart(void) { if (gpsEnabled) { - if (gpsPort) { - // Start gps task - xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); - return 0; - } + // Start gps task + xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); + return 0; } return -1; } @@ -156,8 +153,6 @@ int32_t GPSStart(void) int32_t GPSInitialize(void) { - gpsPort = PIOS_COM_GPS; - HwSettingsInitialize(); #ifdef MODULE_GPS_BUILTIN gpsEnabled = true; @@ -197,7 +192,7 @@ int32_t GPSInitialize(void) // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running updateHwSettings(0); #else /* if defined(REVOLUTION) */ - if (gpsPort && gpsEnabled) { + if (gpsEnabled) { GPSPositionSensorInitialize(); GPSVelocitySensorInitialize(); #if !defined(PIOS_GPS_MINIMAL) @@ -215,7 +210,7 @@ int32_t GPSInitialize(void) } #endif /* if defined(REVOLUTION) */ - 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); #elif defined(PIOS_INCLUDE_GPS_UBX_PARSER) @@ -247,7 +242,13 @@ 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 = 5 / portTICK_RATE_MS; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; #ifdef PIOS_GPS_SETS_HOMELOCATION @@ -273,115 +274,118 @@ static void gpsTask(__attribute__((unused)) void *parameters) // Loop forever while (1) { + if (gpsPort) { #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) - if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { - char *buffer = 0; - uint16_t count = 0; + // do autoconfig stuff for UBX GPS's + if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { + char *buffer = 0; + uint16_t count = 0; - ubx_autoconfig_run(&buffer, &count); - // Something to send? - if (count) { - // clear ack/nak - ubxLastAck.clsID = 0x00; - ubxLastAck.msgID = 0x00; - ubxLastNak.clsID = 0x00; - ubxLastNak.msgID = 0x00; + gps_ubx_autoconfig_run(&buffer, &count); + // Something to send? + if (count) { + // clear ack/nak + ubxLastAck.clsID = 0x00; + ubxLastAck.msgID = 0x00; + ubxLastNak.clsID = 0x00; + ubxLastNak.msgID = 0x00; - PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count); + PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count); + } } - } - // need to do this whether there is received data or not, or the status (e.g. gcs) is not always correct - int32_t ac_status = ubx_autoconfig_get_status(); - static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED - + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING - + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE - + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR; - gpspositionsensor.AutoConfigStatus = - ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED : - ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE : - ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR : - GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING; - if (gpspositionsensor.AutoConfigStatus != lastStatus) { - GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus); - lastStatus = gpspositionsensor.AutoConfigStatus; - } + // 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 + + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE + + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR; + gpspositionsensor.AutoConfigStatus = + ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED : + ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE : + ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR : + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING; + if (gpspositionsensor.AutoConfigStatus != lastStatus) { + GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus); + lastStatus = gpspositionsensor.AutoConfigStatus; + } #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; - cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay); - if (cnt > 0) { - PERF_TIMED_SECTION_START(counterParse); - PERF_TRACK_VALUE(counterBytesIn, cnt); - PERF_MEASURE_PERIOD(counterRate); - int res; - switch (gpsSettings.DataProtocol) { + uint16_t cnt; + // 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); + PERF_MEASURE_PERIOD(counterRate); + int res; + switch (gpsSettings.DataProtocol) { #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) - case GPSSETTINGS_DATAPROTOCOL_NMEA: - res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); - break; + case GPSSETTINGS_DATAPROTOCOL_NMEA: + res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); + break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) - case GPSSETTINGS_DATAPROTOCOL_UBX: - res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); - break; + case GPSSETTINGS_DATAPROTOCOL_UBX: + res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); + break; #endif - default: - res = NO_PARSER; // this should not happen - break; + 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; + timeOfLastCommandMs = timeNowMs; + } } - PERF_TIMED_SECTION_END(counterParse); - if (res == PARSER_COMPLETE) { - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - } - } - - // Check for GPS timeout - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS || - (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) { - // we have not received any valid GPS sentences for a while. - // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS; - GPSPositionSensorStatusSet(&status); - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - } else { - // we appear to be receiving GPS sentences OK, we've had an update - // criteria for GPS-OK taken from this post... - // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 - if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) && - (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && - (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { - AlarmsClear(SYSTEMALARMS_ALARM_GPS); + // Check for GPS timeout + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS || + (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) { + // we have not received any valid GPS sentences for a while. + // either the GPS is not plugged in or a hardware problem or the GPS has locked up. + GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS; + GPSPositionSensorStatusSet(&status); + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); + } else { + // we appear to be receiving GPS sentences OK, we've had an update + // criteria for GPS-OK taken from this post... + // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 + if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) && + (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { + AlarmsClear(SYSTEMALARMS_ALARM_GPS); #ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationData home; - HomeLocationGet(&home); + HomeLocationData home; + HomeLocationGet(&home); - if (home.Set == HOMELOCATION_SET_FALSE) { - if (homelocationSetDelay == 0) { - homelocationSetDelay = xTaskGetTickCount(); - } - if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) { - setHomeLocation(&gpspositionsensor); + if (home.Set == HOMELOCATION_SET_FALSE) { + if (homelocationSetDelay == 0) { + homelocationSetDelay = xTaskGetTickCount(); + } + if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) { + setHomeLocation(&gpspositionsensor); + homelocationSetDelay = 0; + } + } else { homelocationSetDelay = 0; } - } else { - homelocationSetDelay = 0; - } #endif - } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && - (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); + } } - } + } // if (gpsPort) vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS); - } + } // while (1) } #ifdef PIOS_GPS_SETS_HOMELOCATION @@ -436,6 +440,60 @@ static void setHomeLocation(GPSPositionSensorData *gpsData) } #endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */ + +uint32_t hwsettings_gpsspeed_enum_to_baud(uint8_t baud) +{ + switch (baud) { + case HWSETTINGS_GPSSPEED_2400: + return 2400; + + case HWSETTINGS_GPSSPEED_4800: + return 4800; + + default: + case HWSETTINGS_GPSSPEED_9600: + return 9600; + + case HWSETTINGS_GPSSPEED_19200: + return 19200; + + case HWSETTINGS_GPSSPEED_38400: + return 38400; + + case HWSETTINGS_GPSSPEED_57600: + return 57600; + + case HWSETTINGS_GPSSPEED_115200: + return 115200; + + case HWSETTINGS_GPSSPEED_230400: + return 230400; + } +} + + +// 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) +{ + static uint8_t previous_baud = 255; + static uint8_t mutex; // = 0 + + // do we need this? + // can the code stand having two tasks/threads do an XyzSet() call at the same time? + 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)); + GPSPositionSensorBaudRateSet(&baud); + } + } + --mutex; +} + + /** * Update the GPS settings, called on startup. * FIXME: This should be in the GPSSettings object. But objects have @@ -447,161 +505,72 @@ static void setHomeLocation(GPSPositionSensorData *gpsData) // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) { - // with these changes, no one (not even OP board makers) should ever need u-center (the complex UBlox configuration program) - // no booting of Revo should be required during any setup or testing, just Send the settings you want to play with - // with autoconfig enabled, just change the baud rate in HwSettings and it will change the GPS internal baud and then the Revo port - // with autoconfig disabled, it will only change the Revo port, so you can try to find the current GPS baud rate if you don't know it - // GPSPositionSensor.SensorType and .UbxAutoConfigStatus now give correct values at all times, so use .SensorType to prove it is - // connected, even to an incorrectly configured (e.g. factory default) GPS - - // Use cases: - // - the user can plug in a default GPS, use autoconfig-store once and then always use autoconfig-disabled - // - the user can plug in a default GPS that can't store settings (defaults to 9600 baud) and always use autoconfig-nostore - // - - this is one reason for coding this: cheap eBay GPS's that loose their settings sometimes - // - the user can plug in a default GPS that _can_ store settings and always use autoconfig-nostore with that too - // - - if settings change in newer releases of OP, this will make sure to use the correct settings with whatever code you are running - // - the user can plug in a correctly configured GPS and use autoconfig-disabled - // - the user can recover an old or incorrectly configured GPS (internal GPS baud rate or other GPS settings wrong) - // - - disable UBX GPS autoconfig - // - - set HwSettings GPS baud rate to the current GPS internal baud rate to get it to connect at current (especially non-9600) baud rate - // - - - you can tell the baud rate is correct when GPSPositionSensor.SensorType is known (not "Unknown") (e.g. UBX6) - // - - - the SensorType and AutoConfigStatus may fail at 9600 and will fail at 4800 or lower if the GPS is configured to send OP data - // - - - (way more than default) at 4800 baud or slower - // - - enable autoconfig-nostore GPSSettings to set correct messages and enable further magic baud rate settings - // - - change the baud rate to what you want in HwSettings (it will change the baud rate in the GPS and then in the Revo port) - // - - wait for the baud rate change to complete, GPSPositionSensor.AutoConfigStatus will say "DONE" - // - - enable autoconfig-store and it will save the new baud rate and the correct message configuration - // - - wait for the store to complete, GPSPositionSensor.AutoConfigStatus will say "DONE" - // - - for the new baud rate, the user should either choose 9600 for use with autoconfig-nostore or can choose 57600 for use with autoconfig-disabled - // - the user (Dave :)) can configure a whole bunch of default GPS's with no intervention by using autoconfig-store as a saved Revo setting - // - - just plug the default (9600 baud) GPS in to an unpowered Revo, power the Revo/GPS through the servo connector, wait some time, unplug - // - - or with this code, OP could and even should just ship GPS's with default settings - - // if we add an "initial baud rate" instead of assuming 9600 at boot up for autoconfig-nostore/store - // - 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 - // - - 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 - // - - 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: - // - don't play with low baud rates, with OP data settings (lots of data) it can take a long time to auto-configure - // - - at 2400 it could take a long time to autoconfig or error out - // - - at 9600 or lower the autoconfig is skipped and only the baud rate is changed - // - if you autoconfigure an OP configuration at 9600 baud or lower - // - - it will actually make a factory default configuration (not an OP configuration) at that baud rate - // - remember that there are two baud rates (inside the GPS and the Revo port) and you can get them out of sync - // - - rebooting the Revo from the Ground Control Station (without powering the GPS down too) - // - - can leave the baud rates out of sync if you are using autoconfig - // - - just power off and on both the Revo and the GPS - // - my OP GPS #2 will NOT save 115200 baud or higher, but it will use all bauds, even 230400 - // - - 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 - // - if set to 9600 baud (or lower) - - if (gpsPort) { - HwSettingsGPSSpeedOptions speed; - #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) - // use 9600 baud during startup if autoconfig is enabled - // that is the flag that the code should assumes a factory default baud rate - if (ev == NULL && gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) { - speed = HWSETTINGS_GPSSPEED_9600; - } else + static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport #endif - { - // Retrieve settings - HwSettingsGPSSpeedGet(&speed); - } - - // must always set the baud here, even if it looks like it is the same - // e.g. startup sets 9600 here, ubx_autoconfig sets 57600 there, then the user selects a change back to 9600 here - - // on startup (ev == NULL) we must set the Revo port baud rate - // after startup (ev != NULL) we must set the Revo port baud rate only if autoconfig is disabled + // if GPS (UBX or NMEA) is enabled at all + if (gpsPort && gpsEnabled) { + // on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate + // after startup (previousGpsPort == gpsPort) we must set the Revo port baud rate only if autoconfig is disabled // always we must set the Revo port baud rate if not using UBX #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) - if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX) + if (ev == NULL + || previousGpsPort != gpsPort + || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED + || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX) #endif { - // Set Revo port speed - switch (speed) { - case HWSETTINGS_GPSSPEED_2400: - PIOS_COM_ChangeBaud(gpsPort, 2400); - break; - case HWSETTINGS_GPSSPEED_4800: - PIOS_COM_ChangeBaud(gpsPort, 4800); - break; - case HWSETTINGS_GPSSPEED_9600: - PIOS_COM_ChangeBaud(gpsPort, 9600); - break; - case HWSETTINGS_GPSSPEED_19200: - PIOS_COM_ChangeBaud(gpsPort, 19200); - break; - case HWSETTINGS_GPSSPEED_38400: - PIOS_COM_ChangeBaud(gpsPort, 38400); - break; - case HWSETTINGS_GPSSPEED_57600: - PIOS_COM_ChangeBaud(gpsPort, 57600); - break; - case HWSETTINGS_GPSSPEED_115200: - PIOS_COM_ChangeBaud(gpsPort, 115200); - break; - case HWSETTINGS_GPSSPEED_230400: - PIOS_COM_ChangeBaud(gpsPort, 230400); - break; - } + uint8_t speed; + // Retrieve settings + HwSettingsGPSSpeedGet(&speed); + // set fc baud + gps_set_fc_baud_from_arg(speed); #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) // even changing the baud rate will make it re-verify the sensor type - // that way the user can just try some baud rates and it when the sensor type is valid, the baud rate is correct - ubx_reset_sensor_type(); + // that way the user can just try some baud rates and when the sensor type is valid, the baud rate is correct + gps_ubx_reset_sensor_type(); #endif } #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) else { // it will never do this during startup because ev == NULL - ubx_autoconfig_set(NULL); + gps_ubx_autoconfig_set(NULL); } #endif } +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + previousGpsPort = gpsPort; +#endif } + #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { load_mag_settings(); } + void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) { ubx_autoconfig_settings_t newconfig; GPSSettingsGet(&gpsSettings); + // 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 NMEA to UBX or disabled to enabled, that it will start normally + newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig; newconfig.navRate = gpsSettings.UbxRate; - - newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true; - newconfig.storeSettings = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE; - - newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : - gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : - gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : - gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE : - gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA : - gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G : - gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G : - gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G : - UBX_DYNMODEL_AIRBORNE1G; + newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G : + gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G : + UBX_DYNMODEL_AIRBORNE1G; switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) { case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION: @@ -677,7 +646,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) break; } - ubx_autoconfig_set(&newconfig); + gps_ubx_autoconfig_set(&newconfig); } #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ /** diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index 7c33edf11..ab4733a83 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -111,74 +111,102 @@ 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++; + gps_rx_buffer[rx_count++] = c; } // 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 +457,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! + GPSPositionSensorBaudRateGet(&GpsData->BaudRate); GPSPositionSensorSet(GpsData); } return false; @@ -440,6 +470,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData) #ifdef DEBUG_MSGID_IN DEBUG_MSG("U"); #endif + // leave my new field alone! + GPSPositionSensorBaudRateGet(&GpsData->BaudRate); GPSPositionSensorSet(GpsData); } diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 2fc61a283..d34a59195 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -126,9 +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; + int i = 0; - for (int i = 0; i < len; i++) { - c = rx[i]; + while (i < len) { + c = rx[i++]; switch (proto_state) { case START: // detect protocol if (c == UBX_SYNC1) { // first UBX sync char found @@ -170,9 +171,6 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition if (++rx_count == ubx->header.len) { proto_state = UBX_CHK1; } - } else { - gpsRxStats->gpsRxOverflow++; - proto_state = START; } break; case UBX_CHK1: @@ -189,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) { @@ -200,6 +199,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition ret = PARSER_COMPLETE; // message complete & processed } } + return ret; } @@ -528,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! + GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate); GPSPositionSensorSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; id = GPSPOSITIONSENSOR_OBJID; diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h index c952db4cb..4722e1100 100644 --- a/flight/modules/GPS/inc/GPS.h +++ b/flight/modules/GPS/inc/GPS.h @@ -54,6 +54,8 @@ struct GPS_RX_STATS { }; int32_t GPSInitialize(void); +void gps_set_fc_baud_from_arg(uint8_t baud); +uint32_t hwsettings_gpsspeed_enum_to_baud(uint8_t baud); #endif // GPS_H diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index eb3412385..f24bc48a7 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -49,16 +49,15 @@ // 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 -// timeout for ack reception -#define UBX_REPLY_TIMEOUT (500 * 1000) -// timeout for a settings save, in case it has to erase flash -#define UBX_REPLY_TO_SAVE_TIMEOUT (3000 * 1000) +#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 (50 * 1000) -// pause between each unverifiably correct configuration step -#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 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) #define UBX_CFG_CFG_OP_STORE_SETTINGS \ (UBX_CFG_CFG_SETTINGS_IOPORT | \ @@ -85,9 +84,7 @@ typedef enum { #define UBX_ typedef struct { - bool autoconfigEnabled; - bool storeSettings; - + GPSSettingsUbxAutoConfigOptions UbxAutoConfig; bool SBASRanging; bool SBASCorrection; bool SBASIntegrity; @@ -114,9 +111,9 @@ typedef struct UBX_CFG_GNSS ubx_cfg_gnss_t; typedef struct UBXSENTHEADER UBXSentHeader_t; typedef union UBXSENTPACKET UBXSentPacket_t; -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send); -void ubx_autoconfig_set(ubx_autoconfig_settings_t *config); -void ubx_reset_sensor_type(); +void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send); +void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config); +void gps_ubx_reset_sensor_type(); int32_t ubx_autoconfig_get_status(); #endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index a38c7b52b..3e0adf012 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -29,15 +29,19 @@ */ #include #include "hwsettings.h" +#include "gpssettings.h" #include "inc/ubx_autoconfig.h" #include +#include "taskinfo.h" // private type definitions typedef enum { INIT_STEP_DISABLED = 0, INIT_STEP_START, + INIT_STEP_SEND_MON_VER, + INIT_STEP_WAIT_MON_VER_ACK, INIT_STEP_RESET_GPS, INIT_STEP_REVO_9600_BAUD, INIT_STEP_GPS_BAUD, @@ -48,7 +52,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; @@ -58,17 +64,24 @@ typedef struct { uint32_t lastStepTimestampRaw; // timestamp of last operation uint32_t lastConnectedRaw; // timestamp of last time gps was connected struct { - UBXSentPacket_t working_packet; // outbound "buffer" - // bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800 - // failure mode otherwise: - // - send message with baud rate change - // - 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) - uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+ + union { + struct { + UBXSentPacket_t working_packet; // outbound "buffer" + // bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800 + // failure mode otherwise: + // - send message with baud rate change + // - 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 when someone has time + 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 + int8_t lastConfigSent; // index of last configuration string sent struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS uint8_t retryCount; } status_t; @@ -142,6 +155,12 @@ ubx_cfg_msg_t msg_config_ubx7[] = { // note that a reset is always done with autoconfig.store // #define ALWAYS_RESET +// we can enable this when we know how to make the Flight Controller save an object to permanent storage +// also see comment about simple edit in gpssettings.xml +#define AUTOBAUD_CONFIGURE_STORE_AND_DISABLE +// Alessio Morale May 20 3:16 AM +// @Cliff you should update the ObjectPersistence uavo passing the object id and the desired operation. + // private variables // enable the autoconfiguration system @@ -150,7 +169,9 @@ static volatile bool current_step_touched = false; // both the pointer and what it points to are volatile. Yuk. static volatile status_t *volatile status = 0; static uint8_t hwsettings_baud; +static uint8_t baud_to_try_index = 255; +// functions static void append_checksum(UBXSentPacket_t *packet) { @@ -199,7 +220,7 @@ static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t mess static void set_current_step_if_untouched(initSteps_t new_steps) { // assume this one byte initSteps_t is atomic - // take care of some but not all concurrency issues + // take care of some concurrency issues if (!current_step_touched) { status->currentStep = new_steps; @@ -210,11 +231,21 @@ static void set_current_step_if_untouched(initSteps_t new_steps) } -void ubx_reset_sensor_type() +void gps_ubx_reset_sensor_type() { - ubxHwVersion = -1; - sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; - GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType); + static uint8_t mutex; // = 0 + + // is this needed? + // what happens if two tasks / threads try to do an XyzSet() at the same time? + 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; + } + --mutex; } @@ -243,73 +274,16 @@ 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 NMEA enable status->working_packet.message.payload.cfg_prt.inProtoMask = 1; // 1 = UBX only (bit 0) - status->working_packet.message.payload.cfg_prt.outProtoMask = 1; // 1 = UBX only (bit 0) + // disable current UBX messages for low baud rates + status->working_packet.message.payload.cfg_prt.outProtoMask = 1; // Ask GPS to change it's speed - switch (hwsettings_baud) { - case HWSETTINGS_GPSSPEED_2400: - status->working_packet.message.payload.cfg_prt.baudRate = 2400; - break; - case HWSETTINGS_GPSSPEED_4800: - status->working_packet.message.payload.cfg_prt.baudRate = 4800; - break; - case HWSETTINGS_GPSSPEED_9600: - status->working_packet.message.payload.cfg_prt.baudRate = 9600; - break; - case HWSETTINGS_GPSSPEED_19200: - status->working_packet.message.payload.cfg_prt.baudRate = 19200; - break; - case HWSETTINGS_GPSSPEED_38400: - status->working_packet.message.payload.cfg_prt.baudRate = 38400; - break; - case HWSETTINGS_GPSSPEED_57600: - status->working_packet.message.payload.cfg_prt.baudRate = 57600; - break; - case HWSETTINGS_GPSSPEED_115200: - status->working_packet.message.payload.cfg_prt.baudRate = 115200; - break; - case HWSETTINGS_GPSSPEED_230400: - status->working_packet.message.payload.cfg_prt.baudRate = 230400; - break; - } - + status->working_packet.message.payload.cfg_prt.baudRate = hwsettings_gpsspeed_enum_to_baud(hwsettings_baud); *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t)); } -// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate -static void config_baud(uint8_t baud) -{ - // Set Revo port hwsettings_baud - switch (baud) { - case HWSETTINGS_GPSSPEED_2400: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 2400); - break; - case HWSETTINGS_GPSSPEED_4800: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 4800); - break; - case HWSETTINGS_GPSSPEED_9600: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 9600); - break; - case HWSETTINGS_GPSSPEED_19200: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 19200); - break; - case HWSETTINGS_GPSSPEED_38400: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 38400); - break; - case HWSETTINGS_GPSSPEED_57600: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 57600); - break; - case HWSETTINGS_GPSSPEED_115200: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 115200); - break; - case HWSETTINGS_GPSSPEED_230400: - PIOS_COM_ChangeBaud(PIOS_COM_GPS, 230400); - break; - } -} - - static void config_rate(uint16_t *bytes_to_send) { memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t)); @@ -485,144 +459,50 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) } -// End User Documentation +#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) +// permanently store our version of GPSSettings.UbxAutoConfig +// we use this to disable after AbConfigStoreAndDisable is complete +static void setGpsSettings() +{ + // trying to do this as perfectly as possible we must realize that they may have pressed Send on some fields + // and so those fields are not stored permanently + // if we write the memory copy to flash, we will have made those permanent -// There are two baud rates of interest -// The baud rate the GPS is talking at -// The baud rate Revo is talking at -// These two must match for the GPS to work -// You only have direct control of the Revo baud rate -// The two baud rates must be the same for the Revo to send a command to the GPS -// to tell the GPS to change it's baud rate -// So you start out by changing Revo's baud rate to match the GPS's -// and then enable UbxAutoConfig to tell Revo to change the GPS baud every time, just before it changes the Revo baud -// That is the basis of these instructions + // we could save off the uavo memory copy to a local buffer with a standard GPSSettingsGet() + // load from flash to uavo memory with a UAVObjLoad() + // update our one setting in uavo memory with a standard GPSSettingsUbxAutoConfigSet() + // save from uavo memory to flash with a UAVObjSave() + // modify our saved off copy to have our new setting in it too + // and finally copy the local buffer back out to uavo memory -// There are microprocessors and they each have internal settings -// Revo -// GPS -// and each of these settings can be temporary or permanent + // that would do it as correctly as possible, but it doesn't work + // so we do it the way autotune.c does it -// To change a Revo setting -// Use the System tab in the GCS for all the following -// Example: in Settings->GPSSettings click on the VALUE for UbxAutoConfig and change it to Disabled -// Click on UbxAutoConfig itself and the line will turn green and blue -// To change this setting permanently, press the red up arrow (Save) at the top of the screen -// Permanently means that it uses this setting, even if you reboot Revo, e.g. power off and on -// To change this setting temporarily, press the green up arrow (Send) at the top of the screen -// Temporarily means that it overrides the permanent setting, but it goes back to the permanent setting when you reboot Revo, e.g. power off and on - -// To change an internal GPS setting you use the OP GCS System tab to tell Revo to make the GPS changes -// This only works correctly after you have matching baud rates so Revo and GPS can talk together -// "Settings->GPSSettings->UbxAutoConfig = Configure" sets the internal GPS setting temporarily -// "Settings->GPSSettings->UbxAutoConfig = ConfigureAndStore" sets the internal GPS setting permanently - -// You want to wind up with a set of permanent settings that work together -// There are two different sets of permanent settings that work together -// GPS at 9600 baud and factory defaults -// Revo configured to start out at 9600 baud, but then completely configure the GPS and switch both to 57600 baud -// (takes 6 seconds at boot up while you are waiting for it to acquire satellites anyway) -// This is the preferred way so that if we change the settings in the future, the new release will automatically use the correct settings -// GPS at 57600 baud with all the settings for the current release stored in the GPS -// Revo configured to disable UbxAutoConfig since all the GPS settings are permanently stored correctly -// May require reconfiguring in a future release - -// Changable settings of interest -// AutoConfig mode -// Settings->GPSSettings->UbxAutoConfig (Disabled, Configure, ConfigureAndStore, default=Configure) -// Disabled means that changes to the GPS baud setting only affect the Revo port -// It doesn't try to change the GPS's internal baud rate setting -// Configure means change the GPS's internal baud setting temporarily (GPS settings revert to the permanent values when GPS is powered off/on) -// ConfigureAndStore means change the GPS's internal baud setting permanently (even after the GPS is powered off/on) -// GPS baud rate -// Settings->HwSettings->GPSSpeed -// If the baud rates are the same and an AutoConfig mode is enabled this will change both the GPS baud rate and the Revo baud rate -// If the baud rates are not the same and an AutoConfig mode is enabled it will fail -// If AutoConfig mode is disabled this will only change the Revo baud rate - -// View only settings of interest -// Detected GPS type -// Data Objects -> GPSPositionSensor -> SensorType (Unknown, NMEA, UBX, UBX7, UBX8) -// When it says something other than Unknown, the GPS and Revo baud rates are synced and talking -// Real time progress of the GPS detection process -// Data Objects -> GPSPositionSensor -> AutoConfigStatus (DISABLED, RUNNING, DONE, ERROR) - -// Syncing the baud rates means that the GPS's internal baud rate setting is the same as the Revo port setting -// This is necessary for the GPS to work with Revo -// To sync to and find out an unknown GPS baud rate (or sync to and use a known GPS baud rate) -// Temporarily change the AutoConfig mode to Disabled -// Temporarily change the GPS baud rate to a value you think it might be (or go up the list) -// See if that baud rate is correct (Data Objects->GPSPositionSensor->SensorType will be something besides Unknown) -// Repeat, changing the GPS baud rate, until found - -// Some very important facts: -// For 9600 baud or lower, the autoconfig will configure it to factory default settings -// For 19200 baud or higher, the autoconfig will configure it to OP required settings -// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud -// 57600 baud is recommended for the current release -// That can be achieved either by -// autoconfiging the GPS from a permanent 9600 baud (and factory settings) to a temporary 57600 (with OP settings) on each power up -// or by configuring the GPS with a permanent 57600 (with OP settings) and then permanently disabling autoconfig -// Some previous releases used 38400 and had some other settings differences - -// The user should either: -// Permanently configure their GPS to 9600 baud factory settings and tell the Revo configuration to load volatile settings at each startup by: -// (Recommended method because new versions could require new settings and this handles future changes automatically) -// Syncing the baud rates -// Setting it to autoconfig.nostore and waiting for it to complete -// Setting HwSettings.GPSSpeed to 9600 and waiting for it to complete -// Setting it to autoconfig.store and waiting for it to complete (this tells the GPS to store the 9600 permanently) -// Permanently setting it to autoconfig.nostore and waiting for it to complete -// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete -// Permanently configure their GPS to 57600 baud, including OpenPilot settings and telling the Revo configuration to just set the baud to 57600 at each startup by: -// (Less recommended method because new versions could require new settings so you would have to do this again) -// Syncing the baud rates -// Setting it to autoconfig.nostore and waiting for it to complete -// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete -// Setting it to autoconfig.store -// Permanently setting it to autoconfig.disabled - -// The algorithm is: -// If autoconfig is enabled at all -// It will assume that the GPS boot up baud rate is 9600 and the user wants that changed to HwSettings.GPSSpeed -// and that change can be either volatile (must be done each boot up) or non-volatile (stored in GPS's non-volatile settings storage) -// according to whether CONFIGURE is used or CONFIGUREANDSTORE is used -// The only user who should need CONFIGUREANDSTORE stored permanently in Revo is Dave, who configures many OP GPS's before shipping -// plug a factory default GPS in to a Revo, power up, wait for it to configure and permanently store in the GPS, power down, ship -// If autoconfig is not enabled -// 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 -// 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 -// 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 - -// Talking points to point out: -// U-center is no longer needed for any use case with this code -// 9600 is factory default for GPS's -// Some GPS can't even permanently store settings and must start at 9600 baud? -// I have a GPS that sometimes looses settings and reverts to 9600 and this is a fix for that too :) -// This code handles a GPS configured either way (9600 with factory default settings or e.g. 57600 with OP settings) -// Autoconfig.nostore at each boot for 9600, autoconfig.disabled for the 57600 with OP settings (or custom settings and baud) -// This code can permanently configure a GPS to be e.g. 9600 with factory default settings or 57600 with OP settings -// GPS's with 9600 baud and factory default settings would be a good default for future OP releases -// Changing the GPS internal settings multiple times in the future is handled automatically -// This code is written to do a configure from 9600 to 57600 -// (actually 9600 to whatever is stored in HwSettings.GPSSpeed) -// if autoconfig is enabled at boot up -// When autoconfiging to 9600 baud or lower, the autoconfig will configure it to factory default settings, not OP settings -// That is because 9600 baud drops many of the OP messages and because 9600 baud is factory default -// For 19200 baud or higher, the autoconfig will configure it to OP required settings -// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud -// This is good for factory default GPS's -// This is good in case we change some settings in a future release +#if 0 + // get the "in memory" version to a local buffer + GPSSettingsGet((void *)&status->gpsSettings); + // load the permanent version into memory + UAVObjLoad(GPSSettingsHandle(), 0); +#endif + // change the in memory version of the field we want to change + GPSSettingsUbxAutoConfigSet((GPSSettingsUbxAutoConfigOptions *)&status->currentSettings.UbxAutoConfig); + // save the in memory version to permanent + UAVObjSave(GPSSettingsHandle(), 0); +#if 0 + // copy the setting into the struct we will use to Set() + status->gpsSettings.UbxAutoConfig = status->currentSettings.UbxAutoConfig; + // try casting it correctly and it says: + // expected 'struct GPSSettingsData *' but argument is of type 'struct GPSSettingsData *' + // probably a volatile or align issue + GPSSettingsSet((void *)&status->gpsSettings); // set the "in memory" version back into use +#endif +} +#endif /* if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) */ -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) +// 9600 baud and lower are not usable, and are best left at factory default +// if the user selects 9600 +void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) { *bytes_to_send = 0; *buffer = (char *)status->working_packet.buffer; @@ -632,13 +512,12 @@ void 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 + + // get UBX version whether autobaud / autoconfig is enabled or not + // this allows the user to manually try some baud rates and visibly see when it works + // it also is how the autobaud code determines when the baud rate is correct // ubxHwVersion is a global set externally by the caller of this function + // it is set when the GPS responds to a MON_VER message if (ubxHwVersion <= 0) { // at low baud rates and high data rates the ubx gps simply must drop some outgoing data // this isn't really an error @@ -650,14 +529,59 @@ void 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) { return; } + + // at this point we have already waited for the MON_VER reply to time out (except the first time where it times out without being sent) + // and the fact we are here says that ubxHwVersion has not been set (it is set externally) + // so this try at this baud rate has failed + // if we get here + // select the next baud rate, skipping ahead if new baud rate is HwSettings.GPSSpeed + // set Revo baud rate to current++ value (immediate change so we can send right after that) and send the MON_VER request + // baud rate search order are most likely matches first + + // if AutoBaud or higher, do AutoBaud + if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) { + uint8_t baud_to_try; + 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 + }; + + // first try HwSettings.GPSSpeed and then + // get the next baud rate to try from the table, but skip over the value of HwSettings.GPSSpeed + do { + // index is inited to be out of bounds, which is interpreted as "currently defined baud rate" (= HwSettings.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); + // set the FC (Revo) baud rate + gps_set_fc_baud_from_arg(baud_to_try); + } + + // 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 (!enabled) { // keep resetting the timeouts here if we are not actually going to run the configure code // not really necessary, but it keeps the timer from wrapping every 50 seconds @@ -665,23 +589,40 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) 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) { - return; - } - + //////// + // FSM + //////// switch (status->currentStep) { + // if here, we have verified that the baud rates are in sync sometime in the past case INIT_STEP_START: - // we should look for the GPS version again - ubx_reset_sensor_type(); - // do not fall through to next state - // or it might try to get the sensor type when the baud rate is half changed - set_current_step_if_untouched(INIT_STEP_RESET_GPS); - // allow it to get the sensor type immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + // 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) + 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 + // break; + + case INIT_STEP_SEND_MON_VER: + 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); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); break; + case INIT_STEP_WAIT_MON_VER_ACK: + // wait for previous step + // extra wait time might well be unnecessary but we want to make sure + // that we don't stop waiting too soon + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { + return; + } + // Continue with next configuration option + set_current_step_if_untouched(INIT_STEP_RESET_GPS); + // fall through to next state + // we can do that if we choose because we haven't sent any data in this state + // break; + + // if here, we have just verified that the baud rates are in sync (again) case INIT_STEP_RESET_GPS: // make sure we don't change the baud rate too soon and garble the packet being sent // even after pios says the buffer is empty, the serial port buffer still has data in it @@ -694,15 +635,19 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) #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 - // because low baud and high OP data rate doesn't play nice + // if user requests a low baud rate then we just reset and avoid adding navigation sentences + // because low GPS 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.storeSettings) + if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGUREANDSTORE +#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) + || status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE +#endif + ) #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); @@ -712,22 +657,30 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) set_current_step_if_untouched(INIT_STEP_REVO_9600_BAUD); break; + // 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.storeSettings) + if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGUREANDSTORE +#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) + || status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE +#endif + ) #endif { // wait for previous step + // extra wait time might well be unnecessary but we want to make very sure + // that we don't stop waiting too soon as that could leave us at an unknown baud rate + // (i.e. set or not set) if the the transmit buffer was full and we were running at a low baud rate if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { return; } // set the Revo GPS port to 9600 baud to match the reset to factory default that has already been done - config_baud(HWSETTINGS_GPSSPEED_9600); + gps_set_fc_baud_from_arg(HWSETTINGS_GPSSPEED_9600); } // at most, we just set Revo baud and that doesn't send any data // fall through to next state @@ -736,6 +689,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); // break; + // Revo and GPS are both at 9600 baud case INIT_STEP_GPS_BAUD: // https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf // It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could @@ -757,14 +711,16 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); break; + // GPS is at final baud and Revo is at old baud (old is 9600 or initial detected baud) case INIT_STEP_REVO_BAUD: // wait for previous step if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { return; } // set the Revo GPS port baud rate to the (same) user configured value - config_baud(hwsettings_baud); + gps_set_fc_baud_from_arg(hwsettings_baud); 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 @@ -821,7 +777,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // timeout or NAK, resend the message or abort 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; } @@ -835,55 +791,67 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) break; } + // all configurations have been made case INIT_STEP_SAVE: - if (status->currentSettings.storeSettings) { + // now decide whether to save them permanently into the GPS + if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGUREANDSTORE +#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) + || status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE +#endif + ) { 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 + // 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; } + // fall through to next state + // we can do that if we choose because we haven't sent any data in this state + set_current_step_if_untouched(INIT_STEP_PRE_DONE); + // break; + + // the autoconfig has completed normally + case INIT_STEP_PRE_DONE: +#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) + // determine if we need to disable autoconfig via the autoconfig==AUTOBAUDCONFIGSTOREANDDISABLE setting + if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE) { + enabled = false; + status->currentSettings.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED; + // like it says + setGpsSettings(); + } +#endif + set_current_step_if_untouched(INIT_STEP_DONE); break; - case INIT_STEP_ERROR: + // an error, such as retries exhausted, has occurred + case INIT_STEP_PRE_ERROR: // on error we should get the GPS version immediately - ubx_reset_sensor_type(); - // fall through - case INIT_STEP_DISABLED: + 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; } } -void ubx_autoconfig_set(ubx_autoconfig_settings_t *config) +// this can be called from a different thread +// so everything it touches must be declared volatile +void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config) { initSteps_t new_step; @@ -899,11 +867,12 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t *config) if (config != NULL) { status->currentSettings = *config; } - if (status->currentSettings.autoconfigEnabled) { + if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE) { new_step = INIT_STEP_START; } else { new_step = INIT_STEP_DISABLED; } + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); // assume this one byte initSteps_t is atomic // take care of some but not all concurrency issues @@ -914,11 +883,21 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t *config) status->currentStep = new_step; status->currentStepSave = new_step; - if (status->currentSettings.autoconfigEnabled) { + if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE) { + // 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 _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(); } } + int32_t ubx_autoconfig_get_status() { if (!status || !enabled) { diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index 37b1ab02d..2c3deefe0 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -348,6 +348,7 @@ static const struct pios_dsm_cfg pios_dsm_main_cfg = { #endif /* PIOS_INCLUDE_DSM */ + #include #if defined(PIOS_INCLUDE_SBUS) /* @@ -534,6 +535,54 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #endif /* PIOS_INCLUDE_DSM */ +#if defined(PIOS_INCLUDE_SRXL) +/* + * SRXL USART + */ +#include + +static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { + .regs = FLEXI_USART_REGS, + .remap = FLEXI_USART_REMAP, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = FLEXI_USART_IRQ, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = FLEXI_USART_RX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_RX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = FLEXI_USART_TX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_TX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + +#endif /* PIOS_INCLUDE_SRXL */ /* * HK OSD */ diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index f9abf43b1..16803c751 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -107,6 +107,7 @@ /* #define PIOS_INCLUDE_PPM_FLEXI */ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_SRXL #define PIOS_INCLUDE_GCSRCVR // #define PIOS_INCLUDE_OPLINKRCVR diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 20218a00f..41a5da831 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -642,6 +642,27 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_FLEXIPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; + case HWSETTINGS_RM_FLEXIPORT_SRXL: +#if defined(PIOS_INCLUDE_SRXL) + { + uint32_t pios_usart_srxl_id; + if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_srxl_id; + if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) { + PIOS_Assert(0); + } + + uint32_t pios_srxl_rcvr_id; + if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id; + } +#endif + break; } /* hwsettings_rm_flexiport */ diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 86fa7f03d..54776fe34 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler; #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + // ------------------------- // Receiver DSM input // ------------------------- diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 2bf06f959..e35c688e9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -176,19 +176,20 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() GPSSettings *gpsSettings = GPSSettings::GetInstance(m_uavoManager); Q_ASSERT(gpsSettings); GPSSettings::DataFields gpsData = gpsSettings->getData(); - gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_DISABLED; switch (m_configSource->getGpsType()) { case VehicleConfigurationSource::GPS_NMEA: - gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_NMEA; + gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_NMEA; + gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_DISABLED; break; case VehicleConfigurationSource::GPS_UBX: - gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX; + gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX; + gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE; break; case VehicleConfigurationSource::GPS_PLATINUM: { gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX; - gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_CONFIGURE; + gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE; AuxMagSettings *magSettings = AuxMagSettings::GetInstance(m_uavoManager); Q_ASSERT(magSettings); AuxMagSettings::DataFields magsData = magSettings->getData(); diff --git a/shared/uavobjectdefinition/gpspositionsensor.xml b/shared/uavobjectdefinition/gpspositionsensor.xml index a6a5ea194..54034a717 100644 --- a/shared/uavobjectdefinition/gpspositionsensor.xml +++ b/shared/uavobjectdefinition/gpspositionsensor.xml @@ -14,6 +14,7 @@ + diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index 8eb95db8a..57f02f492 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -6,7 +6,8 @@ - + +