rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00
Code Issues Releases Activity

OP-1464 remove debug

Browse Source
This commit is contained in:
Cliff Geerdes 2015-05-19 20:45:57 -04:00
parent de521c9004
commit 1ade8b7f59
5 changed files with 44 additions and 699 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -479,76 +479,28 @@ uint32_t hwsettings_gpsspeed_enum_to_baud(uint8_t baud)
} }
//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 // 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) void gps_set_fc_baud_from_arg(uint8_t baud)
{ {
static uint8_t previous_baud=255; static uint8_t previous_baud=255;
static uint8_t mutex; // = 0 static uint8_t mutex; // = 0
// do we need this? // 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) { if (__sync_fetch_and_add(&mutex, 1) == 0) {
// don't bother doing the baud change if it is actually the same // don't bother doing the baud change if it is actually the same
// might drop less data // might drop less data
if (previous_baud != baud) { if (previous_baud != baud) {
previous_baud = baud; previous_baud = baud;
// Set Revo port hwsettings_baud // Set Revo port hwsettings_baud
PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud)); PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud));
GPSPositionSensorCurrentBaudRateSet(&baud); GPSPositionSensorCurrentBaudRateSet(&baud);
} }
} else {
static uint8_t c;
debugindex(9, ++c);
if (c > 254) c=254;
} }
--mutex; --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. * Update the GPS settings, called on startup.
* FIXME: This should be in the GPSSettings object. But objects have * FIXME: This should be in the GPSSettings object. But objects have
@ -560,283 +512,28 @@ void gps_set_fc_baud_from_settings()
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) 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 NMEA data):
// - the default uBlox settings (different than OP settings) are NMEA and 9600 baud
// - - and that is OK and you use autoconfig-nostore
// - - I personally feel that this is the best way to set it up because if OP dev changes GPS settings,
// - - you get them automatically changed to match whatever version of OP code you are running
// - - but 9600 is only OK for this autoconfig startup
// - by my testing, the 9600 initial to 57600 final baud startup takes:
// - - 0:10 if the GPS has been configured to send OP data at 9600
// - - 0:06 if the GPS has default data settings (NMEA) at 9600
// - - reminder that even 0:10 isn't that bad. You need to wait for the GPS to acquire satellites,
// Some things you want to think about if you want to play around with this:
// - 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 NMEA, I could have coded it to do a factory reset
// - if set to 9600 baud (or lower)
// if GPS (UBX or NMEA) is enabled at all
static uint32_t previousGpsPort=0xf0f0f0f0; // = 0 means deconfigured and at power up it is deconfigured static uint32_t previousGpsPort=0xf0f0f0f0; // = 0 means deconfigured and at power up it is deconfigured
// if GPS (UBX or NMEA) is enabled at all
if (gpsPort && gpsEnabled) { 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?
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
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
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 // 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 // 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 // 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 defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
if (ev == NULL if (ev == NULL
// test prevport || previousGpsPort != gpsPort
|| previousGpsPort != gpsPort
|| gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || 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) || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
#endif #endif
{ {
#if 1
uint8_t speed; uint8_t speed;
// Retrieve settings // Retrieve settings
HwSettingsGPSSpeedGet(&speed); HwSettingsGPSSpeedGet(&speed);
// set fc baud // set fc baud
gps_set_fc_baud_from_arg(speed, 1); gps_set_fc_baud_from_arg(speed);
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// even changing the baud rate will make it re-verify the sensor type // even changing the baud rate will make it re-verify the sensor type
// that way the user can just try some baud rates and when the sensor type is valid, the baud rate is correct // 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(); 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 #endif
} }
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
@ -849,38 +546,25 @@ the ubx parser doesn't handle partially received buffers
previousGpsPort = gpsPort; previousGpsPort = gpsPort;
} }
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
load_mag_settings(); load_mag_settings();
} }
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
{ {
ubx_autoconfig_settings_t newconfig; ubx_autoconfig_settings_t newconfig;
GPSSettingsGet(&gpsSettings); 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 NMEA // 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 // 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 // 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.UbxAutoConfig = gpsSettings.UbxAutoConfig;
#endif
newconfig.navRate = gpsSettings.UbxRate; newconfig.navRate = gpsSettings.UbxRate;
// 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_STATIONARY ? UBX_DYNMODEL_STATIONARY :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :

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

@ -153,19 +153,7 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition
ret = PARSER_OVERRUN; ret = PARSER_OVERRUN;
continue; continue;
} else { } else {
#if 1
gps_rx_buffer[rx_count++] = c; gps_rx_buffer[rx_count++] = c;
#else
uint8_t *p;
i-=1;
p = memchr(&rx[i], '\r', len-i);
if (p) {
copylen = p - &rx[i];
if (rx_count+copylen >= NMEA_MAX_PACKET_LENGTH) {
memcpy(&gps_rx_buffer[rx_count], &rx[i], len-i);
}
found_cr = true;
#endif
} }
// look for ending '\r\n' sequence // look for ending '\r\n' sequence

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

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

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

@ -49,41 +49,6 @@
// later versions dropped this and drop data when the send buffer is full and that could be even longer // 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 // rather than have long timeouts, we will let timeouts * retries handle that if it happens
// known to work
#if 0
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (400 * 1000) 50 has 1 retry 100 has 1 retry 0 retries at 400, 4@200-200@19200 1@400-200@115200-onetime 1@400@19200
// timeout for a settings save, in case it has to erase flash
#define UBX_SAVE_WAIT_TIME (1000 * 1000)
// max retries in case of timeout
#define UBX_MAX_RETRIES 5
// pause between each verifiably correct configuration step
#define UBX_VERIFIED_STEP_WAIT_TIME (20 * 1000)
// pause between each unverifiably correct configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (400 * 1000) was 200
#endif
// need about 60 + 8 char times for the largest config packet to be sent and acked
// that is about 35ms at 19200 baud, figure 40ms
// but then there is the hairy problem of the dropped packets that occur
// when there is more data to send than there is bandwidth
// we generally avoid turning on the OP data packets for configurations of 9600 baud or lower
#if 0
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (500 * 1000)
// 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 (50 * 1000)
// pause between each unverifiably correct configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000)
#else
// timeouts for ack reception
// 1@265-265@19200
// 0@272-272
#define UBX_REPLY_TIMEOUT (280 * 1000) #define UBX_REPLY_TIMEOUT (280 * 1000)
// timeout for a settings save, in case it has to erase flash? // timeout for a settings save, in case it has to erase flash?
#define UBX_SAVE_WAIT_TIME (1000 * 1000) #define UBX_SAVE_WAIT_TIME (1000 * 1000)
@ -93,7 +58,7 @@
#define UBX_PARSER_TIMEOUT (950 * 1000) #define UBX_PARSER_TIMEOUT (950 * 1000)
// pause between each unverifiable (no ack/nak) configuration step // pause between each unverifiable (no ack/nak) configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (280 * 1000) #define UBX_UNVERIFIED_STEP_WAIT_TIME (280 * 1000)
#endif
#define UBX_CFG_CFG_OP_STORE_SETTINGS \ #define UBX_CFG_CFG_OP_STORE_SETTINGS \
(UBX_CFG_CFG_SETTINGS_IOPORT | \ (UBX_CFG_CFG_SETTINGS_IOPORT | \
UBX_CFG_CFG_SETTINGS_MSGCONF | \ UBX_CFG_CFG_SETTINGS_MSGCONF | \
@ -119,8 +84,6 @@ typedef enum {
#define UBX_ #define UBX_
typedef struct { typedef struct {
// bool autoconfigEnabled;
// bool storeSettings;
GPSSettingsUbxAutoConfigOptions UbxAutoConfig; GPSSettingsUbxAutoConfigOptions UbxAutoConfig;
bool SBASRanging; bool SBASRanging;
bool SBASCorrection; bool SBASCorrection;
@ -134,7 +97,6 @@ typedef struct {
bool enableGPS; bool enableGPS;
bool enableGLONASS; bool enableGLONASS;
bool enableBeiDou; bool enableBeiDou;
// bool configStoreAndDisable;
} ubx_autoconfig_settings_t; } ubx_autoconfig_settings_t;
// Sent messages for configuration support // Sent messages for configuration support

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

@ -72,13 +72,7 @@ typedef struct {
// - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly) // - 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 // - change Revo port baud rate
// sometimes fails (much worse for lowest baud rates) // sometimes fails (much worse for lowest baud rates)
// FIXME: remove this and retest // FIXME: remove this and retest when someone has time
// FIXME: remove this and retest
// FIXME: remove this and retest
// FIXME: remove this and retest
// FIXME: remove this and retest
// FIXME: remove this and retest
// FIXME: remove this and retest
uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+ uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
} __attribute__((packed)); } __attribute__((packed));
GPSSettingsData gpsSettings; GPSSettingsData gpsSettings;
@ -167,9 +161,7 @@ static volatile bool current_step_touched = false;
// both the pointer and what it points to are volatile. Yuk. // both the pointer and what it points to are volatile. Yuk.
static volatile status_t *volatile status = 0; static volatile status_t *volatile status = 0;
static uint8_t hwsettings_baud; static uint8_t hwsettings_baud;
//static HwSettingsGPSSpeedOptions hwsettings_baud;
static uint8_t baud_to_try_index = 255; static uint8_t baud_to_try_index = 255;
void debugindex(int index, uint8_t c);
static void append_checksum(UBXSentPacket_t *packet) static void append_checksum(UBXSentPacket_t *packet)
{ {
@ -233,18 +225,15 @@ void gps_ubx_reset_sensor_type()
{ {
static uint8_t mutex; // = 0 static uint8_t mutex; // = 0
// is this needed? // 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) { if (__sync_fetch_and_add(&mutex, 1) == 0) {
ubxHwVersion = -1; ubxHwVersion = -1;
baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
GPSPositionSensorSensorTypeSet(&sensorType); GPSPositionSensorSensorTypeSet(&sensorType);
// make the sensor type / autobaud code time out immediately to send the request immediately // make the sensor type / autobaud code time out immediately to send the request immediately
status->lastStepTimestampRaw += 0x8000000UL; status->lastStepTimestampRaw += 0x8000000UL;
} else {
static uint8_t c;
debugindex(8, ++c);
if (c > 254) c=254;
} }
--mutex; --mutex;
} }
@ -279,7 +268,6 @@ static void config_gps_baud(uint16_t *bytes_to_send)
// for protocol masks, bit 0 is UBX enable, bit 1 is NMEA enable // for protocol masks, bit 0 is UBX enable, bit 1 is NMEA enable
status->working_packet.message.payload.cfg_prt.inProtoMask = 1; // 1 = UBX only (bit 0) status->working_packet.message.payload.cfg_prt.inProtoMask = 1; // 1 = UBX only (bit 0)
// disable current UBX messages for low baud rates // 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; status->working_packet.message.payload.cfg_prt.outProtoMask = 1;
// Ask GPS to change it's speed // Ask GPS to change it's speed
status->working_packet.message.payload.cfg_prt.baudRate = hwsettings_gpsspeed_enum_to_baud(hwsettings_baud); status->working_packet.message.payload.cfg_prt.baudRate = hwsettings_gpsspeed_enum_to_baud(hwsettings_baud);
@ -464,6 +452,9 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
// permanently store our version of GPSSettings.UbxAutoConfig // permanently store our version of GPSSettings.UbxAutoConfig
// we use this to disable after ConfigStoreAndDisable is complete // we use this to disable after ConfigStoreAndDisable is complete
// FIXME: this does not store it permanently
// all that is needed is for the following to store it permanently like HomeLocation is stored by the flight code
// GPSSettingsUbxAutoConfigSet((GPSSettingsUbxAutoConfigOptions *) &status->currentSettings.UbxAutoConfig);
static void setGpsSettings() static void setGpsSettings()
{ {
// try casting it correctly and it says: // try casting it correctly and it says:
@ -475,141 +466,8 @@ static void setGpsSettings()
} }
// End User Documentation // 9600 baud and lower are not usable, and are best left at factory default
// if the user selects 9600
// 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
// There are microprocessors and they each have internal settings
// Revo
// GPS
// and each of these settings can be temporary or permanent
// To change a Revo setting
// Use the System tab in the GCS for all the following
// Example: in Settings->GPSSettings click on the VALUE for UbxAutoConfig and change it to Disabled
// Click on UbxAutoConfig itself and the line will turn green and blue
// To change this setting permanently, press the red up arrow (Save) at the top of the screen
// Permanently means that it uses this setting, even if you reboot Revo, e.g. power off and on
// To change this setting temporarily, press the green up arrow (Send) at the top of the screen
// Temporarily means that it overrides the permanent setting, but it goes back to the permanent setting when you reboot Revo, e.g. power off and on
// To change an internal GPS setting you use the OP GCS System tab to tell Revo to make the GPS changes
// This only works correctly after you have matching baud rates so Revo and GPS can talk together
// "Settings->GPSSettings->UbxAutoConfig = Configure" sets the internal GPS setting temporarily
// "Settings->GPSSettings->UbxAutoConfig = ConfigureAndStore" sets the internal GPS setting permanently
// You want to wind up with a set of permanent settings that work together
// There are two different sets of permanent settings that work together
// GPS at 9600 baud and factory defaults
// Revo configured to start out at 9600 baud, but then completely configure the GPS and switch both to 57600 baud
// (takes 6 seconds at boot up while you are waiting for it to acquire satellites anyway)
// This is the preferred way so that if we change the settings in the future, the new release will automatically use the correct settings
// GPS at 57600 baud with all the settings for the current release stored in the GPS
// Revo configured to disable UbxAutoConfig since all the GPS settings are permanently stored correctly
// May require reconfiguring in a future release
// Changable settings of interest
// AutoConfig mode
// Settings->GPSSettings->UbxAutoConfig (Disabled, Configure, ConfigureAndStore, default=Configure)
// Disabled means that changes to the GPS baud setting only affect the Revo port
// It doesn't try to change the GPS's internal baud rate setting
// Configure means change the GPS's internal baud setting temporarily (GPS settings revert to the permanent values when GPS is powered off/on)
// ConfigureAndStore means change the GPS's internal baud setting permanently (even after the GPS is powered off/on)
// GPS baud rate
// Settings->HwSettings->GPSSpeed
// If the baud rates are the same and an AutoConfig mode is enabled this will change both the GPS baud rate and the Revo baud rate
// If the baud rates are not the same and an AutoConfig mode is enabled it will fail
// If AutoConfig mode is disabled this will only change the Revo baud rate
// View only settings of interest
// Detected GPS type
// Data Objects -> GPSPositionSensor -> SensorType (Unknown, NMEA, UBX, UBX7, UBX8)
// When it says something other than Unknown, the GPS and Revo baud rates are synced and talking
// Real time progress of the GPS detection process
// Data Objects -> GPSPositionSensor -> AutoConfigStatus (DISABLED, RUNNING, DONE, ERROR)
// Syncing the baud rates means that the GPS's internal baud rate setting is the same as the Revo 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 NMEA message settings
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
// 4 Save the current volatile settings to non-volatile storage
// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGURE it will
// 2 Disable NMEA message settings
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
// If the requested baud rate is 9600 or less it skips the step (3) of adding some volatile UBX settings
// Talking points to point out:
// U-center is no longer needed for any use case with this code
// 9600 is factory default for GPS's
// Some GPS can't even permanently store settings and must start at 9600 baud?
// I have a GPS that sometimes looses settings and reverts to 9600 and this is a fix for that too :)
// This code handles a GPS configured either way (9600 with factory default settings or e.g. 57600 with OP settings)
// Autoconfig.nostore at each boot for 9600, autoconfig.disabled for the 57600 with OP settings (or custom settings and baud)
// This code can permanently configure a GPS to be e.g. 9600 with factory default settings or 57600 with OP settings
// GPS's with 9600 baud and factory default settings would be a good default for future OP releases
// Changing the GPS internal settings multiple times in the future is handled automatically
// This code is written to do a configure from 9600 to 57600
// (actually 9600 to whatever is stored in HwSettings.GPSSpeed)
// if autoconfig is enabled at boot up
// When autoconfiging to 9600 baud or lower, the autoconfig will configure it to factory default settings, not OP settings
// That is because 9600 baud drops many of the OP messages and because 9600 baud is factory default
// For 19200 baud or higher, the autoconfig will configure it to OP required settings
// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
// This is good for factory default GPS's
// This is good in case we change some settings in a future release
void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
@ -638,45 +496,20 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
// wait for the normal reply timeout before sending it over and over // wait for the normal reply timeout before sending it over and over
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_PARSER_TIMEOUT) { if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_PARSER_TIMEOUT) {
// 1 sec seems to work, except for just 230400 baud rates and then it takes 4 times
// retries for various timeouts
// 925-280-280 -> 2@2400
// 937-280-280 -> 1@2400
return; 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)
// 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)
// 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
// so this retry at this baud rate has failed // if we get here
// we need gps_ubx_reset_sensor_type() (or what else?) to reset this FSM (baud_to_try_index==-1) and the retry count // select the next baud rate, skipping ahead if new baud rate is HwSettings.GPSSpeed
// need a state machine that counts retries and: // set Revo baud rate to current++ value (immediate change so we can send right after that) and send the MON_VER request
// no, no retries, just loop through the baud rate over and over till it locks // baud rate search order are most likely matches first
// 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 // if AutoBaud or higher, do AutoBaud
// 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)
// if not Disabled, do AutoBaud
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) { if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) {
uint8_t baud_to_try; uint8_t baud_to_try;
//HwSettingsGPSSpeedOptions baud_to_try;
#if 1
static uint8_t baud_array[] = { static uint8_t baud_array[] = {
HWSETTINGS_GPSSPEED_57600, HWSETTINGS_GPSSPEED_57600,
HWSETTINGS_GPSSPEED_9600, HWSETTINGS_GPSSPEED_9600,
@ -700,27 +533,10 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
} }
// skip HwSettings.GPSSpeed when you run across it in the list // skip HwSettings.GPSSpeed when you run across it in the list
} while (baud_to_try == hwsettings_baud); } while (baud_to_try == hwsettings_baud);
#else
// no autobaud, just use current setting
HwSettingsGPSSpeedGet(&hwsettings_baud);
baud_to_try = hwsettings_baud;
#if 0
debugindex(0, hwsettings_baud);
{
static uint8_t c;
debugindex(1, ++c);
}
#endif
#endif
// set the FC (Revo) baud rate // set the FC (Revo) baud rate
gps_set_fc_baud_from_arg(baud_to_try, 2); gps_set_fc_baud_from_arg(baud_to_try);
} }
{
static uint8_t c;
debugindex(4, ++c);
}
// this code is executed even if ubxautoconfig is disabled // this code is executed even if ubxautoconfig is disabled
// it detects the "sensor type" = type of GPS // it detects the "sensor type" = type of GPS
// the user can use this to manually determine if the baud rate is correct // the user can use this to manually determine if the baud rate is correct
@ -730,21 +546,6 @@ debugindex(4, ++c);
return; return;
} }
#if 0
{
static uint8_t c;
uint8_t baud;
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(&baud);
}
}
#endif
if (!enabled) { if (!enabled) {
// keep resetting the timeouts here if we are not actually going to run the configure code // 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 // not really necessary, but it keeps the timer from wrapping every 50 seconds
@ -752,37 +553,12 @@ debugindex(4, ++c);
return; // autoconfig not enabled return; // autoconfig not enabled
} }
debugindex(5, status->currentStep); // FSM
switch (status->currentStep) { 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 (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
// 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 // if here, we have verified that the baud rates are in sync sometime in the past
case INIT_STEP_START: case INIT_STEP_START:
// we should look for the GPS version again (user may plug in a different GPS and then do autoconfig again) // 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) // zero retries for the next state that needs it (INIT_STEP_SAVE)
// not needed if MON_VER ack is a nop
// status->retryCount = 0;
set_current_step_if_untouched(INIT_STEP_SEND_MON_VER); set_current_step_if_untouched(INIT_STEP_SEND_MON_VER);
// fall through to next state // fall through to next state
// we can do that if we choose because we haven't sent any data in this state // we can do that if we choose because we haven't sent any data in this state
@ -792,45 +568,10 @@ debugindex(5, status->currentStep);
build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); 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 // 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); set_current_step_if_untouched(INIT_STEP_WAIT_MON_VER_ACK);
// just for test, remove
// it messes configure up, config has baud synced, and HwSettings.GPSSpeed is already set with new baud,
// so it configures to new baud before GPS has new baud
// gps_ubx_reset_sensor_type();
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break; break;
case INIT_STEP_WAIT_MON_VER_ACK: 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_PRE_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 // wait for previous step
// extra wait time might well be unnecessary but we want to make sure // extra wait time might well be unnecessary but we want to make sure
// that we don't stop waiting too soon // that we don't stop waiting too soon
@ -842,7 +583,6 @@ debugindex(5, ubxLastNak.msgID);
// fall through to next state // fall through to next state
// we can do that if we choose because we haven't sent any data in this state // we can do that if we choose because we haven't sent any data in this state
// break; // break;
#endif
// if here, we have just verified that the baud rates are in sync (again) // if here, we have just verified that the baud rates are in sync (again)
case INIT_STEP_RESET_GPS: case INIT_STEP_RESET_GPS:
@ -854,14 +594,6 @@ debugindex(5, ubxLastNak.msgID);
// Retrieve desired GPS baud rate once for use throughout this module // Retrieve desired GPS baud rate once for use throughout this module
HwSettingsGPSSpeedGet(&hwsettings_baud); 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) #if !defined(ALWAYS_RESET)
// ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore // 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 // but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore
@ -903,14 +635,8 @@ debugindex(3, ++c);
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
return; 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 // set the Revo GPS port to 9600 baud to match the reset to factory default that has already been done
gps_set_fc_baud_from_arg(HWSETTINGS_GPSSPEED_9600, 0); gps_set_fc_baud_from_arg(HWSETTINGS_GPSSPEED_9600);
} }
// at most, we just set Revo baud and that doesn't send any data // at most, we just set Revo baud and that doesn't send any data
// fall through to next state // fall through to next state
@ -936,7 +662,6 @@ debugindex(9, hwsettings_baud);
// so we ignore the ack from this. it has proven to be reliable (with the addition of two dummy bytes after the packet) // 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 // 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); config_gps_baud(bytes_to_send);
set_current_step_if_untouched(INIT_STEP_REVO_BAUD); set_current_step_if_untouched(INIT_STEP_REVO_BAUD);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
@ -949,14 +674,7 @@ debugindex(9, hwsettings_baud);
return; return;
} }
// set the Revo GPS port baud rate to the (same) user configured value // set the Revo GPS port baud rate to the (same) user configured value
#if 0 gps_set_fc_baud_from_arg(hwsettings_baud);
debugindex(4, hwsettings_baud);
{
static uint8_t c;
debugindex(5, ++c);
}
#endif
gps_set_fc_baud_from_arg(hwsettings_baud, 3);
status->lastConfigSent = LAST_CONFIG_SENT_START; status->lastConfigSent = LAST_CONFIG_SENT_START;
// zero the retries for the first "enable sentence" // zero the retries for the first "enable sentence"
status->retryCount = 0; status->retryCount = 0;
@ -1014,9 +732,6 @@ debugindex(5, ++c);
} else { } else {
// timeout or NAK, resend the message or abort // timeout or NAK, resend the message or abort
status->retryCount++; status->retryCount++;
debugindex(0, status->currentStep);
debugindex(1, status->lastConfigSent);
debugindex(2, status->retryCount);
if (status->retryCount > UBX_MAX_RETRIES) { if (status->retryCount > UBX_MAX_RETRIES) {
set_current_step_if_untouched(INIT_STEP_PRE_ERROR); set_current_step_if_untouched(INIT_STEP_PRE_ERROR);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
@ -1055,7 +770,6 @@ debugindex(2, status->retryCount);
} }
// fall through to next state // fall through to next state
// we can do that if we choose because we haven't sent any data in this state // we can do that if we choose because we haven't sent any data in this state
// we actually must fall through because the FSM won't be entered if DONE, ERROR, or DISABLED
set_current_step_if_untouched(INIT_STEP_PRE_DONE); set_current_step_if_untouched(INIT_STEP_PRE_DONE);
// break; // break;
@ -1066,7 +780,6 @@ debugindex(2, status->retryCount);
enabled = false; enabled = false;
status->currentSettings.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED; status->currentSettings.UbxAutoConfig = GPSSETTINGS_UBXAUTOCONFIG_DISABLED;
// like it says // like it says
// GPSSettingsUbxAutoConfigSet((GPSSettingsUbxAutoConfigOptions *) &status->currentSettings.UbxAutoConfig);
setGpsSettings(); setGpsSettings();
} }
set_current_step_if_untouched(INIT_STEP_DONE); set_current_step_if_untouched(INIT_STEP_DONE);
@ -1089,8 +802,6 @@ debugindex(2, status->retryCount);
// this can be called from a different thread // this can be called from a different thread
// so everything it touches must be declared volatile // 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) void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
{ {
initSteps_t new_step; initSteps_t new_step;