rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Merge branch 'rel-nano-15.05' into abeck/OP-1898-VR

Browse Source
This commit is contained in:
abeck70 2015-05-22 18:46:09 +10:00
commit ee43c53cee
13 changed files with 631 additions and 570 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -50,7 +50,6 @@
#include "UBX.h"
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
#include "inc/ubx_autoconfig.h"
// #include "../../libraries/inc/fifo_buffer.h"
#endif
#include <pios_instrumentation_helper.h>
@ -60,8 +59,8 @@ PERF_DEFINE_COUNTER(counterParse);
// ****************
// Private functions
static void gpsTask(void *parameters);
static void updateHwSettings(UAVObjEvent *ev);
static void gpsTask(__attribute__((unused)) void *parameters);
static void updateHwSettings(__attribute__((unused)) UAVObjEvent *ev);
#ifdef PIOS_GPS_SETS_HOMELOCATION
static void setHomeLocation(GPSPositionSensorData *gpsData);
@ -69,9 +68,9 @@ static float GravityAccel(float latitude, float longitude, float altitude);
#endif
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
void AuxMagSettingsUpdatedCb(UAVObjEvent *ev);
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#ifndef PIOS_GPS_MINIMAL
void updateGpsSettings(UAVObjEvent *ev);
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
#endif
#endif
@ -114,7 +113,7 @@ void updateGpsSettings(UAVObjEvent *ev);
static GPSSettingsData gpsSettings;
static uint32_t gpsPort;
#define gpsPort PIOS_COM_GPS
static bool gpsEnabled = false;
static xTaskHandle gpsTaskHandle;
@ -138,12 +137,10 @@ static struct GPS_RX_STATS gpsRxStats;
int32_t GPSStart(void)
{
if (gpsEnabled) {
if (gpsPort) {
// Start gps task
xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle);
return 0;
}
// Start gps task
xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle);
return 0;
}
return -1;
}
@ -156,8 +153,6 @@ int32_t GPSStart(void)
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
HwSettingsInitialize();
#ifdef MODULE_GPS_BUILTIN
gpsEnabled = true;
@ -197,7 +192,7 @@ int32_t GPSInitialize(void)
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
updateHwSettings(0);
#else /* if defined(REVOLUTION) */
if (gpsPort && gpsEnabled) {
if (gpsEnabled) {
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
#if !defined(PIOS_GPS_MINIMAL)
@ -215,7 +210,7 @@ int32_t GPSInitialize(void)
}
#endif /* if defined(REVOLUTION) */
if (gpsPort && gpsEnabled) {
if (gpsEnabled) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
@ -247,7 +242,13 @@ MODULE_INITCALL(GPSInitialize, GPSStart);
static void gpsTask(__attribute__((unused)) void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
// 57600 baud = 5760 bytes per second
// PIOS serial buffers appear to be 32 bytes long
// that is a maximum of 5760/32 = 180 buffers per second
// that is 1/180 = 0.005555556 seconds per packet
// we must never wait more than 5ms since packet was last drained or it may overflow
// 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible
portTickType xDelay = 5 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
#ifdef PIOS_GPS_SETS_HOMELOCATION
@ -273,115 +274,118 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// Loop forever
while (1) {
if (gpsPort) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0;
uint16_t count = 0;
// do autoconfig stuff for UBX GPS's
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0;
uint16_t count = 0;
ubx_autoconfig_run(&buffer, &count);
// Something to send?
if (count) {
// clear ack/nak
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
ubxLastNak.clsID = 0x00;
ubxLastNak.msgID = 0x00;
gps_ubx_autoconfig_run(&buffer, &count);
// Something to send?
if (count) {
// clear ack/nak
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
ubxLastNak.clsID = 0x00;
ubxLastNak.msgID = 0x00;
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
}
}
}
// need to do this whether there is received data or not, or the status (e.g. gcs) is not always correct
int32_t ac_status = ubx_autoconfig_get_status();
static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
gpspositionsensor.AutoConfigStatus =
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
lastStatus = gpspositionsensor.AutoConfigStatus;
}
// need to do this whether there is received data or not, or the status (e.g. gcs) will not always be correct
int32_t ac_status = ubx_autoconfig_get_status();
static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
gpspositionsensor.AutoConfigStatus =
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
lastStatus = gpspositionsensor.AutoConfigStatus;
}
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
// This blocks the task until there is something on the buffer (or 100ms? passes)
uint16_t cnt;
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
if (cnt > 0) {
PERF_TIMED_SECTION_START(counterParse);
PERF_TRACK_VALUE(counterBytesIn, cnt);
PERF_MEASURE_PERIOD(counterRate);
int res;
switch (gpsSettings.DataProtocol) {
uint16_t cnt;
// This blocks the task until there is something on the buffer (or 100ms? passes)
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
if (cnt > 0) {
PERF_TIMED_SECTION_START(counterParse);
PERF_TRACK_VALUE(counterBytesIn, cnt);
PERF_MEASURE_PERIOD(counterRate);
int res;
switch (gpsSettings.DataProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA:
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
case GPSSETTINGS_DATAPROTOCOL_NMEA:
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case GPSSETTINGS_DATAPROTOCOL_UBX:
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
case GPSSETTINGS_DATAPROTOCOL_UBX:
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
default:
res = NO_PARSER; // this should not happen
break;
default:
res = NO_PARSER; // this should not happen
break;
}
PERF_TIMED_SECTION_END(counterParse);
if (res == PARSER_COMPLETE) {
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
PERF_TIMED_SECTION_END(counterParse);
if (res == PARSER_COMPLETE) {
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
// we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
// we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
#ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationData home;
HomeLocationGet(&home);
HomeLocationData home;
HomeLocationGet(&home);
if (home.Set == HOMELOCATION_SET_FALSE) {
if (homelocationSetDelay == 0) {
homelocationSetDelay = xTaskGetTickCount();
}
if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) {
setHomeLocation(&gpspositionsensor);
if (home.Set == HOMELOCATION_SET_FALSE) {
if (homelocationSetDelay == 0) {
homelocationSetDelay = xTaskGetTickCount();
}
if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) {
setHomeLocation(&gpspositionsensor);
homelocationSetDelay = 0;
}
} else {
homelocationSetDelay = 0;
}
} else {
homelocationSetDelay = 0;
}
#endif
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
}
} // if (gpsPort)
vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
}
} // while (1)
}
#ifdef PIOS_GPS_SETS_HOMELOCATION
@ -436,6 +440,60 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
}
#endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */
uint32_t hwsettings_gpsspeed_enum_to_baud(uint8_t baud)
{
switch (baud) {
case HWSETTINGS_GPSSPEED_2400:
return 2400;
case HWSETTINGS_GPSSPEED_4800:
return 4800;
default:
case HWSETTINGS_GPSSPEED_9600:
return 9600;
case HWSETTINGS_GPSSPEED_19200:
return 19200;
case HWSETTINGS_GPSSPEED_38400:
return 38400;
case HWSETTINGS_GPSSPEED_57600:
return 57600;
case HWSETTINGS_GPSSPEED_115200:
return 115200;
case HWSETTINGS_GPSSPEED_230400:
return 230400;
}
}
// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
void gps_set_fc_baud_from_arg(uint8_t baud)
{
static uint8_t previous_baud = 255;
static uint8_t mutex; // = 0
// do we need this?
// can the code stand having two tasks/threads do an XyzSet() call at the same time?
if (__sync_fetch_and_add(&mutex, 1) == 0) {
// don't bother doing the baud change if it is actually the same
// might drop less data
if (previous_baud != baud) {
previous_baud = baud;
// Set Revo port hwsettings_baud
PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud));
GPSPositionSensorBaudRateSet(&baud);
}
}
--mutex;
}
/**
* Update the GPS settings, called on startup.
* FIXME: This should be in the GPSSettings object. But objects have
@ -447,161 +505,72 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
{
// with these changes, no one (not even OP board makers) should ever need u-center (the complex UBlox configuration program)
// no booting of Revo should be required during any setup or testing, just Send the settings you want to play with
// with autoconfig enabled, just change the baud rate in HwSettings and it will change the GPS internal baud and then the Revo port
// with autoconfig disabled, it will only change the Revo port, so you can try to find the current GPS baud rate if you don't know it
// GPSPositionSensor.SensorType and .UbxAutoConfigStatus now give correct values at all times, so use .SensorType to prove it is
// connected, even to an incorrectly configured (e.g. factory default) GPS
// Use cases:
// - the user can plug in a default GPS, use autoconfig-store once and then always use autoconfig-disabled
// - the user can plug in a default GPS that can't store settings (defaults to 9600 baud) and always use autoconfig-nostore
// - - this is one reason for coding this: cheap eBay GPS's that loose their settings sometimes
// - the user can plug in a default GPS that _can_ store settings and always use autoconfig-nostore with that too
// - - if settings change in newer releases of OP, this will make sure to use the correct settings with whatever code you are running
// - the user can plug in a correctly configured GPS and use autoconfig-disabled
// - the user can recover an old or incorrectly configured GPS (internal GPS baud rate or other GPS settings wrong)
// - - disable UBX GPS autoconfig
// - - set HwSettings GPS baud rate to the current GPS internal baud rate to get it to connect at current (especially non-9600) baud rate
// - - - you can tell the baud rate is correct when GPSPositionSensor.SensorType is known (not "Unknown") (e.g. UBX6)
// - - - the SensorType and AutoConfigStatus may fail at 9600 and will fail at 4800 or lower if the GPS is configured to send OP data
// - - - (way more than default) at 4800 baud or slower
// - - enable autoconfig-nostore GPSSettings to set correct messages and enable further magic baud rate settings
// - - change the baud rate to what you want in HwSettings (it will change the baud rate in the GPS and then in the Revo port)
// - - wait for the baud rate change to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
// - - enable autoconfig-store and it will save the new baud rate and the correct message configuration
// - - wait for the store to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
// - - for the new baud rate, the user should either choose 9600 for use with autoconfig-nostore or can choose 57600 for use with autoconfig-disabled
// - the user (Dave :)) can configure a whole bunch of default GPS's with no intervention by using autoconfig-store as a saved Revo setting
// - - just plug the default (9600 baud) GPS in to an unpowered Revo, power the Revo/GPS through the servo connector, wait some time, unplug
// - - or with this code, OP could and even should just ship GPS's with default settings
// if we add an "initial baud rate" instead of assuming 9600 at boot up for autoconfig-nostore/store
// - the user can use the same GPS with both an older release of OP (e.g. GPS settings at 38400) and the current release (autoconfig-nostore)
// - the 57600 could be fixed instead of the 9600 as part of autoconfig-store/nostore and the HwSettings.GPSSpeed be the initial baud rate?
// About using UBlox GPS's with default settings (9600 baud and NEMA data):
// - the default uBlox settings (different than OP settings) are NEMA and 9600 baud
// - - and that is OK and you use autoconfig-nostore
// - - I personally feel that this is the best way to set it up because if OP dev changes GPS settings,
// - - you get them automatically changed to match whatever version of OP code you are running
// - - but 9600 is only OK for this autoconfig startup
// - by my testing, the 9600 initial to 57600 final baud startup takes:
// - - 0:10 if the GPS has been configured to send OP data at 9600
// - - 0:06 if the GPS has default data settings (NEMA) at 9600
// - - reminder that even 0:10 isn't that bad. You need to wait for the GPS to acquire satellites,
// Some things you want to think about if you want to play around with this:
// - don't play with low baud rates, with OP data settings (lots of data) it can take a long time to auto-configure
// - - at 2400 it could take a long time to autoconfig or error out
// - - at 9600 or lower the autoconfig is skipped and only the baud rate is changed
// - if you autoconfigure an OP configuration at 9600 baud or lower
// - - it will actually make a factory default configuration (not an OP configuration) at that baud rate
// - remember that there are two baud rates (inside the GPS and the Revo port) and you can get them out of sync
// - - rebooting the Revo from the Ground Control Station (without powering the GPS down too)
// - - can leave the baud rates out of sync if you are using autoconfig
// - - just power off and on both the Revo and the GPS
// - my OP GPS #2 will NOT save 115200 baud or higher, but it will use all bauds, even 230400
// - - so you could use autoconfig.nostore with this high baud rate, but not autoconfig.store (once) followed by autoconfig.disabled
// - - I haven't tested other GPS's in regard to this
// since 9600 baud and lower are not usable, and are best left at NEMA, I could have coded it to do a factory reset
// - if set to 9600 baud (or lower)
if (gpsPort) {
HwSettingsGPSSpeedOptions speed;
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// use 9600 baud during startup if autoconfig is enabled
// that is the flag that the code should assumes a factory default baud rate
if (ev == NULL && gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) {
speed = HWSETTINGS_GPSSPEED_9600;
} else
static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport
#endif
{
// Retrieve settings
HwSettingsGPSSpeedGet(&speed);
}
// must always set the baud here, even if it looks like it is the same
// e.g. startup sets 9600 here, ubx_autoconfig sets 57600 there, then the user selects a change back to 9600 here
// on startup (ev == NULL) we must set the Revo port baud rate
// after startup (ev != NULL) we must set the Revo port baud rate only if autoconfig is disabled
// if GPS (UBX or NMEA) is enabled at all
if (gpsPort && gpsEnabled) {
// on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate
// after startup (previousGpsPort == gpsPort) we must set the Revo port baud rate only if autoconfig is disabled
// always we must set the Revo port baud rate if not using UBX
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
if (ev == NULL
|| previousGpsPort != gpsPort
|| gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED
|| gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
#endif
{
// Set Revo port speed
switch (speed) {
case HWSETTINGS_GPSSPEED_2400:
PIOS_COM_ChangeBaud(gpsPort, 2400);
break;
case HWSETTINGS_GPSSPEED_4800:
PIOS_COM_ChangeBaud(gpsPort, 4800);
break;
case HWSETTINGS_GPSSPEED_9600:
PIOS_COM_ChangeBaud(gpsPort, 9600);
break;
case HWSETTINGS_GPSSPEED_19200:
PIOS_COM_ChangeBaud(gpsPort, 19200);
break;
case HWSETTINGS_GPSSPEED_38400:
PIOS_COM_ChangeBaud(gpsPort, 38400);
break;
case HWSETTINGS_GPSSPEED_57600:
PIOS_COM_ChangeBaud(gpsPort, 57600);
break;
case HWSETTINGS_GPSSPEED_115200:
PIOS_COM_ChangeBaud(gpsPort, 115200);
break;
case HWSETTINGS_GPSSPEED_230400:
PIOS_COM_ChangeBaud(gpsPort, 230400);
break;
}
uint8_t speed;
// Retrieve settings
HwSettingsGPSSpeedGet(&speed);
// set fc baud
gps_set_fc_baud_from_arg(speed);
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// even changing the baud rate will make it re-verify the sensor type
// that way the user can just try some baud rates and it when the sensor type is valid, the baud rate is correct
ubx_reset_sensor_type();
// that way the user can just try some baud rates and when the sensor type is valid, the baud rate is correct
gps_ubx_reset_sensor_type();
#endif
}
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
else {
// it will never do this during startup because ev == NULL
ubx_autoconfig_set(NULL);
gps_ubx_autoconfig_set(NULL);
}
#endif
}
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
previousGpsPort = gpsPort;
#endif
}
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
load_mag_settings();
}
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
{
ubx_autoconfig_settings_t newconfig;
GPSSettingsGet(&gpsSettings);
// it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running NMEA
// because ubx auto config never gets called
// setting it up completely means that if we switch from initial NMEA to UBX or disabled to enabled, that it will start normally
newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig;
newconfig.navRate = gpsSettings.UbxRate;
newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
newconfig.storeSettings = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
UBX_DYNMODEL_AIRBORNE1G;
newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
UBX_DYNMODEL_AIRBORNE1G;
switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
@ -677,7 +646,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
break;
}
ubx_autoconfig_set(&newconfig);
gps_ubx_autoconfig_set(&newconfig);
}
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
/**

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

@ -111,74 +111,102 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition
static uint8_t rx_count = 0;
static bool start_flag = false;
static bool found_cr = false;
bool goodParse = false;
uint8_t c;
int i = 0;
for (int i = 0; i < len; i++) {
c = rx[i];
while (i < len) {
c = rx[i++];
// detect start while acquiring stream
if (!start_flag && (c == '$')) { // NMEA identifier found
start_flag = true;
found_cr = false;
rx_count = 0;
} else if (!start_flag) {
return PARSER_ERROR;
// if we find a $ in the middle it was a bad packet (e.g. maybe UBX binary),
// and this may be the start of another packet
// silently cancel the current sentence
if (c == '$') { // NMEA identifier found
start_flag = false;
}
if (!start_flag) { // if no NMEA identifier ('$') found yet
if (c == '$') { // NMEA identifier found
start_flag = true;
found_cr = false;
rx_count = 0;
} else {
// find a likely candidate for a NMEA string
// skip over some e.g. uBlox packets
uint8_t *p;
p = memchr(&rx[i], '$', len - i);
if (p) {
i += p - &rx[i];
} else {
i = len;
}
// if no more data, we can return an error
ret = PARSER_ERROR;
// loop to restart at the $ if there is one
continue;
}
}
if (rx_count >= NMEA_MAX_PACKET_LENGTH) {
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxStats->gpsRxOverflow++;
start_flag = false;
found_cr = false;
rx_count = 0;
ret = PARSER_OVERRUN;
continue;
} else {
gps_rx_buffer[rx_count] = c;
rx_count++;
gps_rx_buffer[rx_count++] = c;
}
// look for ending '\r\n' sequence
if (!found_cr && (c == '\r')) {
found_cr = true;
} else if (found_cr && (c != '\n')) {
found_cr = false; // false end flag
} else if (found_cr && (c == '\n')) {
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count - 2] = 0;
} else if (found_cr) {
if (c != '\n') {
found_cr = false; // false end flag
} else {
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count - 2] = 0;
// prepare to parse next sentence
start_flag = false;
found_cr = false;
rx_count = 0;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// prepare to parse next sentence
start_flag = false;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx.
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxChkSumError++;
// PIOS_DEBUG_PinLow(2);
ret = PARSER_ERROR;
} else { // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx.
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxParserError++;
gpsRxStats->gpsRxChkSumError++;
// PIOS_DEBUG_PinLow(2);
} else {
gpsRxStats->gpsRxReceived++;
};
ret = PARSER_COMPLETE;
ret = PARSER_ERROR;
} else { // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
// PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxParserError++;
// PIOS_DEBUG_PinLow(2);
ret = PARSER_ERROR;
} else {
gpsRxStats->gpsRxReceived++;
goodParse = true;
}
}
continue;
}
}
}
return ret;
if (goodParse) {
// if so much as one good sentence we return a good status so the connection status says "alive"
// if we didn't do this, a lot of garbage (e.g. UBX protocol) mixed in with enough NMEA to fly
// might think the GPS was offline
return PARSER_COMPLETE;
} else {
return ret;
}
}
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
@ -429,6 +457,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
#endif
if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
// leave my new field alone!
GPSPositionSensorBaudRateGet(&GpsData->BaudRate);
GPSPositionSensorSet(GpsData);
}
return false;
@ -440,6 +470,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
#ifdef DEBUG_MSGID_IN
DEBUG_MSG("U");
#endif
// leave my new field alone!
GPSPositionSensorBaudRateGet(&GpsData->BaudRate);
GPSPositionSensorSet(GpsData);
}

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

@ -126,9 +126,10 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
static enum proto_states proto_state = START;
static uint16_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
int i = 0;
for (int i = 0; i < len; i++) {
c = rx[i];
while (i < len) {
c = rx[i++];
switch (proto_state) {
case START: // detect protocol
if (c == UBX_SYNC1) { // first UBX sync char found
@ -170,9 +171,6 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
if (++rx_count == ubx->header.len) {
proto_state = UBX_CHK1;
}
} else {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
}
break;
case UBX_CHK1:
@ -189,7 +187,8 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
proto_state = START;
}
break;
default: break;
default:
break;
}
if (proto_state == START) {
@ -200,6 +199,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
ret = PARSER_COMPLETE; // message complete & processed
}
}
return ret;
}
@ -528,6 +528,8 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
GpsPosition->SensorType = sensorType;
if (msgtracker.msg_received == ALL_RECEIVED) {
// leave my new field alone!
GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate);
GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;
id = GPSPOSITIONSENSOR_OBJID;

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

@ -54,6 +54,8 @@ struct GPS_RX_STATS {
};
int32_t GPSInitialize(void);
void gps_set_fc_baud_from_arg(uint8_t baud);
uint32_t hwsettings_gpsspeed_enum_to_baud(uint8_t baud);
#endif // GPS_H

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

@ -49,16 +49,15 @@
// later versions dropped this and drop data when the send buffer is full and that could be even longer
// rather than have long timeouts, we will let timeouts * retries handle that if it happens
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (500 * 1000)
// timeout for a settings save, in case it has to erase flash
#define UBX_REPLY_TO_SAVE_TIMEOUT (3000 * 1000)
#define UBX_REPLY_TIMEOUT (280 * 1000)
// timeout for a settings save, in case it has to erase flash?
#define UBX_SAVE_WAIT_TIME (1000 * 1000)
// max retries in case of timeout
#define UBX_MAX_RETRIES 5
// pause between each verifiably correct configuration step
#define UBX_VERIFIED_STEP_WAIT_TIME (50 * 1000)
// pause between each unverifiably correct configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000)
// max time for ubx parser to respond to MON_VER
#define UBX_PARSER_TIMEOUT (950 * 1000)
// pause between each unverifiable (no ack/nak) configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (280 * 1000)
#define UBX_CFG_CFG_OP_STORE_SETTINGS \
(UBX_CFG_CFG_SETTINGS_IOPORT | \
@ -85,9 +84,7 @@ typedef enum {
#define UBX_
typedef struct {
bool autoconfigEnabled;
bool storeSettings;
GPSSettingsUbxAutoConfigOptions UbxAutoConfig;
bool SBASRanging;
bool SBASCorrection;
bool SBASIntegrity;
@ -114,9 +111,9 @@ typedef struct UBX_CFG_GNSS ubx_cfg_gnss_t;
typedef struct UBXSENTHEADER UBXSentHeader_t;
typedef union UBXSENTPACKET UBXSentPacket_t;
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
void ubx_autoconfig_set(ubx_autoconfig_settings_t *config);
void ubx_reset_sensor_type();
void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config);
void gps_ubx_reset_sensor_type();
int32_t ubx_autoconfig_get_status();
#endif /* UBX_AUTOCONFIG_H_ */

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

@ -29,15 +29,19 @@
*/
#include <openpilot.h>
#include "hwsettings.h"
#include "gpssettings.h"
#include "inc/ubx_autoconfig.h"
#include <pios_mem.h>
#include "taskinfo.h"
// private type definitions
typedef enum {
INIT_STEP_DISABLED = 0,
INIT_STEP_START,
INIT_STEP_SEND_MON_VER,
INIT_STEP_WAIT_MON_VER_ACK,
INIT_STEP_RESET_GPS,
INIT_STEP_REVO_9600_BAUD,
INIT_STEP_GPS_BAUD,
@ -48,7 +52,9 @@ typedef enum {
INIT_STEP_CONFIGURE_WAIT_ACK,
INIT_STEP_SAVE,
INIT_STEP_SAVE_WAIT_ACK,
INIT_STEP_PRE_DONE,
INIT_STEP_DONE,
INIT_STEP_PRE_ERROR,
INIT_STEP_ERROR
} initSteps_t;
@ -58,17 +64,24 @@ typedef struct {
uint32_t lastStepTimestampRaw; // timestamp of last operation
uint32_t lastConnectedRaw; // timestamp of last time gps was connected
struct {
UBXSentPacket_t working_packet; // outbound "buffer"
// bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
// failure mode otherwise:
// - send message with baud rate change
// - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly)
// - change Revo port baud rate
// sometimes fails (much worse for lowest baud rates)
uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
union {
struct {
UBXSentPacket_t working_packet; // outbound "buffer"
// bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
// failure mode otherwise:
// - send message with baud rate change
// - wait 1 second (even at 2400, the baud rate change command
// - should clear even an initially full 31 byte PIOS buffer much more quickly)
// - change Revo port baud rate
// sometimes fails (much worse for lowest baud rates)
// FIXME: remove this and retest when someone has time
uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
} __attribute__((packed));
GPSSettingsData gpsSettings;
} __attribute__((packed));
} __attribute__((packed));
volatile ubx_autoconfig_settings_t currentSettings;
int8_t lastConfigSent; // index of last configuration string sent
int8_t lastConfigSent; // index of last configuration string sent
struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
uint8_t retryCount;
} status_t;
@ -142,6 +155,12 @@ ubx_cfg_msg_t msg_config_ubx7[] = {
// note that a reset is always done with autoconfig.store
// #define ALWAYS_RESET
// we can enable this when we know how to make the Flight Controller save an object to permanent storage
// also see comment about simple edit in gpssettings.xml
#define AUTOBAUD_CONFIGURE_STORE_AND_DISABLE
// Alessio Morale May 20 3:16 AM
// @Cliff you should update the ObjectPersistence uavo passing the object id and the desired operation.
// private variables
// enable the autoconfiguration system
@ -150,7 +169,9 @@ static volatile bool current_step_touched = false;
// both the pointer and what it points to are volatile. Yuk.
static volatile status_t *volatile status = 0;
static uint8_t hwsettings_baud;
static uint8_t baud_to_try_index = 255;
// functions
static void append_checksum(UBXSentPacket_t *packet)
{
@ -199,7 +220,7 @@ static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t mess
static void set_current_step_if_untouched(initSteps_t new_steps)
{
// assume this one byte initSteps_t is atomic
// take care of some but not all concurrency issues
// take care of some concurrency issues
if (!current_step_touched) {
status->currentStep = new_steps;
@ -210,11 +231,21 @@ static void set_current_step_if_untouched(initSteps_t new_steps)
}
void ubx_reset_sensor_type()
void gps_ubx_reset_sensor_type()
{
ubxHwVersion = -1;
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
static uint8_t mutex; // = 0
// is this needed?
// what happens if two tasks / threads try to do an XyzSet() at the same time?
if (__sync_fetch_and_add(&mutex, 1) == 0) {
ubxHwVersion = -1;
baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
GPSPositionSensorSensorTypeSet(&sensorType);
// make the sensor type / autobaud code time out immediately to send the request immediately
status->lastStepTimestampRaw += 0x8000000UL;
}
--mutex;
}
@ -243,73 +274,16 @@ static void config_gps_baud(uint16_t *bytes_to_send)
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_prt_t));
status->working_packet.message.payload.cfg_prt.mode = UBX_CFG_PRT_MODE_DEFAULT; // 8databits, 1stopbit, noparity, and non-zero reserved
status->working_packet.message.payload.cfg_prt.portID = 1; // 1 = UART1, 2 = UART2
// for protocol masks, bit 0 is UBX enable, bit 1 is NMEA enable
status->working_packet.message.payload.cfg_prt.inProtoMask = 1; // 1 = UBX only (bit 0)
status->working_packet.message.payload.cfg_prt.outProtoMask = 1; // 1 = UBX only (bit 0)
// disable current UBX messages for low baud rates
status->working_packet.message.payload.cfg_prt.outProtoMask = 1;
// Ask GPS to change it's speed
switch (hwsettings_baud) {
case HWSETTINGS_GPSSPEED_2400:
status->working_packet.message.payload.cfg_prt.baudRate = 2400;
break;
case HWSETTINGS_GPSSPEED_4800:
status->working_packet.message.payload.cfg_prt.baudRate = 4800;
break;
case HWSETTINGS_GPSSPEED_9600:
status->working_packet.message.payload.cfg_prt.baudRate = 9600;
break;
case HWSETTINGS_GPSSPEED_19200:
status->working_packet.message.payload.cfg_prt.baudRate = 19200;
break;
case HWSETTINGS_GPSSPEED_38400:
status->working_packet.message.payload.cfg_prt.baudRate = 38400;
break;
case HWSETTINGS_GPSSPEED_57600:
status->working_packet.message.payload.cfg_prt.baudRate = 57600;
break;
case HWSETTINGS_GPSSPEED_115200:
status->working_packet.message.payload.cfg_prt.baudRate = 115200;
break;
case HWSETTINGS_GPSSPEED_230400:
status->working_packet.message.payload.cfg_prt.baudRate = 230400;
break;
}
status->working_packet.message.payload.cfg_prt.baudRate = hwsettings_gpsspeed_enum_to_baud(hwsettings_baud);
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t));
}
// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
static void config_baud(uint8_t baud)
{
// Set Revo port hwsettings_baud
switch (baud) {
case HWSETTINGS_GPSSPEED_2400:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 2400);
break;
case HWSETTINGS_GPSSPEED_4800:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 4800);
break;
case HWSETTINGS_GPSSPEED_9600:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 9600);
break;
case HWSETTINGS_GPSSPEED_19200:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 19200);
break;
case HWSETTINGS_GPSSPEED_38400:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 38400);
break;
case HWSETTINGS_GPSSPEED_57600:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 57600);
break;
case HWSETTINGS_GPSSPEED_115200:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 115200);
break;
case HWSETTINGS_GPSSPEED_230400:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 230400);
break;
}
}
static void config_rate(uint16_t *bytes_to_send)
{
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
@ -485,144 +459,50 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
}
// End User Documentation
#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE)
// permanently store our version of GPSSettings.UbxAutoConfig
// we use this to disable after AbConfigStoreAndDisable is complete
static void setGpsSettings()
{
// trying to do this as perfectly as possible we must realize that they may have pressed Send on some fields
// and so those fields are not stored permanently
// if we write the memory copy to flash, we will have made those permanent
// There are two baud rates of interest
// The baud rate the GPS is talking at
// The baud rate Revo is talking at
// These two must match for the GPS to work
// You only have direct control of the Revo baud rate
// The two baud rates must be the same for the Revo to send a command to the GPS
// to tell the GPS to change it's baud rate
// So you start out by changing Revo's baud rate to match the GPS's
// and then enable UbxAutoConfig to tell Revo to change the GPS baud every time, just before it changes the Revo baud
// That is the basis of these instructions
// we could save off the uavo memory copy to a local buffer with a standard GPSSettingsGet()
// load from flash to uavo memory with a UAVObjLoad()
// update our one setting in uavo memory with a standard GPSSettingsUbxAutoConfigSet()
// save from uavo memory to flash with a UAVObjSave()
// modify our saved off copy to have our new setting in it too
// and finally copy the local buffer back out to uavo memory
// There are microprocessors and they each have internal settings
// Revo
// GPS
// and each of these settings can be temporary or permanent
// that would do it as correctly as possible, but it doesn't work
// so we do it the way autotune.c does it
// To change a Revo setting
// Use the System tab in the GCS for all the following
// Example: in Settings->GPSSettings click on the VALUE for UbxAutoConfig and change it to Disabled
// Click on UbxAutoConfig itself and the line will turn green and blue
// To change this setting permanently, press the red up arrow (Save) at the top of the screen
// Permanently means that it uses this setting, even if you reboot Revo, e.g. power off and on
// To change this setting temporarily, press the green up arrow (Send) at the top of the screen
// Temporarily means that it overrides the permanent setting, but it goes back to the permanent setting when you reboot Revo, e.g. power off and on
// To change an internal GPS setting you use the OP GCS System tab to tell Revo to make the GPS changes
// This only works correctly after you have matching baud rates so Revo and GPS can talk together
// "Settings->GPSSettings->UbxAutoConfig = Configure" sets the internal GPS setting temporarily
// "Settings->GPSSettings->UbxAutoConfig = ConfigureAndStore" sets the internal GPS setting permanently
// You want to wind up with a set of permanent settings that work together
// There are two different sets of permanent settings that work together
// GPS at 9600 baud and factory defaults
// Revo configured to start out at 9600 baud, but then completely configure the GPS and switch both to 57600 baud
// (takes 6 seconds at boot up while you are waiting for it to acquire satellites anyway)
// This is the preferred way so that if we change the settings in the future, the new release will automatically use the correct settings
// GPS at 57600 baud with all the settings for the current release stored in the GPS
// Revo configured to disable UbxAutoConfig since all the GPS settings are permanently stored correctly
// May require reconfiguring in a future release
// Changable settings of interest
// AutoConfig mode
// Settings->GPSSettings->UbxAutoConfig (Disabled, Configure, ConfigureAndStore, default=Configure)
// Disabled means that changes to the GPS baud setting only affect the Revo port
// It doesn't try to change the GPS's internal baud rate setting
// Configure means change the GPS's internal baud setting temporarily (GPS settings revert to the permanent values when GPS is powered off/on)
// ConfigureAndStore means change the GPS's internal baud setting permanently (even after the GPS is powered off/on)
// GPS baud rate
// Settings->HwSettings->GPSSpeed
// If the baud rates are the same and an AutoConfig mode is enabled this will change both the GPS baud rate and the Revo baud rate
// If the baud rates are not the same and an AutoConfig mode is enabled it will fail
// If AutoConfig mode is disabled this will only change the Revo baud rate
// View only settings of interest
// Detected GPS type
// Data Objects -> GPSPositionSensor -> SensorType (Unknown, NMEA, UBX, UBX7, UBX8)
// When it says something other than Unknown, the GPS and Revo baud rates are synced and talking
// Real time progress of the GPS detection process
// Data Objects -> GPSPositionSensor -> AutoConfigStatus (DISABLED, RUNNING, DONE, ERROR)
// Syncing the baud rates means that the GPS's internal baud rate setting is the same as the Revo port setting
// This is necessary for the GPS to work with Revo
// To sync to and find out an unknown GPS baud rate (or sync to and use a known GPS baud rate)
// Temporarily change the AutoConfig mode to Disabled
// Temporarily change the GPS baud rate to a value you think it might be (or go up the list)
// See if that baud rate is correct (Data Objects->GPSPositionSensor->SensorType will be something besides Unknown)
// Repeat, changing the GPS baud rate, until found
// Some very important facts:
// For 9600 baud or lower, the autoconfig will configure it to factory default settings
// For 19200 baud or higher, the autoconfig will configure it to OP required settings
// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
// 57600 baud is recommended for the current release
// That can be achieved either by
// autoconfiging the GPS from a permanent 9600 baud (and factory settings) to a temporary 57600 (with OP settings) on each power up
// or by configuring the GPS with a permanent 57600 (with OP settings) and then permanently disabling autoconfig
// Some previous releases used 38400 and had some other settings differences
// The user should either:
// Permanently configure their GPS to 9600 baud factory settings and tell the Revo configuration to load volatile settings at each startup by:
// (Recommended method because new versions could require new settings and this handles future changes automatically)
// Syncing the baud rates
// Setting it to autoconfig.nostore and waiting for it to complete
// Setting HwSettings.GPSSpeed to 9600 and waiting for it to complete
// Setting it to autoconfig.store and waiting for it to complete (this tells the GPS to store the 9600 permanently)
// Permanently setting it to autoconfig.nostore and waiting for it to complete
// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
// Permanently configure their GPS to 57600 baud, including OpenPilot settings and telling the Revo configuration to just set the baud to 57600 at each startup by:
// (Less recommended method because new versions could require new settings so you would have to do this again)
// Syncing the baud rates
// Setting it to autoconfig.nostore and waiting for it to complete
// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
// Setting it to autoconfig.store
// Permanently setting it to autoconfig.disabled
// The algorithm is:
// If autoconfig is enabled at all
// It will assume that the GPS boot up baud rate is 9600 and the user wants that changed to HwSettings.GPSSpeed
// and that change can be either volatile (must be done each boot up) or non-volatile (stored in GPS's non-volatile settings storage)
// according to whether CONFIGURE is used or CONFIGUREANDSTORE is used
// The only user who should need CONFIGUREANDSTORE stored permanently in Revo is Dave, who configures many OP GPS's before shipping
// plug a factory default GPS in to a Revo, power up, wait for it to configure and permanently store in the GPS, power down, ship
// If autoconfig is not enabled
// it will use HwSettings.GPSSpeed for the baud rate and not do any configuration changes
// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE it will
// 1 Reset the permanent configuration back to factory default
// 2 Disable NEMA message settings
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
// 4 Save the current volatile settings to non-volatile storage
// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGURE it will
// 2 Disable NEMA message settings
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
// If the requested baud rate is 9600 or less it skips the step (3) of adding some volatile UBX settings
// Talking points to point out:
// U-center is no longer needed for any use case with this code
// 9600 is factory default for GPS's
// Some GPS can't even permanently store settings and must start at 9600 baud?
// I have a GPS that sometimes looses settings and reverts to 9600 and this is a fix for that too :)
// This code handles a GPS configured either way (9600 with factory default settings or e.g. 57600 with OP settings)
// Autoconfig.nostore at each boot for 9600, autoconfig.disabled for the 57600 with OP settings (or custom settings and baud)
// This code can permanently configure a GPS to be e.g. 9600 with factory default settings or 57600 with OP settings
// GPS's with 9600 baud and factory default settings would be a good default for future OP releases
// Changing the GPS internal settings multiple times in the future is handled automatically
// This code is written to do a configure from 9600 to 57600
// (actually 9600 to whatever is stored in HwSettings.GPSSpeed)
// if autoconfig is enabled at boot up
// When autoconfiging to 9600 baud or lower, the autoconfig will configure it to factory default settings, not OP settings
// That is because 9600 baud drops many of the OP messages and because 9600 baud is factory default
// For 19200 baud or higher, the autoconfig will configure it to OP required settings
// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
// This is good for factory default GPS's
// This is good in case we change some settings in a future release
#if 0
// get the "in memory" version to a local buffer
GPSSettingsGet((void *)&status->gpsSettings);
// load the permanent version into memory
UAVObjLoad(GPSSettingsHandle(), 0);
#endif
// change the in memory version of the field we want to change
GPSSettingsUbxAutoConfigSet((GPSSettingsUbxAutoConfigOptions *)&status->currentSettings.UbxAutoConfig);
// save the in memory version to permanent
UAVObjSave(GPSSettingsHandle(), 0);
#if 0
// copy the setting into the struct we will use to Set()
status->gpsSettings.UbxAutoConfig = status->currentSettings.UbxAutoConfig;
// try casting it correctly and it says:
// expected 'struct GPSSettingsData *' but argument is of type 'struct GPSSettingsData *'
// probably a volatile or align issue
GPSSettingsSet((void *)&status->gpsSettings); // set the "in memory" version back into use
#endif
}
#endif /* if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) */
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// 9600 baud and lower are not usable, and are best left at factory default
// if the user selects 9600
void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
{
*bytes_to_send = 0;
*buffer = (char *)status->working_packet.buffer;
@ -632,13 +512,12 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
if (!status) {
return;
}
// smallest delay between each step
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_VERIFIED_STEP_WAIT_TIME) {
return;
}
// get UBX version whether autoconfig is enabled or not
// this allows the user to try some baud rates and visibly see when it works
// get UBX version whether autobaud / autoconfig is enabled or not
// this allows the user to manually try some baud rates and visibly see when it works
// it also is how the autobaud code determines when the baud rate is correct
// ubxHwVersion is a global set externally by the caller of this function
// it is set when the GPS responds to a MON_VER message
if (ubxHwVersion <= 0) {
// at low baud rates and high data rates the ubx gps simply must drop some outgoing data
// this isn't really an error
@ -650,14 +529,59 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped
// wait for the normal reply timeout before sending it over and over
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT) {
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_PARSER_TIMEOUT) {
return;
}
// at this point we have already waited for the MON_VER reply to time out (except the first time where it times out without being sent)
// and the fact we are here says that ubxHwVersion has not been set (it is set externally)
// so this try at this baud rate has failed
// if we get here
// select the next baud rate, skipping ahead if new baud rate is HwSettings.GPSSpeed
// set Revo baud rate to current++ value (immediate change so we can send right after that) and send the MON_VER request
// baud rate search order are most likely matches first
// if AutoBaud or higher, do AutoBaud
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) {
uint8_t baud_to_try;
static uint8_t baud_array[] = {
HWSETTINGS_GPSSPEED_57600,
HWSETTINGS_GPSSPEED_9600,
HWSETTINGS_GPSSPEED_115200,
HWSETTINGS_GPSSPEED_38400,
HWSETTINGS_GPSSPEED_19200,
HWSETTINGS_GPSSPEED_230400,
HWSETTINGS_GPSSPEED_4800,
HWSETTINGS_GPSSPEED_2400
};
// first try HwSettings.GPSSpeed and then
// get the next baud rate to try from the table, but skip over the value of HwSettings.GPSSpeed
do {
// index is inited to be out of bounds, which is interpreted as "currently defined baud rate" (= HwSettings.GPSSpeed)
if (baud_to_try_index >= sizeof(baud_array) / sizeof(baud_array[0])) {
HwSettingsGPSSpeedGet(&hwsettings_baud);
baud_to_try = hwsettings_baud;
baud_to_try_index = 0;
break;
} else {
baud_to_try = baud_array[baud_to_try_index++];
}
// skip HwSettings.GPSSpeed when you run across it in the list
} while (baud_to_try == hwsettings_baud);
// set the FC (Revo) baud rate
gps_set_fc_baud_from_arg(baud_to_try);
}
// this code is executed even if ubxautoconfig is disabled
// it detects the "sensor type" = type of GPS
// the user can use this to manually determine if the baud rate is correct
build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
// keep timeouts running properly, we (will have) just sent a packet that generates a reply
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
if (!enabled) {
// keep resetting the timeouts here if we are not actually going to run the configure code
// not really necessary, but it keeps the timer from wrapping every 50 seconds
@ -665,23 +589,40 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
return; // autoconfig not enabled
}
// replaying constantly could wear the settings memory out
// don't allow constant reconfiging when offline
// don't even allow program bugs that could constantly toggle between connected and disconnected to cause configuring
if (status->currentStep == INIT_STEP_DONE || status->currentStep == INIT_STEP_ERROR) {
return;
}
////////
// FSM
////////
switch (status->currentStep) {
// if here, we have verified that the baud rates are in sync sometime in the past
case INIT_STEP_START:
// we should look for the GPS version again
ubx_reset_sensor_type();
// do not fall through to next state
// or it might try to get the sensor type when the baud rate is half changed
set_current_step_if_untouched(INIT_STEP_RESET_GPS);
// allow it to get the sensor type immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
// we should look for the GPS version again (user may plug in a different GPS and then do autoconfig again)
// zero retries for the next state that needs it (INIT_STEP_SAVE)
set_current_step_if_untouched(INIT_STEP_SEND_MON_VER);
// fall through to next state
// we can do that if we choose because we haven't sent any data in this state
// break;
case INIT_STEP_SEND_MON_VER:
build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
// keep timeouts running properly, we (will have) just sent a packet that generates a reply
set_current_step_if_untouched(INIT_STEP_WAIT_MON_VER_ACK);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
case INIT_STEP_WAIT_MON_VER_ACK:
// wait for previous step
// extra wait time might well be unnecessary but we want to make sure
// that we don't stop waiting too soon
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
return;
}
// Continue with next configuration option
set_current_step_if_untouched(INIT_STEP_RESET_GPS);
// fall through to next state
// we can do that if we choose because we haven't sent any data in this state
// break;
// if here, we have just verified that the baud rates are in sync (again)
case INIT_STEP_RESET_GPS:
// make sure we don't change the baud rate too soon and garble the packet being sent
// even after pios says the buffer is empty, the serial port buffer still has data in it
@ -694,15 +635,19 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
#if !defined(ALWAYS_RESET)
// ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore
// but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore
// if user requests a low baud rate then we just reset and leave it set to NEMA
// because low baud and high OP data rate doesn't play nice
// if user requests a low baud rate then we just reset and avoid adding navigation sentences
// because low GPS baud and high OP data rate doesn't play nice
// if user requests that settings be saved, we will reset here too
// that makes sure that all strange settings are reset to factory default
// else these strange settings may persist because we don't reset all settings by table
if (status->currentSettings.storeSettings)
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGUREANDSTORE
#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE)
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE
#endif
)
#endif
{
// reset all GPS parameters to factory default (configure low rate NEMA for low baud rates)
// reset all GPS parameters to factory default (configure low rate NMEA for low baud rates)
// this is not usable by OP code for either baud rate or types of messages sent
// but it starts up very quickly for use with autoconfig-nostore (which sets a high baud and enables all the necessary messages)
config_reset(bytes_to_send);
@ -712,22 +657,30 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
set_current_step_if_untouched(INIT_STEP_REVO_9600_BAUD);
break;
// GPS was just reset, so GPS is running 9600 baud, and Revo is running whatever baud it was before
case INIT_STEP_REVO_9600_BAUD:
#if !defined(ALWAYS_RESET)
// if user requests a low baud rate then we just reset and leave it set to NEMA
// if user requests a low baud rate then we just reset and leave it set to NMEA
// because low baud and high OP data rate doesn't play nice
// if user requests that settings be saved, we will reset here too
// that makes sure that all strange settings are reset to factory default
// else these strange settings may persist because we don't reset all settings by hand
if (status->currentSettings.storeSettings)
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGUREANDSTORE
#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE)
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE
#endif
)
#endif
{
// wait for previous step
// extra wait time might well be unnecessary but we want to make very sure
// that we don't stop waiting too soon as that could leave us at an unknown baud rate
// (i.e. set or not set) if the the transmit buffer was full and we were running at a low baud rate
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
return;
}
// set the Revo GPS port to 9600 baud to match the reset to factory default that has already been done
config_baud(HWSETTINGS_GPSSPEED_9600);
gps_set_fc_baud_from_arg(HWSETTINGS_GPSSPEED_9600);
}
// at most, we just set Revo baud and that doesn't send any data
// fall through to next state
@ -736,6 +689,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
// break;
// Revo and GPS are both at 9600 baud
case INIT_STEP_GPS_BAUD:
// https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
// It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could
@ -757,14 +711,16 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
// GPS is at final baud and Revo is at old baud (old is 9600 or initial detected baud)
case INIT_STEP_REVO_BAUD:
// wait for previous step
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
return;
}
// set the Revo GPS port baud rate to the (same) user configured value
config_baud(hwsettings_baud);
gps_set_fc_baud_from_arg(hwsettings_baud);
status->lastConfigSent = LAST_CONFIG_SENT_START;
// zero the retries for the first "enable sentence"
status->retryCount = 0;
// skip enabling UBX sentences for low baud rates
// low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration
@ -821,7 +777,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// timeout or NAK, resend the message or abort
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES) {
set_current_step_if_untouched(INIT_STEP_ERROR);
set_current_step_if_untouched(INIT_STEP_PRE_ERROR);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
}
@ -835,55 +791,67 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
break;
}
// all configurations have been made
case INIT_STEP_SAVE:
if (status->currentSettings.storeSettings) {
// now decide whether to save them permanently into the GPS
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGUREANDSTORE
#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE)
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE
#endif
) {
config_save(bytes_to_send);
set_current_step_if_untouched(INIT_STEP_SAVE_WAIT_ACK);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
} else {
set_current_step_if_untouched(INIT_STEP_DONE);
// allow it enter INIT_STEP_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
set_current_step_if_untouched(INIT_STEP_PRE_DONE);
// allow it enter INIT_STEP_PRE_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
}
break;
// we could remove this state
// if we retry, it writes to settings storage a few more times
// and it is probably the ack that was dropped, with the save actually performed correctly
// command to save configuration has already been issued
case INIT_STEP_SAVE_WAIT_ACK:
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
// Continue with next configuration option
set_current_step_if_untouched(INIT_STEP_DONE);
// note that we increase the reply timeout in case the GPS must do a flash erase
} else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TO_SAVE_TIMEOUT &&
(ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
// allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
} else {
// timeout or NAK, resend the message or abort
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES / 2) {
// give up on the retries
set_current_step_if_untouched(INIT_STEP_ERROR);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
} else {
// retry a few times
set_current_step_if_untouched(INIT_STEP_SAVE);
}
// save doesn't appear to respond, even in 24 seconds
// just delay a while, in case there it is busy with a flash write, etc.
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_SAVE_WAIT_TIME) {
return;
}
// fall through to next state
// we can do that if we choose because we haven't sent any data in this state
set_current_step_if_untouched(INIT_STEP_PRE_DONE);
// break;
// the autoconfig has completed normally
case INIT_STEP_PRE_DONE:
#if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE)
// determine if we need to disable autoconfig via the autoconfig==AUTOBAUDCONFIGSTOREANDDISABLE setting
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDCONFIGURESTOREANDDISABLE) {
enabled = false;
status->currentSettings.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED;
// like it says
setGpsSettings();
}
#endif
set_current_step_if_untouched(INIT_STEP_DONE);
break;
case INIT_STEP_ERROR:
// an error, such as retries exhausted, has occurred
case INIT_STEP_PRE_ERROR:
// on error we should get the GPS version immediately
ubx_reset_sensor_type();
// fall through
case INIT_STEP_DISABLED:
gps_ubx_reset_sensor_type();
set_current_step_if_untouched(INIT_STEP_ERROR);
break;
case INIT_STEP_DONE:
case INIT_STEP_ERROR:
case INIT_STEP_DISABLED:
break;
}
}
void ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
// this can be called from a different thread
// so everything it touches must be declared volatile
void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
{
initSteps_t new_step;
@ -899,11 +867,12 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
if (config != NULL) {
status->currentSettings = *config;
}
if (status->currentSettings.autoconfigEnabled) {
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE) {
new_step = INIT_STEP_START;
} else {
new_step = INIT_STEP_DISABLED;
}
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
// assume this one byte initSteps_t is atomic
// take care of some but not all concurrency issues
@ -914,11 +883,21 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
status->currentStep = new_step;
status->currentStepSave = new_step;
if (status->currentSettings.autoconfigEnabled) {
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE) {
// enabled refers to autoconfigure
// note that sensor type (gps type) detection happens even if completely disabled
// also note that AutoBaud is less than Configure
enabled = true;
} else {
// this forces the sensor type detection to occur outside the FSM
// and _can_ also engage the autobaud detection that is outside the FSM
// don't do it if FSM is enabled as FSM can change the baud itself
// (don't do it because the baud rates are already in sync)
gps_ubx_reset_sensor_type();
}
}
int32_t ubx_autoconfig_get_status()
{
if (!status || !enabled) {

49
flight/targets/boards/revonano/board_hw_defs.c
View File

@ -348,6 +348,7 @@ static const struct pios_dsm_cfg pios_dsm_main_cfg = {
#endif /* PIOS_INCLUDE_DSM */
#include <pios_sbus_priv.h>
#if defined(PIOS_INCLUDE_SBUS)
/*
@ -534,6 +535,54 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
#endif /* PIOS_INCLUDE_DSM */
#if defined(PIOS_INCLUDE_SRXL)
/*
* SRXL USART
*/
#include <pios_srxl_priv.h>
static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = {
.regs = FLEXI_USART_REGS,
.remap = FLEXI_USART_REMAP,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = FLEXI_USART_IRQ,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = FLEXI_USART_RX_GPIO,
.init = {
.GPIO_Pin = FLEXI_USART_RX_PIN,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = FLEXI_USART_TX_GPIO,
.init = {
.GPIO_Pin = FLEXI_USART_TX_PIN,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
#endif /* PIOS_INCLUDE_SRXL */
/*
* HK OSD
*/

1
flight/targets/boards/revonano/firmware/inc/pios_config.h
View File

@ -107,6 +107,7 @@
/* #define PIOS_INCLUDE_PPM_FLEXI */
#define PIOS_INCLUDE_DSM
#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_SRXL
#define PIOS_INCLUDE_GCSRCVR
// #define PIOS_INCLUDE_OPLINKRCVR

21
flight/targets/boards/revonano/firmware/pios_board.c
View File

@ -642,6 +642,27 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RM_FLEXIPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
case HWSETTINGS_RM_FLEXIPORT_SRXL:
#if defined(PIOS_INCLUDE_SRXL)
{
uint32_t pios_usart_srxl_id;
if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_srxl_id;
if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) {
PIOS_Assert(0);
}
uint32_t pios_srxl_rcvr_id;
if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id;
}
#endif
break;
} /* hwsettings_rm_flexiport */

6
flight/targets/boards/revonano/pios_board.h
View File

@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler;
#define PIOS_SBUS_MAX_DEVS 1
#define PIOS_SBUS_NUM_INPUTS (16 + 2)
// -------------------------
// Receiver Multiplex SRXL input
// -------------------------
#define PIOS_SRXL_MAX_DEVS 1
#define PIOS_SRXL_NUM_INPUTS 16
// -------------------------
// Receiver DSM input
// -------------------------

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

@ -176,19 +176,20 @@ void VehicleConfigurationHelper::applyHardwareConfiguration()
GPSSettings *gpsSettings = GPSSettings::GetInstance(m_uavoManager);
Q_ASSERT(gpsSettings);
GPSSettings::DataFields gpsData = gpsSettings->getData();
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_DISABLED;
switch (m_configSource->getGpsType()) {
case VehicleConfigurationSource::GPS_NMEA:
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_NMEA;
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_NMEA;
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_DISABLED;
break;
case VehicleConfigurationSource::GPS_UBX:
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX;
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX;
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE;
break;
case VehicleConfigurationSource::GPS_PLATINUM:
{
gpsData.DataProtocol = GPSSettings::DATAPROTOCOL_UBX;
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_CONFIGURE;
gpsData.UbxAutoConfig = GPSSettings::UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE;
AuxMagSettings *magSettings = AuxMagSettings::GetInstance(m_uavoManager);
Q_ASSERT(magSettings);
AuxMagSettings::DataFields magsData = magSettings->getData();

1
shared/uavobjectdefinition/gpspositionsensor.xml
View File

@ -14,6 +14,7 @@
<field name="VDOP" units="" type="float" elements="1"/>
<field name="SensorType" units="" type="enum" elements="1" options="Unknown,NMEA,UBX,UBX7,UBX8" defaultvalue="Unknown" />
<field name="AutoConfigStatus" units="" type="enum" elements="1" options="DISABLED,RUNNING,DONE,ERROR" defaultvalue="DISABLED" />
<field name="BaudRate" units="" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400,Unknown" defaultvalue="Unknown" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

3
shared/uavobjectdefinition/gpssettings.xml
View File

@ -6,7 +6,8 @@
<field name="MaxPDOP" units="" type="float" elements="1" defaultvalue="3.5"/>
<!-- Ubx self configuration. Enable to allow the flight board to auto configure ubx GPS options,
store does AutoConfig and save GPS settings (i.e. to prevent issues if gps is power cycled) -->
<field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,Configure,ConfigureAndStore" defaultvalue="Disabled"/>
<!-- add AbConfigStoreAndDisable after AbConfigAndStore and uncomment AUTOBAUD_CONFIGURE_STORE_AND_DISABLE in ubx_autoconfig.c-->
<field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,AutoBaud,AutoBaudAndConfigure,AutoBaudConfigureAndStore,AutoBaudConfigureStoreAndDisable" defaultvalue="AutoBaudAndConfigure"/>
<!-- Ubx position update rate, -1 for auto -->
<field name="UbxRate" units="Hz" type="int8" elements="1" defaultvalue="5" />
<!-- Ubx dynamic model, see UBX datasheet for more details -->