From 48bea16ab97f6bd4231880f019c95c57a5f4afb6 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 29 Apr 2015 23:46:47 -0400 Subject: [PATCH 1/5] OP-1840 GPS serial improvements initial coding --- flight/modules/GPS/GPS.c | 336 +++++++---- flight/modules/GPS/UBX.c | 6 +- flight/modules/GPS/inc/UBX.h | 319 +++++++++-- flight/modules/GPS/inc/ubx_autoconfig.h | 209 ++----- flight/modules/GPS/ubx_autoconfig.c | 704 +++++++++++++++++++----- 5 files changed, 1122 insertions(+), 452 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 460f6fdc1..a7671e714 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -50,6 +50,7 @@ #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,7 +61,7 @@ PERF_DEFINE_COUNTER(counterParse); // Private functions static void gpsTask(void *parameters); -static void updateHwSettings(); +static void updateHwSettings(UAVObjEvent *ev); #ifdef PIOS_GPS_SETS_HOMELOCATION static void setHomeLocation(GPSPositionSensorData *gpsData); @@ -111,6 +112,8 @@ void updateGpsSettings(UAVObjEvent *ev); // **************** // Private variables +static GPSSettingsData gpsSettings; + static uint32_t gpsPort; static bool gpsEnabled = false; @@ -150,15 +153,15 @@ int32_t GPSStart(void) * \return -1 if initialisation failed * \return 0 on success */ + int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; - uint8_t gpsProtocol; + HwSettingsInitialize(); #ifdef MODULE_GPS_BUILTIN gpsEnabled = true; #else - HwSettingsInitialize(); HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); @@ -188,7 +191,11 @@ int32_t GPSInitialize(void) AuxMagSettingsUpdatedCb(NULL); AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb); #endif - updateHwSettings(); + GPSSettingsInitialize(); + // updateHwSettings() uses gpsSettings + GPSSettingsGet(&gpsSettings); + // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running + updateHwSettings(0); #else if (gpsPort && gpsEnabled) { GPSPositionSensorInitialize(); @@ -200,27 +207,29 @@ int32_t GPSInitialize(void) #if defined(PIOS_GPS_SETS_HOMELOCATION) HomeLocationInitialize(); #endif - updateHwSettings(); + GPSSettingsInitialize(); + // updateHwSettings() uses gpsSettings + GPSSettingsGet(&gpsSettings); + // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running + updateHwSettings(0); } #endif /* if defined(REVOLUTION) */ if (gpsPort && gpsEnabled) { - GPSSettingsInitialize(); - GPSSettingsDataProtocolGet(&gpsProtocol); - switch (gpsProtocol) { -#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) - case GPSSETTINGS_DATAPROTOCOL_NMEA: - gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH); - break; +#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) + gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket)); +#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER) + gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH); +#else + gps_rx_buffer = NULL; #endif - case GPSSETTINGS_DATAPROTOCOL_UBX: - gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket)); - break; - default: - gps_rx_buffer = NULL; - } +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) PIOS_Assert(gps_rx_buffer); +#endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup GPSSettingsConnectCallback(updateGpsSettings); #endif return 0; @@ -235,6 +244,27 @@ MODULE_INITCALL(GPSInitialize, GPSStart); /** * Main gps task. It does not return. */ +enum pios_com_dev_magic { + PIOS_COM_DEV_MAGIC = 0xaa55aa55, +}; + +struct pios_com_dev { + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver *driver; + +#if defined(PIOS_INCLUDE_FREERTOS) + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; + xSemaphoreHandle sendbuffer_sem; +#endif + + bool has_rx; + bool has_tx; + + t_fifo_buffer rx; + t_fifo_buffer tx; +}; static void gpsTask(__attribute__((unused)) void *parameters) { @@ -245,15 +275,13 @@ static void gpsTask(__attribute__((unused)) void *parameters) portTickType homelocationSetDelay = 0; #endif GPSPositionSensorData gpspositionsensor; - GPSSettingsData gpsSettings; - - GPSSettingsGet(&gpsSettings); timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; GPSPositionSensorGet(&gpspositionsensor); #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) + // this should be done in the task because it calls out to actually start the GPS serial reads updateGpsSettings(0); #endif @@ -270,9 +298,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { char *buffer = 0; uint16_t count = 0; - uint8_t status; - GPSPositionSensorStatusGet(&status); - ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS); + + ubx_autoconfig_run(&buffer, &count); // Something to send? if (count) { // clear ack/nak @@ -284,10 +311,28 @@ static void gpsTask(__attribute__((unused)) void *parameters) 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; + } #endif - // This blocks the task until there is something on the buffer + + // This blocks the task until there is something on the buffer (or 100ms? passes) uint16_t cnt; - while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) { + 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); @@ -300,24 +345,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) case GPSSETTINGS_DATAPROTOCOL_UBX: - { -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) - int32_t ac_status = ubx_autoconfig_get_status(); - static uint8_t lastStatus = 0; - - 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 res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); - } - break; + break; #endif default: res = NO_PARSER; // this should not happen @@ -435,40 +464,139 @@ static void setHomeLocation(GPSPositionSensorData *gpsData) * like protocol, etc. Thus the HwSettings object which contains the * GPS port speed is used for now. */ -static void updateHwSettings() -{ - if (gpsPort) { - // Retrieve settings - uint8_t speed; - HwSettingsGPSSpeedGet(&speed); - // Set 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; +// 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 + + // 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 my main 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 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) + // - should the 57600 be fixed 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 startup and only if you NEVER enable and store (even once) OP messages (a lot more data) at 9600 + // - - using autoconfig-store just one time at 9600 will pretty much force you to use 57600 + // - by my testing, the 9600 initial to 57600 final baud startup takes: + // - - 1:13 if the GPS has been configured to send OP data at 9600 + // - - 0:12 if the GPS has default data settings (NEMA) + // - - reminder that even 1:13 isn't that bad. You need to wait for the GPS to acquire satellites, + // - - and it does that the whole time it is being configured + + // Some things you want to know if you want to play around with this: + // - 57600 baud is what you want stored inside the GPS settings if you autoconfig-store it even once at any baud rate + // - - then use autoconfig-disabled once the GPS settings are stored in the GPS + // - don't play with low baud rates, with OP data settings (lots of data) at best it takes a long time to auto-configure + // - - at 19200 or lower it will fail to autoconfig and set the status to 'ERROR' + // - - at 19200 or lower the autoconfig is skipped and only the baud rate is changed + // - if you autoconfigure-store an OP configuration at 19200 baud or lower + // - - it will actually store a factory default configuration at that baud rate + // - - you will probably have to use u-center to fix it + // - - default settings (NEMA) at 9600 are OK because there is not nearly as much data as the OP config uses, + // - 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-nostore + // - - 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 + + // since 9600 baud and lower are not usable, and are best left at NEMA, I could / should / will code it to do a factory reset + // - if set to 9600 baud (or lower???) + + if (gpsPort) { + // static uint8_t old_speed = HWSETTINGS_GPSSPEED_230400+1; + uint8_t 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 +#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 + // 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) +#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; + } + // 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(); + } +#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); + } +#endif } } @@ -480,32 +608,26 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) { - uint8_t ubxAutoConfig; - uint8_t ubxDynamicModel; - uint8_t ubxSbasMode; ubx_autoconfig_settings_t newconfig; - uint8_t ubxSbasSats; - uint8_t ubxGnssMode; - GPSSettingsUbxRateGet(&newconfig.navRate); + GPSSettingsGet(&gpsSettings); - GPSSettingsUbxAutoConfigGet(&ubxAutoConfig); - newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true; - newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE; + newconfig.navRate = gpsSettings.UbxRate; - GPSSettingsUbxDynamicModelGet(&ubxDynamicModel); - newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G : - ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G : + 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; - GPSSettingsUbxSBASModeGet(&ubxSbasMode); - switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) { + switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) { case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION: case GPSSETTINGS_UBXSBASMODE_CORRECTION: case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY: @@ -516,7 +638,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) newconfig.SBASCorrection = false; } - switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) { + switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) { case GPSSETTINGS_UBXSBASMODE_RANGING: case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION: case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY: @@ -527,7 +649,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) newconfig.SBASRanging = false; } - switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) { + switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) { case GPSSETTINGS_UBXSBASMODE_INTEGRITY: case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY: case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY: @@ -538,19 +660,15 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) newconfig.SBASIntegrity = false; } - GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed); + newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed; - GPSSettingsUbxSBASSatsGet(&ubxSbasSats); + newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN : + gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN; - newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN : - ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN; - - GPSSettingsUbxGNSSModeGet(&ubxGnssMode); - - switch (ubxGnssMode) { + switch (gpsSettings.UbxGNSSMode) { case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS: newconfig.enableGPS = true; newconfig.enableGLONASS = true; @@ -583,7 +701,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) break; } - ubx_autoconfig_set(newconfig); + ubx_autoconfig_set(&newconfig); } #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ /** diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index a75efd28a..74ebc78e8 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -43,7 +43,7 @@ static bool useMag = false; #endif -static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; +GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; static bool usePvt = false; static uint32_t lastPvtTime = 0; @@ -466,9 +466,11 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver; ubxHwVersion = atoi(mon_ver->hwVersion); - sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); + // send sensor type right now because on UBX NEMA we don't get a full set of messages + // and we want to be able to see sensor type even on UBX NEMA GPS's + GPSPositionSensorSensorTypeSet((uint8_t *) &sensorType); } static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index a7e938f46..51c8552fa 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -102,7 +102,8 @@ typedef enum { UBX_ID_CFG_MSG = 0x01, UBX_ID_CFG_CFG = 0x09, UBX_ID_CFG_SBAS = 0x16, - UBX_ID_CFG_GNSS = 0x3E + UBX_ID_CFG_GNSS = 0x3E, + UBX_ID_CFG_PRT = 0x00 } ubx_class_cfg_id; typedef enum { @@ -129,6 +130,39 @@ typedef enum { UBX_ID_RXM_SFRB = 0x11, UBX_ID_RXM_SVSI = 0x20, } ubx_class_rxm_id; + +typedef enum { + UBX_GNSS_ID_GPS = 0, + UBX_GNSS_ID_SBAS = 1, + UBX_GNSS_ID_GALILEO = 2, + UBX_GNSS_ID_BEIDOU = 3, + UBX_GNSS_ID_IMES = 4, + UBX_GNSS_ID_QZSS = 5, + UBX_GNSS_ID_GLONASS = 6, + UBX_GNSS_ID_MAX +} ubx_config_gnss_id_t; + +// Enumeration options for field UBXDynamicModel +typedef enum { + UBX_DYNMODEL_PORTABLE = 0, + UBX_DYNMODEL_STATIONARY = 2, + UBX_DYNMODEL_PEDESTRIAN = 3, + UBX_DYNMODEL_AUTOMOTIVE = 4, + UBX_DYNMODEL_SEA = 5, + UBX_DYNMODEL_AIRBORNE1G = 6, + UBX_DYNMODEL_AIRBORNE2G = 7, + UBX_DYNMODEL_AIRBORNE4G = 8 +} ubx_config_dynamicmodel_t; + +typedef enum { + UBX_SBAS_SATS_AUTOSCAN = 0, + UBX_SBAS_SATS_WAAS = 1, + UBX_SBAS_SATS_EGNOS = 2, + UBX_SBAS_SATS_MSAS = 3, + UBX_SBAS_SATS_GAGAN = 4, + UBX_SBAS_SATS_SDCM = 5 +} ubx_config_sats_t; + // private structures // Geodetic Position Solution @@ -140,10 +174,9 @@ struct UBX_NAV_POSLLH { int32_t hMSL; // Height above mean sea level (mm) uint32_t hAcc; // Horizontal Accuracy Estimate (mm) uint32_t vAcc; // Vertical Accuracy Estimate (mm) -}; +} __attribute__((packed)); // Receiver Navigation Status - #define STATUS_GPSFIX_NOFIX 0x00 #define STATUS_GPSFIX_DRONLY 0x01 #define STATUS_GPSFIX_2DFIX 0x02 @@ -164,7 +197,7 @@ struct UBX_NAV_STATUS { uint8_t flags2; // Additional navigation output information uint32_t ttff; // Time to first fix (ms) uint32_t msss; // Milliseconds since startup/reset (ms) -}; +} __attribute__((packed)); // Dilution of precision struct UBX_NAV_DOP { @@ -176,10 +209,9 @@ struct UBX_NAV_DOP { uint16_t hDOP; // Horizontal DOP uint16_t nDOP; // Northing DOP uint16_t eDOP; // Easting DOP -}; +} __attribute__((packed)); // Navigation solution - struct UBX_NAV_SOL { uint32_t iTOW; // GPS Millisecond Time of Week (ms) int32_t fTOW; // fractional nanoseconds (ns) @@ -198,41 +230,9 @@ struct UBX_NAV_SOL { uint8_t reserved1; // Reserved uint8_t numSV; // Number of SVs used in Nav Solution uint32_t reserved2; // Reserved -}; - -// North/East/Down velocity - -struct UBX_NAV_VELNED { - uint32_t iTOW; // ms GPS Millisecond Time of Week - int32_t velN; // cm/s NED north velocity - int32_t velE; // cm/s NED east velocity - int32_t velD; // cm/s NED down velocity - uint32_t speed; // cm/s Speed (3-D) - uint32_t gSpeed; // cm/s Ground Speed (2-D) - int32_t heading; // 1e-5 *deg Heading of motion 2-D - uint32_t sAcc; // cm/s Speed Accuracy Estimate - uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate -}; - -// UTC Time Solution - -#define TIMEUTC_VALIDTOW (1 << 0) -#define TIMEUTC_VALIDWKN (1 << 1) -#define TIMEUTC_VALIDUTC (1 << 2) - -struct UBX_NAV_TIMEUTC { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint32_t tAcc; // Time Accuracy Estimate (ns) - int32_t nano; // Nanoseconds of second - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; // Validity Flags -}; +} __attribute__((packed)); +// PVT Navigation Position Velocity Time Solution #define PVT_VALID_VALIDDATE 0x01 #define PVT_VALID_VALIDTIME 0x02 #define PVT_VALID_FULLYRESOLVED 0x04 @@ -252,7 +252,6 @@ struct UBX_NAV_TIMEUTC { #define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2) #define PVT_FLAGS_PSMSTATE_INACTIVE (5 << 2) -// PVT Navigation Position Velocity Time Solution struct UBX_NAV_PVT { uint32_t iTOW; uint16_t year; @@ -286,10 +285,39 @@ struct UBX_NAV_PVT { uint32_t reserved3; } __attribute__((packed)); +// North/East/Down velocity +struct UBX_NAV_VELNED { + uint32_t iTOW; // ms GPS Millisecond Time of Week + int32_t velN; // cm/s NED north velocity + int32_t velE; // cm/s NED east velocity + int32_t velD; // cm/s NED down velocity + uint32_t speed; // cm/s Speed (3-D) + uint32_t gSpeed; // cm/s Ground Speed (2-D) + int32_t heading; // 1e-5 *deg Heading of motion 2-D + uint32_t sAcc; // cm/s Speed Accuracy Estimate + uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate +} __attribute__((packed)); + +// UTC Time Solution +#define TIMEUTC_VALIDTOW (1 << 0) +#define TIMEUTC_VALIDWKN (1 << 1) +#define TIMEUTC_VALIDUTC (1 << 2) + +struct UBX_NAV_TIMEUTC { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint32_t tAcc; // Time Accuracy Estimate (ns) + int32_t nano; // Nanoseconds of second + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // Validity Flags +} __attribute__((packed)); + // Space Vehicle (SV) Information - // Single SV information block - #define SVUSED (1 << 0) // This SV is used for navigation #define DIFFCORR (1 << 1) // Differential correction available #define ORBITAVAIL (1 << 2) // Orbit information available @@ -308,7 +336,7 @@ struct UBX_NAV_SVINFO_SV { int8_t elev; // Elevation (integer degrees) int16_t azim; // Azimuth (integer degrees) int32_t prRes; // Pseudo range residual (cm) -}; +} __attribute__((packed)); // SV information message #define MAX_SVS 32 @@ -319,19 +347,179 @@ struct UBX_NAV_SVINFO { uint8_t globalFlags; // uint16_t reserved2; // Reserved struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times -}; +} __attribute__((packed)); // ACK message class - struct UBX_ACK_ACK { uint8_t clsID; // ClassID uint8_t msgID; // MessageID -}; +} __attribute__((packed)); struct UBX_ACK_NAK { uint8_t clsID; // ClassID uint8_t msgID; // MessageID -}; +} __attribute__((packed)); + +// Communication port information +// default cfg_prt packet at 9600,N,8,1 (from u-center) +// 0000 B5 62 06 00 14 00-01 00 +// 0008 00 00 D0 08 00 00 80 25 +// 0010 00 00 07 00 03 00 00 00 +// 0018 00 00-A2 B5 +#define UBX_CFG_PRT_PORTID_DDC 0 +#define UBX_CFG_PRT_PORTID_UART1 1 +#define UBX_CFG_PRT_PORTID_UART2 2 +#define UBX_CFG_PRT_PORTID_USB 3 +#define UBX_CFG_PRT_PORTID_SPI 4 +#define UBX_CFG_PRT_MODE_DATABITS5 0x00 +#define UBX_CFG_PRT_MODE_DATABITS6 0x40 +#define UBX_CFG_PRT_MODE_DATABITS7 0x80 +#define UBX_CFG_PRT_MODE_DATABITS8 0xC0 +#define UBX_CFG_PRT_MODE_EVENPARITY 0x000 +#define UBX_CFG_PRT_MODE_ODDPARITY 0x200 +#define UBX_CFG_PRT_MODE_NOPARITY 0x800 +#define UBX_CFG_PRT_MODE_STOPBITS1_0 0x0000 +#define UBX_CFG_PRT_MODE_STOPBITS1_5 0x1000 +#define UBX_CFG_PRT_MODE_STOPBITS2_0 0x2000 +#define UBX_CFG_PRT_MODE_STOPBITS0_5 0x3000 +#define UBX_CFG_PRT_MODE_RESERVED 0x10 + +#define UBX_CFG_PRT_MODE_DEFAULT (UBX_CFG_PRT_MODE_DATABITS8 | UBX_CFG_PRT_MODE_NOPARITY | UBX_CFG_PRT_MODE_STOPBITS1_0 | UBX_CFG_PRT_MODE_RESERVED) + +struct UBX_CFG_PRT { + uint8_t portID; // 1 or 2 for UART ports + uint8_t res0; // reserved + uint16_t res1; // reserved + uint32_t mode; // bit masks for databits, stopbits, parity, and non-zero reserved + uint32_t baudRate; // bits per second, 9600 means 9600 + uint16_t inProtoMask; // bit 0 on = UBX, bit 1 on = NEMA + uint16_t outProtoMask; // bit 0 on = UBX, bit 1 on = NEMA + uint16_t flags; // reserved + uint16_t pad; // reserved +} __attribute__((packed)); + +struct UBX_CFG_MSG { + uint8_t msgClass; + uint8_t msgID; + uint8_t rate; +} __attribute__((packed)); + +struct UBX_CFG_RATE { + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; +} __attribute__((packed)); + +// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash +#define UBX_CFG_CFG_DEVICE_BBR 0x01 +#define UBX_CFG_CFG_DEVICE_FLASH 0x02 +#define UBX_CFG_CFG_DEVICE_EEPROM 0x04 +#define UBX_CFG_CFG_DEVICE_SPIFLASH 0x10 +#define UBX_CFG_CFG_DEVICE_ALL (UBX_CFG_CFG_DEVICE_BBR | UBX_CFG_CFG_DEVICE_FLASH | UBX_CFG_CFG_DEVICE_EEPROM | UBX_CFG_CFG_DEVICE_SPIFLASH) +#define UBX_CFG_CFG_SETTINGS_NONE 0x000 +#define UBX_CFG_CFG_SETTINGS_IOPORT 0x001 +#define UBX_CFG_CFG_SETTINGS_MSGCONF 0x002 +#define UBX_CFG_CFG_SETTINGS_INFMSG 0x004 +#define UBX_CFG_CFG_SETTINGS_NAVCONF 0x008 +#define UBX_CFG_CFG_SETTINGS_TPCONF 0x010 +#define UBX_CFG_CFG_SETTINGS_SFDRCONF 0x100 +#define UBX_CFG_CFG_SETTINGS_RINVCONF 0x200 +#define UBX_CFG_CFG_SETTINGS_ANTCONF 0x400 + +#define UBX_CFG_CFG_SETTINGS_ALL (UBX_CFG_CFG_SETTINGS_IOPORT | \ + UBX_CFG_CFG_SETTINGS_MSGCONF | \ + UBX_CFG_CFG_SETTINGS_INFMSG | \ + UBX_CFG_CFG_SETTINGS_NAVCONF | \ + UBX_CFG_CFG_SETTINGS_TPCONF | \ + UBX_CFG_CFG_SETTINGS_SFDRCONF | \ + UBX_CFG_CFG_SETTINGS_RINVCONF | \ + UBX_CFG_CFG_SETTINGS_ANTCONF) + +// Sent messages for configuration support +struct UBX_CFG_CFG { + uint32_t clearMask; + uint32_t saveMask; + uint32_t loadMask; + uint8_t deviceMask; +} __attribute__((packed)); + +#define UBX_CFG_SBAS_MODE_ENABLED 0x01 +#define UBX_CFG_SBAS_MODE_TEST 0x02 +#define UBX_CFG_SBAS_USAGE_RANGE 0x01 +#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02 +#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04 + +// SBAS used satellite PNR bitmask (120-151) +// -------------------------------------1---------1---------1---------1 +// -------------------------------------5---------4---------3---------2 +// ------------------------------------10987654321098765432109876543210 +// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100 +// EGNOS 120, 124, 126, 131-------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001 +// MSAS 129, 137------------------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000 +// GAGAN 127, 128-----------------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000 +// SDCM 125, 140, 141-------------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000 + +#define UBX_CFG_SBAS_SCANMODE2 0x00 +struct UBX_CFG_SBAS { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} __attribute__((packed)); + +#define UBX_CFG_GNSS_FLAGS_ENABLED 0x000001 +#define UBX_CFG_GNSS_FLAGS_GPS_L1CA 0x010000 +#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA 0x010000 +#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I 0x010000 +#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA 0x010000 +#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF 0x040000 +#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000 + +#define UBX_CFG_GNSS_NUMCH_VER7 22 +#define UBX_CFG_GNSS_NUMCH_VER8 32 + +struct UBX_CFG_GNSS_CFGBLOCK { + uint8_t gnssId; + uint8_t resTrkCh; + uint8_t maxTrkCh; + uint8_t resvd; + uint32_t flags; +} __attribute__((packed)); + +struct UBX_CFG_GNSS { + uint8_t msgVer; + uint8_t numTrkChHw; + uint8_t numTrkChUse; + uint8_t numConfigBlocks; + struct UBX_CFG_GNSS_CFGBLOCK cfgBlocks[UBX_GNSS_ID_MAX]; +} __attribute__((packed)); + +struct UBX_CFG_NAV5 { + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint8_t cnoThreshNumSVs; + uint8_t cnoThresh; + uint16_t reserved2; + uint32_t reserved3; + uint32_t reserved4; +} __attribute__((packed)); // MON message Class #define UBX_MON_MAX_EXT 5 @@ -341,8 +529,7 @@ struct UBX_MON_VER { #if UBX_MON_MAX_EXT > 0 char extension[UBX_MON_MAX_EXT][30]; #endif -}; - +} __attribute__((packed)); // OP custom messages struct UBX_OP_SYSINFO { @@ -360,7 +547,7 @@ struct UBX_OP_MAG { int16_t y; int16_t z; uint16_t Status; -}; +} __attribute__((packed)); typedef union { uint8_t payload[0]; @@ -374,6 +561,7 @@ typedef union { struct UBX_NAV_PVT nav_pvt; struct UBX_NAV_TIMEUTC nav_timeutc; struct UBX_NAV_SVINFO nav_svinfo; + struct UBX_CFG_PRT cfg_prt; // Ack Class struct UBX_ACK_ACK ack_ack; struct UBX_ACK_NAK ack_nak; @@ -390,15 +578,40 @@ struct UBXHeader { uint16_t len; uint8_t ck_a; uint8_t ck_b; -}; +} __attribute__((packed)); struct UBXPacket { struct UBXHeader header; UBXPayload payload; -}; +} __attribute__((packed)); + +struct UBXSENTHEADER { + uint8_t prolog[2]; + uint8_t class; + uint8_t id; + uint16_t len; +} __attribute__((packed)); + +union UBXSENTPACKET { + uint8_t buffer[0]; + struct { + struct UBXSENTHEADER header; + union { + struct UBX_CFG_CFG cfg_cfg; + struct UBX_CFG_MSG cfg_msg; + struct UBX_CFG_NAV5 cfg_nav5; + struct UBX_CFG_PRT cfg_prt; + struct UBX_CFG_RATE cfg_rate; + struct UBX_CFG_SBAS cfg_sbas; + struct UBX_CFG_GNSS cfg_gnss; + } payload; + uint8_t resvd[2]; // added space for checksum bytes + } message; +} __attribute__((packed)); // Used by AutoConfig code extern int32_t ubxHwVersion; +extern GPSPositionSensorSensorTypeOptions sensorType; extern struct UBX_ACK_ACK ubxLastAck; extern struct UBX_ACK_NAK ubxLastNak; diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 0dcb13397..343b755d8 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -37,16 +37,42 @@ #define UBX_MAX_RATE_VER7 10 #define UBX_MAX_RATE 5 -// time to wait before reinitializing the fsm due to disconnection -#define UBX_CONNECTION_TIMEOUT (2000 * 1000) -// times between retries in case an error does occurs -#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000) +// reset to factory (and 9600 baud), and baud changes are not acked +// it could be 31 (full PIOS buffer) + 60 (gnss) + overhead bytes at 240 bytes per second +// = .42 seconds for the longest sentence to be fully transmitted +// and then it may have to do something like erase flash before replying... + +// at low baud rates and high data rates the ubx gps simply must drop some outgoing data +// and when a lot of data is being dropped, the MON VER reply often gets dropped +// on the other hand, uBlox documents that some versions discard data that is over a second old +// implying that it could be over 1 second before a reply is received +// 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) +#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) // max retries in case of timeout -#define UBX_MAX_RETRIES 5 -// pause between each configuration step -#define UBX_STEP_WAIT_TIME (10 * 1000) +#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) + +#define UBX_CFG_CFG_OP_STORE_SETTINGS (UBX_CFG_CFG_SETTINGS_IOPORT | \ + UBX_CFG_CFG_SETTINGS_MSGCONF | \ + UBX_CFG_CFG_SETTINGS_NAVCONF) +#define UBX_CFG_CFG_OP_CLEAR_SETTINGS ((~UBX_CFG_CFG_OP_STORE_SETTINGS) & UBX_CFG_CFG_SETTINGS_ALL) +// don't clear rinv as that is just text as configured by the owner +#define UBX_CFG_CFG_OP_RESET_SETTINGS (UBX_CFG_CFG_SETTINGS_IOPORT | \ + UBX_CFG_CFG_SETTINGS_MSGCONF | \ + UBX_CFG_CFG_SETTINGS_INFMSG | \ + UBX_CFG_CFG_SETTINGS_NAVCONF | \ + UBX_CFG_CFG_SETTINGS_TPCONF | \ + UBX_CFG_CFG_SETTINGS_SFDRCONF | \ + UBX_CFG_CFG_SETTINGS_ANTCONF) + // types typedef enum { UBX_AUTOCONFIG_STATUS_DISABLED = 0, @@ -54,26 +80,6 @@ typedef enum { UBX_AUTOCONFIG_STATUS_DONE, UBX_AUTOCONFIG_STATUS_ERROR } ubx_autoconfig_status_t; -// Enumeration options for field UBXDynamicModel -typedef enum { - UBX_DYNMODEL_PORTABLE = 0, - UBX_DYNMODEL_STATIONARY = 2, - UBX_DYNMODEL_PEDESTRIAN = 3, - UBX_DYNMODEL_AUTOMOTIVE = 4, - UBX_DYNMODEL_SEA = 5, - UBX_DYNMODEL_AIRBORNE1G = 6, - UBX_DYNMODEL_AIRBORNE2G = 7, - UBX_DYNMODEL_AIRBORNE4G = 8 -} ubx_config_dynamicmodel_t; - -typedef enum { - UBX_SBAS_SATS_AUTOSCAN = 0, - UBX_SBAS_SATS_WAAS = 1, - UBX_SBAS_SATS_EGNOS = 2, - UBX_SBAS_SATS_MSAS = 3, - UBX_SBAS_SATS_GAGAN = 4, - UBX_SBAS_SATS_SDCM = 5 -} ubx_config_sats_t; #define UBX_ typedef struct { @@ -94,142 +100,21 @@ typedef struct { bool enableBeiDou; } ubx_autoconfig_settings_t; -// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash -#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10) - // Sent messages for configuration support -typedef struct { - uint32_t clearMask; - uint32_t saveMask; - uint32_t loadMask; - uint8_t deviceMask; -} __attribute__((packed)) ubx_cfg_cfg_t; +typedef struct UBX_CFG_CFG ubx_cfg_cfg_t; +typedef struct UBX_CFG_NAV5 ubx_cfg_nav5_t; +typedef struct UBX_CFG_RATE ubx_cfg_rate_t; +typedef struct UBX_CFG_MSG ubx_cfg_msg_t; +typedef struct UBX_CFG_PRT ubx_cfg_prt_t; +typedef struct UBX_CFG_SBAS ubx_cfg_sbas_t; +typedef struct UBX_CFG_GNSS_CFGBLOCK ubx_cfg_gnss_cfgblock_t; +typedef struct UBX_CFG_GNSS ubx_cfg_gnss_t; +typedef struct UBXSENTHEADER UBXSentHeader_t; +typedef union UBXSENTPACKET UBXSentPacket_t; -typedef struct { - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint8_t cnoThreshNumSVs; - uint8_t cnoThresh; - uint16_t reserved2; - uint32_t reserved3; - uint32_t reserved4; -} __attribute__((packed)) ubx_cfg_nav5_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(); -typedef struct { - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; -} __attribute__((packed)) ubx_cfg_rate_t; - -typedef struct { - uint8_t msgClass; - uint8_t msgID; - uint8_t rate; -} __attribute__((packed)) ubx_cfg_msg_t; - -#define UBX_CFG_SBAS_MODE_ENABLED 0x01 -#define UBX_CFG_SBAS_MODE_TEST 0x02 -#define UBX_CFG_SBAS_USAGE_RANGE 0x01 -#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02 -#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04 - -// SBAS used satellite PNR bitmask (120-151) -// -------------------------------------1---------1---------1---------1 -// -------------------------------------5---------4---------3---------2 -// ------------------------------------10987654321098765432109876543210 -// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100 -// EGNOS 120, 124, 126, 131-------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001 -// MSAS 129, 137------------------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000 -// GAGAN 127, 128-----------------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000 -// SDCM 125, 140, 141-------------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000 - -#define UBX_CFG_SBAS_SCANMODE2 0x00 -typedef struct { - uint8_t mode; - uint8_t usage; - uint8_t maxSBAS; - uint8_t scanmode2; - uint32_t scanmode1; -} __attribute__((packed)) ubx_cfg_sbas_t; - -typedef enum { - UBX_GNSS_ID_GPS = 0, - UBX_GNSS_ID_SBAS = 1, - UBX_GNSS_ID_GALILEO = 2, - UBX_GNSS_ID_BEIDOU = 3, - UBX_GNSS_ID_IMES = 4, - UBX_GNSS_ID_QZSS = 5, - UBX_GNSS_ID_GLONASS = 6, - UBX_GNSS_ID_MAX -} ubx_config_gnss_id_t; - -#define UBX_CFG_GNSS_FLAGS_ENABLED 0x01 -#define UBX_CFG_GNSS_FLAGS_GPS_L1CA 0x010000 -#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA 0x010000 -#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I 0x010000 -#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA 0x010000 -#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF 0x040000 -#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000 - -#define UBX_CFG_GNSS_NUMCH_VER7 22 -#define UBX_CFG_GNSS_NUMCH_VER8 32 - -typedef struct { - uint8_t gnssId; - uint8_t resTrkCh; - uint8_t maxTrkCh; - uint8_t resvd; - uint32_t flags; -} __attribute__((packed)) ubx_cfg_gnss_cfgblock_t; - -typedef struct { - uint8_t msgVer; - uint8_t numTrkChHw; - uint8_t numTrkChUse; - uint8_t numConfigBlocks; - ubx_cfg_gnss_cfgblock_t cfgBlocks[UBX_GNSS_ID_MAX]; -} __attribute__((packed)) ubx_cfg_gnss_t; - -typedef struct { - uint8_t prolog[2]; - uint8_t class; - uint8_t id; - uint16_t len; -} __attribute__((packed)) UBXSentHeader_t; - -typedef union { - uint8_t buffer[0]; - struct { - UBXSentHeader_t header; - union { - ubx_cfg_cfg_t cfg_cfg; - ubx_cfg_msg_t cfg_msg; - ubx_cfg_nav5_t cfg_nav5; - ubx_cfg_rate_t cfg_rate; - ubx_cfg_sbas_t cfg_sbas; - ubx_cfg_gnss_t cfg_gnss; - } payload; - uint8_t resvd[2]; // added space for checksum bytes - } message; -} __attribute__((packed)) UBXSentPacket_t; - -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected); -void ubx_autoconfig_set(ubx_autoconfig_settings_t config); 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 c86fbb445..5fa036596 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -27,29 +27,48 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include +#include "hwsettings.h" + #include "inc/ubx_autoconfig.h" #include + // private type definitions + typedef enum { INIT_STEP_DISABLED = 0, INIT_STEP_START, - INIT_STEP_ASK_VER, - INIT_STEP_WAIT_VER, + INIT_STEP_RESET_GPS, + INIT_STEP_REVO_9600_BAUD, + INIT_STEP_GPS_BAUD, + INIT_STEP_REVO_BAUD, INIT_STEP_ENABLE_SENTENCES, INIT_STEP_ENABLE_SENTENCES_WAIT_ACK, INIT_STEP_CONFIGURE, INIT_STEP_CONFIGURE_WAIT_ACK, + INIT_STEP_SAVE, + INIT_STEP_SAVE_WAIT_ACK, INIT_STEP_DONE, - INIT_STEP_ERROR, + INIT_STEP_ERROR } initSteps_t; typedef struct { initSteps_t currentStep; // Current configuration "fsm" status + initSteps_t currentStepSave; // Current configuration "fsm" status uint32_t lastStepTimestampRaw; // timestamp of last operation uint32_t lastConnectedRaw; // timestamp of last time gps was connected - UBXSentPacket_t working_packet; // outbound "buffer" - ubx_autoconfig_settings_t currentSettings; - int8_t lastConfigSent; // index of last configuration string sent + 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+ + } __attribute__((packed)); + volatile ubx_autoconfig_settings_t currentSettings; + 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; @@ -114,15 +133,24 @@ ubx_cfg_msg_t msg_config_ubx7[] = { }; // private defines + #define LAST_CONFIG_SENT_START (-1) #define LAST_CONFIG_SENT_COMPLETED (-2) +// always reset the stored GPS configuration, even when doing autoconfig.nostore +// that is required to do a 100% correct configuration +// but is unexpected because it changes the stored configuration when doing autoconfig.nostore +// note that a reset is always done with autoconfig.store +//#define ALWAYS_RESET // private variables // enable the autoconfiguration system -static bool enabled; +static volatile bool enabled = false; +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 status_t *status = 0; static void append_checksum(UBXSentPacket_t *packet) { @@ -139,29 +167,152 @@ static void append_checksum(UBXSentPacket_t *packet) packet->buffer[len] = ck_a; packet->buffer[len + 1] = ck_b; } + + /** * prepare a packet to be sent, fill the header and appends the checksum. * return the total packet lenght comprising header and checksum */ static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len) { + memset((uint8_t *)status->working_packet.buffer + len + sizeof(UBXSentHeader_t) + 2, 0, sizeof(status->bufferPaddingForPiosBugAt2400Baud)); packet->message.header.prolog[0] = UBX_SYNC1; packet->message.header.prolog[1] = UBX_SYNC2; packet->message.header.class = classID; packet->message.header.id = messageID; packet->message.header.len = len; append_checksum(packet); - return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum + + status->requiredAck.clsID = classID; + status->requiredAck.msgID = messageID; + + return (len + sizeof(UBXSentHeader_t) + 2 + sizeof(status->bufferPaddingForPiosBugAt2400Baud)); // payload + header + checksum + extra bytes } + static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send) { *bytes_to_send = prepare_packet(packet, classID, messageID, 0); } -void config_rate(uint16_t *bytes_to_send) + +static void set_current_step_if_untouched(initSteps_t new_steps) { - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t)); + // assume this one byte initSteps_t is atomic + // take care of some but not all concurrency issues + + if (!current_step_touched) { + status->currentStep = new_steps; + } + if (current_step_touched) { + status->currentStep = status->currentStepSave; + } +} + + +void ubx_reset_sensor_type() +{ + ubxHwVersion = -1; + sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; + GPSPositionSensorSensorTypeSet((uint8_t *) &sensorType); +} + + +static void config_reset(uint16_t *bytes_to_send) +{ + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t)); + // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB + // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400 + // first: reset (permanent settings to default) all but rinv = e.g. owner name + status->working_packet.message.payload.cfg_cfg.clearMask = UBX_CFG_CFG_OP_RESET_SETTINGS; + // then: don't store any current settings to permanent + status->working_packet.message.payload.cfg_cfg.saveMask = UBX_CFG_CFG_SETTINGS_NONE; + // lastly: load (immediately start to use) all but rinv = e.g. owner name + status->working_packet.message.payload.cfg_cfg.loadMask = UBX_CFG_CFG_OP_RESET_SETTINGS; + // all devices + status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); +} + + +// set the GPS baud rate to the user specified baud rate +// because we may have started up with 9600 baud (for a GPS with no permanent settings) +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 + 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) + // 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; + } + + *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)); // if rate is less than 1 uses the highest rate for current hardware uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99; if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) { @@ -172,36 +323,31 @@ void config_rate(uint16_t *bytes_to_send) rate = UBX_MAX_RATE_VER8; } uint16_t period = 1000 / rate; - status->working_packet.message.payload.cfg_rate.measRate = period; status->working_packet.message.payload.cfg_rate.navRate = 1; // must be set to 1 status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_RATE; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); } -void config_nav(uint16_t *bytes_to_send) -{ - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t)); +static void config_nav(uint16_t *bytes_to_send) +{ + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t)); status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel; status->working_packet.message.payload.cfg_nav5.fixMode = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D // mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB - status->working_packet.message.payload.cfg_nav5.mask = 0x01 + 0x04; // Dyn Model | posFixMode configuration - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_NAV5; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t)); } -void config_sbas(uint16_t *bytes_to_send) + +static void config_sbas(uint16_t *bytes_to_send) { - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t)); - - status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ? - status->currentSettings.SBASChannelsUsed : 3; - + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t)); + status->working_packet.message.payload.cfg_sbas.maxSBAS = + status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3; status->working_packet.message.payload.cfg_sbas.usage = (status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) | (status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) | @@ -209,34 +355,28 @@ void config_sbas(uint16_t *bytes_to_send) // If sbas is used for anything then set mode as enabled status->working_packet.message.payload.cfg_sbas.mode = status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0; - status->working_packet.message.payload.cfg_sbas.scanmode1 = status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS : status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS : status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS : status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN : status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN; + status->working_packet.message.payload.cfg_sbas.scanmode2 = + UBX_CFG_SBAS_SCANMODE2; - status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2; - - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_SBAS; + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t)); } -void config_gnss(uint16_t *bytes_to_send) + +static void config_gnss(uint16_t *bytes_to_send) { - int32_t i; - - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t)); - + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t)); status->working_packet.message.payload.cfg_gnss.numConfigBlocks = UBX_GNSS_ID_MAX; status->working_packet.message.payload.cfg_gnss.numTrkChHw = (ubxHwVersion > UBX_HW_VERSION_7) ? UBX_CFG_GNSS_NUMCH_VER8 : UBX_CFG_GNSS_NUMCH_VER7; status->working_packet.message.payload.cfg_gnss.numTrkChUse = status->working_packet.message.payload.cfg_gnss.numTrkChHw; - for (i = 0; i < UBX_GNSS_ID_MAX; i++) { + for (int32_t i = 0; i < UBX_GNSS_ID_MAX; i++) { status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].gnssId = i; - switch (i) { case UBX_GNSS_ID_GPS: if (status->currentSettings.enableGPS) { @@ -278,30 +418,35 @@ void config_gnss(uint16_t *bytes_to_send) } } - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_GNSS; + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t)); } -void config_save(uint16_t *bytes_to_send) + +static void config_save(uint16_t *bytes_to_send) { - memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t)); + memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t)); // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB - status->working_packet.message.payload.cfg_cfg.saveMask = 0x02 | 0x08; // msgConf + navConf - status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK; - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_CFG; + // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400 + status->working_packet.message.payload.cfg_cfg.saveMask = UBX_CFG_CFG_OP_STORE_SETTINGS; // a list of settings we just set + status->working_packet.message.payload.cfg_cfg.clearMask = UBX_CFG_CFG_OP_CLEAR_SETTINGS; // everything else gets factory default + status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; + + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); } + + static void configure(uint16_t *bytes_to_send) { switch (status->lastConfigSent) { case LAST_CONFIG_SENT_START: + // increase message rates to 5 fixes per second config_rate(bytes_to_send); break; + case LAST_CONFIG_SENT_START + 1: config_nav(bytes_to_send); break; + case LAST_CONFIG_SENT_START + 2: if (status->currentSettings.enableGLONASS || status->currentSettings.enableGPS) { config_gnss(bytes_to_send); @@ -310,24 +455,19 @@ static void configure(uint16_t *bytes_to_send) // Skip and fall through to next step status->lastConfigSent++; } + // in the else case we must fall through because we must send something each time because successful send is tested externally + case LAST_CONFIG_SENT_START + 3: config_sbas(bytes_to_send); - if (!status->currentSettings.storeSettings) { - // skips saving - status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; - } break; - case LAST_CONFIG_SENT_START + 4: - config_save(bytes_to_send); - status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; - return; default: status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; - return; + break; } } + static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) { int8_t msg = status->lastConfigSent + 1; @@ -338,66 +478,303 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) if (msg >= 0 && msg < msg_count) { status->working_packet.message.payload.cfg_msg = msg_config[msg]; - - *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t)); - status->requiredAck.clsID = UBX_CLASS_CFG; - status->requiredAck.msgID = UBX_ID_CFG_MSG; + *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t)); } else { status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; } } -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected) + +// End User Documentation + +// There are two baud rates of interest +// The baud rate the GPS is talking at +// The baud rate Revo or CC/3D 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 + +// There are microprocessors and they each have internal settings +// Revo +// GPS +// and each of these settings can be temporary or permanent + +// 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 (or CC/3D) 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 + + +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) { *bytes_to_send = 0; *buffer = (char *)status->working_packet.buffer; - if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) { - return; // autoconfig not enabled - } - if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) { + current_step_touched = false; + + // autoconfig struct not yet allocated + if (!status) { return; } - // when gps is disconnected it will replay the autoconfig sequence. - if (!gps_connected) { - if (status->currentStep == INIT_STEP_DONE) { - // if some operation is in progress and it is not running into issues it maybe that - // the disconnection is only due to wrong message rates, so reinit only when done. - // errors caused by disconnection are handled by error retry logic - if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) { - status->currentStep = INIT_STEP_START; - 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 + 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 + // and when a lot of data is being dropped, the MON VER reply often gets dropped + // on the other hand, uBlox documents that some versions discard data that is over 1 second old + // implying a 1 second send buffer and that it could be over 1 second before a reply is received + // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full + // and that could be even longer than 1 second + // 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) { + return; } - } else { - // reset connection timer - status->lastConnectedRaw = PIOS_DELAY_GetRaw(); + 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 + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + 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; } switch (status->currentStep) { - case INIT_STEP_ERROR: - case INIT_STEP_DISABLED: - case INIT_STEP_DONE: - return; - case INIT_STEP_START: - case INIT_STEP_ASK_VER: - status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); - status->currentStep = INIT_STEP_WAIT_VER; + // 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(); break; - case INIT_STEP_WAIT_VER: - if (ubxHwVersion > 0) { - status->lastConfigSent = LAST_CONFIG_SENT_START; - status->currentStep = INIT_STEP_ENABLE_SENTENCES; + 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 + // and changing the baud will screw it up + // when the GPS is configured to send a lot of data, but has a low baud rate + // it has way too many messages to send and has to drop most of them + + // Retrieve desired GPS baud rate once for use throughout this module + HwSettingsGPSSpeedGet(&hwsettings_baud); +#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 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) +#endif + { + // reset all GPS parameters to factory default (configure low rate NEMA 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); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + // else allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + set_current_step_if_untouched(INIT_STEP_REVO_9600_BAUD); + break; + + 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 + // 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) +#endif + { + // wait for previous step + 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); + } + // at most, we just set Revo baud and that doesn't send any data + // 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_GPS_BAUD); + // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + // break; + + 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 + // affect baud rate and other transmission parameters. Because there may be messages queued for transmission + // there may be uncertainty about which protocol applies to such messages. In addition a message currently in + // transmission may be corrupted by a protocol change. Host data reception parameters may have to be changed to + // be able to receive future messages, including the acknowledge message associated with the UBX-CFG-CFG message. + + // so the message that changes the baud rate will send it's acknowledgement back at the new baud rate; this is not good. + // if your message was corrupted, you didn't change the baud rate and you have to guess; try pinging at both baud rates. + // also, you would have to change the baud rate instantly after the last byte of the sentence was sent, + // and you would have to poll the port in real time for that, and there may be messages ahead of the baud rate change. + // + // so we ignore the ack from this. it has proven to be reliable (with the addition of two dummy bytes after the packet) + + // set the GPS internal baud rate to the user configured value + config_gps_baud(bytes_to_send); + set_current_step_if_untouched(INIT_STEP_REVO_BAUD); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + break; + + case INIT_STEP_REVO_BAUD: + // wait for previous step + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { return; } - - if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) { - status->currentStep = INIT_STEP_ASK_VER; + // set the Revo GPS port baud rate to the (same) user configured value + config_baud(hwsettings_baud); + status->lastConfigSent = LAST_CONFIG_SENT_START; + 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 + if (hwsettings_baud <= HWSETTINGS_GPSSPEED_9600) { + set_current_step_if_untouched(INIT_STEP_SAVE); + } else { + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); } - return; + // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + break; case INIT_STEP_ENABLE_SENTENCES: case INIT_STEP_CONFIGURE: @@ -409,61 +786,136 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec enable_sentences(bytes_to_send); } - status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + // for some branches, allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { - status->lastConfigSent = LAST_CONFIG_SENT_START; - status->currentStep = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE; + if (step_configure) { + // zero retries for the next state that needs it (INIT_STEP_SAVE) + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_SAVE); + } else { + // finished enabling sentences, now configure() needs to start at the beginning + status->lastConfigSent = LAST_CONFIG_SENT_START; + set_current_step_if_untouched(INIT_STEP_CONFIGURE); + } } else { - status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK; + set_current_step_if_untouched(step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); } - return; + break; } + case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK: case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS { bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK); if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) { // Continue with next configuration option + // start retries over for the next setting to be sent status->retryCount = 0; status->lastConfigSent++; - } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT || - (ubxLastNak.clsID == status->requiredAck.clsID && ubxLastNak.msgID == status->requiredAck.msgID)) { + } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_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) { - status->currentStep = INIT_STEP_ERROR; + set_current_step_if_untouched(INIT_STEP_ERROR); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - return; + break; } } - // no abort condition, continue or retries., + // success or failure here, retries are handled elsewhere if (step_configure) { - status->currentStep = INIT_STEP_CONFIGURE; + set_current_step_if_untouched(INIT_STEP_CONFIGURE); } else { - status->currentStep = INIT_STEP_ENABLE_SENTENCES; + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); } - return; + break; } + + case INIT_STEP_SAVE: + if (status->currentSettings.storeSettings) { + 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(); + } + 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 + 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); + } + } + break; + + case INIT_STEP_ERROR: + // on error we should get the GPS version immediately + ubx_reset_sensor_type(); + // fall through + case INIT_STEP_DISABLED: + case INIT_STEP_DONE: + break; } } -void ubx_autoconfig_set(ubx_autoconfig_settings_t config) + +void ubx_autoconfig_set(ubx_autoconfig_settings_t *config) { + initSteps_t new_step; + enabled = false; - if (config.autoconfigEnabled) { - if (!status) { - status = (status_t *)pios_malloc(sizeof(status_t)); - PIOS_Assert(status); - memset(status, 0, sizeof(status_t)); - status->currentStep = INIT_STEP_DISABLED; - } - status->currentSettings = config; - status->currentStep = INIT_STEP_START; + + if (!status) { + status = (status_t *)pios_malloc(sizeof(status_t)); + PIOS_Assert(status); + memset((status_t *)status, 0, sizeof(status_t)); + } + + // if caller used NULL, just use current settings to restart autoconfig process + if (config != NULL) { + status->currentSettings = *config; + } + if (status->currentSettings.autoconfigEnabled) { + new_step = INIT_STEP_START; + } else { + new_step = INIT_STEP_DISABLED; + } + + // assume this one byte initSteps_t is atomic + // take care of some but not all concurrency issues + + status->currentStep = new_step; + status->currentStepSave = new_step; + current_step_touched = true; + status->currentStep = new_step; + status->currentStepSave = new_step; + + if (status->currentSettings.autoconfigEnabled) { enabled = true; - } else { - if (status) { - status->currentStep = INIT_STEP_DISABLED; - } } } From f331ca0daf86b9b3a8e635dcadc58d23d9a7975c Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Thu, 30 Apr 2015 01:27:19 -0400 Subject: [PATCH 2/5] OP-1840 CC3D wouldn't build and update some comments --- flight/modules/GPS/GPS.c | 69 ++++++++++++++-------------------------- 1 file changed, 23 insertions(+), 46 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index a7671e714..794fd30a4 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -244,27 +244,6 @@ MODULE_INITCALL(GPSInitialize, GPSStart); /** * Main gps task. It does not return. */ -enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, -}; - -struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t lower_id; - const struct pios_com_driver *driver; - -#if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreHandle tx_sem; - xSemaphoreHandle rx_sem; - xSemaphoreHandle sendbuffer_sem; -#endif - - bool has_rx; - bool has_tx; - - t_fifo_buffer rx; - t_fifo_buffer tx; -}; static void gpsTask(__attribute__((unused)) void *parameters) { @@ -472,11 +451,13 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) // 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 my main reason for coding this: cheap eBay GPS's that loose their settings sometimes + // - - 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 @@ -496,45 +477,39 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) // - - 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 for autoconfig-nostore/store + // 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) - // - should the 57600 be fixed as part of autoconfig-store/nostore and the HwSettings.GPSSpeed be the initial baud rate? + // - 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 startup and only if you NEVER enable and store (even once) OP messages (a lot more data) at 9600 - // - - using autoconfig-store just one time at 9600 will pretty much force you to use 57600 + // - - but 9600 is only OK for this autoconfig startup // - by my testing, the 9600 initial to 57600 final baud startup takes: - // - - 1:13 if the GPS has been configured to send OP data at 9600 - // - - 0:12 if the GPS has default data settings (NEMA) - // - - reminder that even 1:13 isn't that bad. You need to wait for the GPS to acquire satellites, - // - - and it does that the whole time it is being configured + // - - 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 know if you want to play around with this: - // - 57600 baud is what you want stored inside the GPS settings if you autoconfig-store it even once at any baud rate - // - - then use autoconfig-disabled once the GPS settings are stored in the GPS - // - don't play with low baud rates, with OP data settings (lots of data) at best it takes a long time to auto-configure - // - - at 19200 or lower it will fail to autoconfig and set the status to 'ERROR' - // - - at 19200 or lower the autoconfig is skipped and only the baud rate is changed - // - if you autoconfigure-store an OP configuration at 19200 baud or lower - // - - it will actually store a factory default configuration at that baud rate - // - - you will probably have to use u-center to fix it - // - - default settings (NEMA) at 9600 are OK because there is not nearly as much data as the OP config uses, + // 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-nostore + // - - 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 - // my OP GPS #2 will NOT save 115200 baud or higher, but it will use all bauds, even 230400 - - // since 9600 baud and lower are not usable, and are best left at NEMA, I could / should / will code it to do a factory reset - // - if set to 9600 baud (or lower???) + // 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) { - // static uint8_t old_speed = HWSETTINGS_GPSSPEED_230400+1; uint8_t speed; #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) @@ -587,9 +562,11 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) PIOS_COM_ChangeBaud(gpsPort, 230400); break; } +#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(); +#endif } #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) else { From c365bd30329e336581d3d490283559445dbd2567 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 1 May 2015 02:46:38 -0400 Subject: [PATCH 3/5] OP-1840 Removed unused debug .h and update comments --- flight/modules/GPS/GPS.c | 4 ++-- flight/modules/GPS/ubx_autoconfig.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 794fd30a4..cbd5ba2b4 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -50,7 +50,7 @@ #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" +//#include "../../libraries/inc/fifo_buffer.h" #endif #include @@ -535,7 +535,7 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX) #endif { - // Set Revo port speed + // Set Revo / Nano port speed switch (speed) { case HWSETTINGS_GPSSPEED_2400: PIOS_COM_ChangeBaud(gpsPort, 2400); diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 5fa036596..15f17f610 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -489,7 +489,7 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) // There are two baud rates of interest // The baud rate the GPS is talking at -// The baud rate Revo or CC/3D 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 @@ -547,7 +547,7 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) // 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 (or CC/3D) port setting +// 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 From dad8f40c4d72830b79c4447aa624f1d6a7c244b5 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 1 May 2015 02:49:15 -0400 Subject: [PATCH 4/5] OP-1840 Remove reference to unreleased hardware --- flight/modules/GPS/GPS.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index cbd5ba2b4..438177f6f 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -535,7 +535,7 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX) #endif { - // Set Revo / Nano port speed + // Set Revo port speed switch (speed) { case HWSETTINGS_GPSSPEED_2400: PIOS_COM_ChangeBaud(gpsPort, 2400); From 193911ba20725259d2410c2d5251ecfe4348d233 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 5 May 2015 23:26:08 -0400 Subject: [PATCH 5/5] OP-1840 another merge conflict --- flight/modules/GPS/GPS.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index e2e2ee8be..a0a12b4c3 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -157,10 +157,6 @@ int32_t GPSStart(void) int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; -<<<<<<< HEAD -======= - GPSSettingsDataProtocolOptions gpsProtocol; ->>>>>>> next HwSettingsInitialize(); #ifdef MODULE_GPS_BUILTIN