1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

OP-1464 GPS autobaud - stash while debugging newobject issue

This commit is contained in:
Cliff Geerdes 2015-05-18 18:37:53 -04:00
parent 112a79cfbe
commit 92295559b0
8 changed files with 657 additions and 172 deletions

View File

@ -60,8 +60,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 +69,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
@ -88,18 +88,18 @@ void updateGpsSettings(UAVObjEvent *ev);
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 1024
#define STACK_SIZE_BYTES 1024+64
#else
#if defined(PIOS_GPS_MINIMAL)
#define GPS_READ_BUFFER 32
#ifdef PIOS_INCLUDE_GPS_NMEA_PARSER
#define STACK_SIZE_BYTES 580 // NMEA
#define STACK_SIZE_BYTES 580+64 // NMEA
#else
#define STACK_SIZE_BYTES 440 // UBX
#define STACK_SIZE_BYTES 440+64 // UBX
#endif // PIOS_INCLUDE_GPS_NMEA_PARSER
#else
#define STACK_SIZE_BYTES 650
#define STACK_SIZE_BYTES 650+64
#endif // PIOS_GPS_MINIMAL
#endif // PIOS_GPS_SETS_HOMELOCATION
@ -114,7 +114,8 @@ void updateGpsSettings(UAVObjEvent *ev);
static GPSSettingsData gpsSettings;
static uint32_t gpsPort;
//static uint32_t gpsPort;
#define gpsPort PIOS_COM_GPS
static bool gpsEnabled = false;
static xTaskHandle gpsTaskHandle;
@ -138,12 +139,12 @@ static struct GPS_RX_STATS gpsRxStats;
int32_t GPSStart(void)
{
if (gpsEnabled) {
if (gpsPort) {
// 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;
}
// }
}
return -1;
}
@ -156,7 +157,7 @@ int32_t GPSStart(void)
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
// gpsPort = PIOS_COM_GPS;
HwSettingsInitialize();
#ifdef MODULE_GPS_BUILTIN
@ -197,7 +198,8 @@ 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 (gpsPort && gpsEnabled) {
if (gpsEnabled) {
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
#if !defined(PIOS_GPS_MINIMAL)
@ -215,7 +217,8 @@ int32_t GPSInitialize(void)
}
#endif /* if defined(REVOLUTION) */
if (gpsPort && gpsEnabled) {
// 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)
@ -269,16 +272,19 @@ static void gpsTask(__attribute__((unused)) void *parameters)
PERF_INIT_COUNTER(counterBytesIn, 0x97510001);
PERF_INIT_COUNTER(counterRate, 0x97510002);
PERF_INIT_COUNTER(counterParse, 0x97510003);
uint8_t c[GPS_READ_BUFFER];
// uint8_t c[GPS_READ_BUFFER];
uint8_t c[GPS_READ_BUFFER+8];
uint16_t bytes_remaining = 0;
// Loop forever
while (1) {
if (gpsPort) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0;
uint16_t count = 0;
ubx_autoconfig_run(&buffer, &count);
gps_ubx_autoconfig_run(&buffer, &count);
// Something to send?
if (count) {
// clear ack/nak
@ -310,7 +316,9 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// 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);
uint16_t bytes_used;
cnt = PIOS_COM_ReceiveBuffer(gpsPort, &c[bytes_remaining], GPS_READ_BUFFER-bytes_remaining, xDelay);
// cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
if (cnt > 0) {
PERF_TIMED_SECTION_START(counterParse);
PERF_TRACK_VALUE(counterBytesIn, cnt);
@ -324,7 +332,13 @@ static void gpsTask(__attribute__((unused)) void *parameters)
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case GPSSETTINGS_DATAPROTOCOL_UBX:
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
cnt += bytes_remaining;
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats, &bytes_used);
// res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats, &bytes_used);
#if 1
bytes_remaining = cnt - bytes_used;
memmove(c, &c[bytes_used], bytes_remaining);
#endif
break;
#endif
default:
@ -380,8 +394,9 @@ static void gpsTask(__attribute__((unused)) void *parameters)
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 +451,93 @@ 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);
}
}
//void debugindex(__attribute__((unused)) int index, __attribute__((unused)) uint8_t c)
void debugindex(int index, uint8_t c)
{
//#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
static GPSExtendedStatusData olddata;
static GPSExtendedStatusData data;
if (index < 8) {
data.FirmwareHash[index] = c;
} else {
data.BoardType[index-8] = c;
}
if (memcmp(&data, &olddata, sizeof(data))) {
olddata = data;
static uint8_t mutex; // = 0
if (__sync_fetch_and_add(&mutex, 1) == 0) {
GPSExtendedStatusSet(&data);
}
--mutex;
}
//#endif
}
// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
void gps_set_fc_baud_from_arg(uint8_t baud, __attribute__((unused)) uint8_t addit)
{
static uint8_t mutex; // = 0
if (__sync_fetch_and_add(&mutex, 1) == 0) {
// Set Revo port hwsettings_baud
PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud));
GPSPositionSensorCurrentBaudRateSet(&baud);
} else {
static uint8_t c;
debugindex(9, ++c);
if (c > 254) c=254;
}
--mutex;
debugindex(6, baud);
{
static uint8_t c;
debugindex(7, ++c);
}
}
#if 0
void gps_set_fc_baud_from_settings()
{
HwSettingsGPSSpeedOptions speed;
// Retrieve settings
HwSettingsGPSSpeedGet(&speed);
gps_set_fc_baud_from_arg((uint8_t) speed);
}
#endif
/**
* Update the GPS settings, called on startup.
* FIXME: This should be in the GPSSettings object. But objects have
@ -509,71 +611,236 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
// 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 GPS (UBX or NEMA) is enabled at all
static uint32_t previousGpsPort=0xf0f0f0f0; // = 0 means deconfigured and at power up it is deconfigured
if (gpsPort && gpsEnabled) {
//we didn't have a gpsPort when we booted, and disabled did not find GPS's
//but booting with gpsPort valid immediately worked
//oh, PIOS_COM_GPS is certainly set during init and not elsewhere
//and that means changing the GPS port will not be recognized till reboot
/*
it autobauds even if autoconfig is disabled
initial thought is disable that, but is it a good thing to leave in?
we could have several levels, autobaud, autoconfig, save
on one hand, always autobaud is good for user
on other hand, always autobaud is somewhat disruptive to experimentation? I can't think why. To test simulated GPS failure without rewiring?
#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);
}
gps at 2400 baud with autoconfig disabled appears to not have autobauded correctly
and autoconfig runs forever if it gets enabled
yes it did work, but why is SensorType Unknown?
is it running the first step over and over which resets sensor type?
no date so no updates means alarmset()
and all it takes is a MON_VER to make it OK
but why did it work at 57600 and 38400 autobauds?
makes me wonder if 2400 is so slow that it never completes before timeout
so try 9600 etc.
gpstime is working, so baud rate is correct at least part of the time
is it possible that once it gets 2400, it can't get out?
GPSPositionSensor.Status jumps around from NoGPS to NoFix to Fix3D
all else fails try putting gpsPort back the way it was for a test
could be that alarm is disabling gpsPort? that would be stupid
// 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
Oh!!!!
It got stuck because I configured it to 57600 and then let it autobaud to 2400 so it has all messages at 2400
so autobaud is not good without deconfiguring for low baud rates
the previous procedure was, first change to a new baud rate (assuming it was a higher rate)
then ALWAYS configure (including removing messages for 9600 and lower)
so the code would never let it get it in a bad way (2400 with OP messages)
how about starting at 57600 and going to 2400?
doesn't seem like that should work in the old code
// 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
So do we need to disable messages somehow at start of config?
maybe disable ubx in port
maybe disable known messages
could use a flag with same tables and don't wait for ack???
will MON_VER work if outProtoMask = 0?
if so, then maybe all rates <= 9600 (19200?) should set outProtoMask = 0
remember that autoconfig=disabled and u-center is an option for dev experimentation
I really want to let them try 19200 with sentences
do we want baud rate to autobaud always?
is then there any reason for manual setting of baud?
well these fixes maybe make it so it won't get into trouble
but will it get someone out of their own trouble?
first version of new code
4800 works but 2400 does not
2400 works if no data
if increasing timeouts fixes it
maybe we can make timeouts proportional to inverse baud rate
doubling timeouts did not fix it
IMPORTANT
with autoconfig disabled, when you change the baud rate
you just get autobauded back to the original baud rate, but don't know it
that is confusing
it needs one extra step to set desired baud rate after autobauding?
changing baud without configuring at all is dangerous
changing from 57600 to 2400 and leaving messages enabled
if we disable messages, then it is not baud only and it is not baud + configure
and that is confusing
it appears that outprotocol=0 means the MON_VER request doesn't reply
after setting baud to 2400??? with 0 and flashing, it did not get MON_VER
it will go into 2400 but won't come out
it still might be possible to use outprotocol=0 if we set it back to 1
but that is no good, MON_VER will still be bad then
well it won't come out of 2400 with messages, even with outprotocol=0
it truely never gets started on the FSM because MON_VER is blocked
well if MON_VER responds, we could set outprotocol=0 just before MON_VER???
so the big problem is MOV_VER
but there are some messages coming back and it must be MON_VER?
could try commenting out MON_VER send and see if messages go away
it still bounces NoGPS to NoFix to Fix3D even with MON_VER turned off
well this is 2400 with messages so that makes sense
How to get it out of 2400 with messages
maybe the problem is that it took a bunch of retries before
and now it takes 10 times as many because it has to try all baud rates
maybe we can change the cycle factor and make all messages come every 99 cycles at 1000ms per cycle
as part of the MON_VER packet
maybe only allow autobaud if autoconfig is enabled
test old version
test version where it only uses hwsettings_baud (and thus autobaud is disabled)
maybe it is a loop to set gpssettings.ubxautoconfig from a task awakened by gpssettings callback
where did 38400 come from
38400 is boot up HwSettings.GPSSpeed
but I had set HwSettings.GPSSpeed to 2400
it toggled back and forth between 2400 and 38400 and then locked on 38400
it locked at the time the SensorType went to UBX7
There are no GPSSatellites messages being received for UBX7
Is there a reentrancy?
maybe we need to just set flags in one thread and do it all from the other thread
maybe from ubx_autoconfig.c I should just set hwsettings and let GPS.c do the real baud rate set.
and for gps_ubx_reset_sensor_type() I can just skip it (just GPSPositionSensorSensorTypeSet()) if already being called
nobody set the baud, but it quit working when I changed baud in GCS
that might imply that calling HwSettingsGPSSpeedSet() actually change the baud rate.
if that is true, (and it probably is) then it is not possible to use the gcs HwSettings.GPSSpeed to change first the GPS and then the port.
we need a NewGpsSpeed in GpsSettings.
if we already have a baud rate and configure is enabled,
we don't want autobaud running to mess up the acquired baud rate
OTOH we need changing the baud to reaquire in case they changed GPS's?
we could say that you must reboot after changing a GPS or unplugging/plugging a cable
should autobaud connect also set the GPS baud to HwSettings?
autobaud connect (set Revo to GPS baud rate)
autobaud change (set GPS to HwSettings.GPSSpeed)
configure
save
disable
should disable still do autobaud connect on startup and changes?
get GPS at 2400 with sentences
see what it does now
after baud is correct and we set GPSSpeed, it resets sensortype
which causes it to set baud to set baud to GPSSpeed before FSM ran to set GPS baud
design the flow, disabled does nothing
if disabled
callback sets baud
callback resets sensortype
ac sees invalid sensor type and ...
how to handle "connection" i.e. when baud is right or when it is wrong
want to skip some things in some cases when connection is already correct
that will make baud changes quick
want to do some things in some cases when connection is already correct
that will allow them to know real status
at startup, don't assume connection is valid, test it
autobaud does bauds in probability order
starting with hwsettings.gpsspeed so it almost isn't even an autobaud
once connection is made, we need to avoid resetting sensortype
unless what ... 'what' is
maybe monitor MON_VER somehow to see if cable gets unplugged
maybe don't bother and tell them not to disconnect cables
and when something new goes bad, start over with autobaud
should we monitor MON_VER to know when connection goes down?
if so, we need a better way than resetting sensor type
a simple ack would be good.
reset sensor type
reduce to just startup and error
no, it isn't a problem, it is the baud rate setting that is now associated with it
yes, reduce it, it should not be popping back and forth between good and bad just to poll it
baud_to_try_index is reset there, where should it be reset?
at least have a procedure for starting over without reboot
like when plugging in a different GPS
currently the sensortype global is what drives the autoconfig process, so it must be validated
as part of the FSM
so it is initially detected outside FSM, and then for each configure, it is detected again, but without resetting it
that can be done with an FSM wait after sending MON_VER again.
error will reset sensor type
how to set baud rate in callback without causing unsyncing
for synced baud rate and when allowing autobaud in ... huh?
IMPORTANT
the ubx parser doesn't handle partially received buffers
*/
//new code: at startup does it always need to be HwSettings.GPSSpeed? yes
// 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 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
// test prevport
|| previousGpsPort != gpsPort
|| gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED
// this needs to be disabled if we allow it to autobaud when autoconfig is 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;
}
#if 1
uint8_t speed;
// Retrieve settings
HwSettingsGPSSpeedGet(&speed);
// set fc baud
gps_set_fc_baud_from_arg(speed, 1);
debugindex(2, speed);
{
static uint8_t c;
debugindex(3, ++c);
}
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// even changing the baud rate will make it re-verify the sensor type
// 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();
//careful here if the sensor type is connected to autobaud
// make sure that normal autoconfig still works
// that you can set just Revo baud manually and both automatically
#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
}
previousGpsPort = gpsPort;
}
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
@ -588,16 +855,31 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
GPSSettingsGet(&gpsSettings);
#if 0
// if GPS is enabled and UBX GPS is enabled
if (gpsPort && gpsEnabled && gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) {
newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig;
} else {
newconfig.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED;
}
#else
// it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running NEMA
// because ubx auto config never gets called
// setting it up completely means that if we switch from initial NEMA to UBX or disabled to enabled, that it will start normally
newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig;
#endif
newconfig.navRate = gpsSettings.UbxRate;
newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
newconfig.storeSettings = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
// newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED;
// newconfig.storeSettings = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
// newconfig.configStoreAndDisable = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE;
newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
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_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 :
@ -638,11 +920,11 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed;
newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
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_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;
gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
switch (gpsSettings.UbxGNSSMode) {
case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS:
@ -677,7 +959,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) */
/**

View File

@ -107,7 +107,7 @@ struct UBX_ACK_NAK ubxLastNak;
#define UBX_PVT_TIMEOUT (1000)
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats, __attribute__((unused)) uint16_t *bytes_used)
{
int ret = PARSER_INCOMPLETE; // message not (yet) complete
enum proto_states {
@ -127,8 +127,14 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
static uint16_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
#if 0
for (int i = 0; i < len; i++) {
c = rx[i];
#else
int i = 0;
while (i < len) {
c = rx[i++];
#endif
switch (proto_state) {
case START: // detect protocol
if (c == UBX_SYNC1) { // first UBX sync char found
@ -170,10 +176,13 @@ 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 {
}
#if 0 // will never happen
else {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
}
#endif
break;
case UBX_CHK1:
ubx->header.ck_a = c;
@ -198,8 +207,10 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
gpsRxStats->gpsRxReceived++;
proto_state = START;
ret = PARSER_COMPLETE; // message complete & processed
break;
}
}
*bytes_used = i;
return ret;
}

View File

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

View File

@ -619,7 +619,7 @@ extern struct UBX_ACK_NAK ubxLastNak;
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *, uint16_t *bytes_used);
void load_mag_settings();
#endif /* UBX_H */

View File

@ -49,6 +49,7 @@
// 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
#if 0
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (500 * 1000)
// timeout for a settings save, in case it has to erase flash
@ -59,7 +60,18 @@
#define UBX_VERIFIED_STEP_WAIT_TIME (50 * 1000)
// pause between each unverifiably correct configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000)
#else
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (2000 * 1000)
// timeout for a settings save, in case it has to erase flash
#define UBX_REPLY_TO_SAVE_TIMEOUT (3000 * 1000)
// max retries in case of timeout
#define UBX_MAX_RETRIES 5
// pause between each verifiably correct configuration step
#define UBX_VERIFIED_STEP_WAIT_TIME (1000 * 1000)
// pause between each unverifiably correct configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (2000 * 1000)
#endif
#define UBX_CFG_CFG_OP_STORE_SETTINGS \
(UBX_CFG_CFG_SETTINGS_IOPORT | \
UBX_CFG_CFG_SETTINGS_MSGCONF | \
@ -85,9 +97,9 @@ typedef enum {
#define UBX_
typedef struct {
bool autoconfigEnabled;
bool storeSettings;
// bool autoconfigEnabled;
// bool storeSettings;
GPSSettingsUbxAutoConfigOptions UbxAutoConfig;
bool SBASRanging;
bool SBASCorrection;
bool SBASIntegrity;
@ -100,6 +112,7 @@ typedef struct {
bool enableGPS;
bool enableGLONASS;
bool enableBeiDou;
// bool configStoreAndDisable;
} ubx_autoconfig_settings_t;
// Sent messages for configuration support
@ -114,9 +127,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_ */

View File

@ -29,6 +29,7 @@
*/
#include <openpilot.h>
#include "hwsettings.h"
#include "gpssettings.h"
#include "inc/ubx_autoconfig.h"
#include <pios_mem.h>
@ -38,6 +39,8 @@
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,
@ -150,7 +153,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 HwSettingsGPSSpeedOptions hwsettings_baud;
static uint8_t baud_to_try_index = 255;
void debugindex(int index, uint8_t c);
static void append_checksum(UBXSentPacket_t *packet)
{
@ -210,11 +215,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
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);
} else {
static uint8_t c;
debugindex(8, ++c);
if (c > 254) c=254;
}
--mutex;
}
@ -238,78 +253,23 @@ static void config_reset(uint16_t *bytes_to_send)
// 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, uint8_t baud, uint16_t outProtoMask)
static void config_gps_baud(uint16_t *bytes_to_send)
{
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_prt_t));
status->working_packet.message.payload.cfg_prt.mode = UBX_CFG_PRT_MODE_DEFAULT; // 8databits, 1stopbit, noparity, and non-zero reserved
status->working_packet.message.payload.cfg_prt.portID = 1; // 1 = UART1, 2 = UART2
// for protocol masks, bit 0 is UBX enable, bit 1 is NEMA enable
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 = (hwsettings_baud<=HWSETTINGS_GPSSPEED_9600) ? 0 : 1; // 1 = UBX only (bit 0)
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));
@ -622,7 +582,7 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
// This is good in case we change some settings in a future release
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
{
*bytes_to_send = 0;
*buffer = (char *)status->working_packet.buffer;
@ -653,11 +613,93 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT) {
return;
}
// this is where the autobaud code needs to go as it won't get farther till MON_VER returns something
// 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 retry at this baud rate has failed
// we need gps_ubx_reset_sensor_type() (or what else?) to reset this FSM (baud_to_try_index==-1) and the retry count
// need a state machine that counts retries and:
// no, no retries, just loop through the baud rate over and over till it locks
// that may be a bit slower if it misses just one MON_VER than trying twice on the first few (sorted in likelyhood order) baud rates
// but it gets to the other baud rates faster and loops faster
// 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 (most likely matches first)
// HwSettings.GPSSpeed, 57600, 9600, 38400, 115200,
// all values from #2 on are only tried if GPSSpeed!=them
// no, init to HwSettings.GPSSpeed only at power up and if it fails just run the whole list over and over?
// the order to try the baud rates with the most likely first
// these come after trying HwSettings.GPSSpeed but will not duplicate the HwSettings.GPSSpeed test
// without an if for this block, it autobauds even with autoconfig disabled
// if (status->currentSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED)
// this is the same thing
// if (enabled)
{
uint8_t baud_to_try;
//HwSettingsGPSSpeedOptions baud_to_try;
#if 0
static uint8_t baud_array[] = {
HWSETTINGS_GPSSPEED_57600,
HWSETTINGS_GPSSPEED_9600,
HWSETTINGS_GPSSPEED_115200,
HWSETTINGS_GPSSPEED_38400,
HWSETTINGS_GPSSPEED_19200,
HWSETTINGS_GPSSPEED_230400,
HWSETTINGS_GPSSPEED_4800,
HWSETTINGS_GPSSPEED_2400
};
do {
if (baud_to_try_index >= sizeof(baud_array)/sizeof(baud_array[0])) {
HwSettingsGPSSpeedGet(&hwsettings_baud);
baud_to_try = hwsettings_baud;
baud_to_try_index = 0;
break;
} else {
baud_to_try = baud_array[baud_to_try_index++];
}
} while (baud_to_try == hwsettings_baud);
#else
HwSettingsGPSSpeedGet(&hwsettings_baud);
baud_to_try = hwsettings_baud;
debugindex(0, hwsettings_baud);
{
static uint8_t c;
debugindex(1, ++c);
}
#endif
// set the Revo baud rate
gps_set_fc_baud_from_arg(baud_to_try, 2);
}
build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
// keep timeouts running properly, we (will have) just sent a packet that generates a reply
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
{
static uint8_t c;
uint8_t baud;
GPSPositionSensorCurrentBaudRateOptions trythis;
trythis=GPSPOSITIONSENSOR_CURRENTBAUDRATE_9600;
GPSPositionSensorCurrentBaudRateGet(&baud);
debugindex(8, baud);
if (baud != hwsettings_baud) {
debugindex(9, ++c);
// big hack to keep the malfunctioning GPSPositionSensor.CurrentBaudRate correct
// for some reason it keeps going back to boot value (or default if xml has a default)
GPSPositionSensorCurrentBaudRateSet(&trythis);
}
}
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
@ -669,19 +711,98 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// don't allow constant reconfiging when offline
// don't even allow program bugs that could constantly toggle between connected and disconnected to cause configuring
if (status->currentStep == INIT_STEP_DONE || status->currentStep == INIT_STEP_ERROR) {
//do we want this here
// status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
switch (status->currentStep) {
#if 0
// if here, we have just verified that the baud rates are in sync
case INIT_STEP_START:
// we should look for the GPS version again
ubx_reset_sensor_type();
// we should look for the GPS version again (user may plug in a different GPS and then do autoconfig again)
//gps_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);
// note that resetting sensor type causes it to send MON_VER msg and wait for reply before coming back here
#if 0
set_current_step_if_untouched(INIT_STEP_DISABLE_MSGS);
// allow it to get the sensor type immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
case INIT_STEP_DISABLE_MSGS:
config_gps_baud(bytes_to_send, baud_to_try, 0);
// do not fall through to next state
// or it might try to get the sensor type when the baud rate is half changed
// note that resetting sensor type causes it to send MON_VER msg and wait for reply before coming back here
#endif
set_current_step_if_untouched(INIT_STEP_RESET_GPS);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
#endif
// 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 (user may plug in a different GPS and then do autoconfig again)
// zero retries for the next state that needs it (INIT_STEP_SAVE)
status->retryCount = 0;
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:
#if 0
debugindex(0, status->requiredAck.clsID);
debugindex(1, ubxLastAck.clsID);
debugindex(2, status->requiredAck.msgID);
debugindex(3, ubxLastAck.msgID);
debugindex(4, ubxLastNak.clsID);
debugindex(5, ubxLastNak.msgID);
#endif
#if 0
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
// Continue with next configuration option
set_current_step_if_untouched(INIT_STEP_RESET_GPS);
// 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_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) {
// 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_SEND_MON_VER);
}
}
break;
#else
// 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;
#endif
// 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
@ -691,6 +812,14 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// Retrieve desired GPS baud rate once for use throughout this module
HwSettingsGPSSpeedGet(&hwsettings_baud);
#if 0
debugindex(2, hwsettings_baud);
{
static uint8_t c;
debugindex(3, ++c);
}
#endif
//move this up higher / earlier?
#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
@ -699,7 +828,8 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// 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_CONFIGUREANDSTORE
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE)
#endif
{
// reset all GPS parameters to factory default (configure low rate NEMA for low baud rates)
@ -712,6 +842,7 @@ 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
@ -719,23 +850,34 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// 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_CONFIGUREANDSTORE
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE)
#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;
}
#if 0
debugindex(8, hwsettings_baud);
if (hwsettings_baud >= 8) {
debugindex(9, hwsettings_baud);
}
#endif
// 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, 0);
}
// 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;
// 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;
// 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
@ -752,18 +894,25 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// 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, hwsettings_baud, 1);
config_gps_baud(bytes_to_send);
set_current_step_if_untouched(INIT_STEP_REVO_BAUD);
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);
debugindex(4, hwsettings_baud);
{
static uint8_t c;
debugindex(5, ++c);
}
gps_set_fc_baud_from_arg(hwsettings_baud, 3);
status->lastConfigSent = LAST_CONFIG_SENT_START;
status->retryCount = 0;
// skip enabling UBX sentences for low baud rates
@ -835,8 +984,11 @@ 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_CONFIGUREANDSTORE
|| status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE) {
config_save(bytes_to_send);
set_current_step_if_untouched(INIT_STEP_SAVE_WAIT_ACK);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
@ -849,6 +1001,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// we could remove this state
// if we retry, it writes to settings storage a few more times
// and it is probably the ack that was dropped, with the save actually performed correctly
// because e.g. OP sentences at 19200 baud don't fit and stuff (like acks) gets dropped
case INIT_STEP_SAVE_WAIT_ACK:
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
// Continue with next configuration option
@ -872,18 +1025,35 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
}
break;
// an error, such as retries exhausted, has occurred
case INIT_STEP_ERROR:
// on error we should get the GPS version immediately
ubx_reset_sensor_type();
// fall through
gps_ubx_reset_sensor_type();
break;
// user has disable the autoconfig
case INIT_STEP_DISABLED:
break;
// the autoconfig has completed normally
case INIT_STEP_DONE:
// determine if we need to disable autoconfig via the autoconfig==CONFIGSTOREANDDISABLE setting
if (status->currentSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGSTOREANDDISABLE) {
enabled = false;
status->currentSettings.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED;
// like it says
GPSSettingsUbxAutoConfigSet((GPSSettingsUbxAutoConfigOptions *) &status->currentSettings.UbxAutoConfig);
}
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
// I hope that I took care of all the concurrency issues that are likely to happen in the lifetime of OpenPilot
// by lowering the probability from 1 in 50,000 calls to this function (my SWAG) to one in 50,000**3 (forgive my Fortran)
void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
{
initSteps_t new_step;
@ -899,11 +1069,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_DISABLED) {
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 +1085,16 @@ 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_DISABLED) {
enabled = true;
} else {
// this forces the sensor type detection to occur outside the FSM
// and also engages the autobaud detection that is outside the FSM
gps_ubx_reset_sensor_type();
}
}
int32_t ubx_autoconfig_get_status()
{
if (!status || !enabled) {

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="CurrentBaudRate" units="" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

View File

@ -6,7 +6,7 @@
<field name="MaxPDOP" units="" type="float" elements="1" defaultvalue="3.5"/>
<!-- Ubx self configuration. Enable to allow the flight board to auto configure ubx GPS options,
store does AutoConfig and save GPS settings (i.e. to prevent issues if gps is power cycled) -->
<field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,Configure,ConfigureAndStore" defaultvalue="Disabled"/>
<field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,Configure,ConfigureAndStore,ConfigStoreAndDisable" defaultvalue="ConfigStoreAndDisable"/>
<!-- 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 -->