mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-27 16:54:15 +01:00
OP-1464 remove commented out or unused code and pretty it up
This commit is contained in:
Cliff Geerdes
parent
1ade8b7f59
commit
3bbe7274ca
@ -50,7 +50,6 @@
|
||||
#include "UBX.h"
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
#include "inc/ubx_autoconfig.h"
|
||||
// #include "../../libraries/inc/fifo_buffer.h"
|
||||
#endif
|
||||
|
||||
#include <pios_instrumentation_helper.h>
|
||||
@ -88,18 +87,18 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
|
||||
|
||||
#ifdef PIOS_GPS_SETS_HOMELOCATION
|
||||
// Unfortunately need a good size stack for the WMM calculation
|
||||
#define STACK_SIZE_BYTES 1024+64
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
#else
|
||||
#if defined(PIOS_GPS_MINIMAL)
|
||||
#define GPS_READ_BUFFER 32
|
||||
|
||||
#ifdef PIOS_INCLUDE_GPS_NMEA_PARSER
|
||||
#define STACK_SIZE_BYTES 580+64 // NMEA
|
||||
#define STACK_SIZE_BYTES 580 // NMEA
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 440+64 // UBX
|
||||
#define STACK_SIZE_BYTES 440 // UBX
|
||||
#endif // PIOS_INCLUDE_GPS_NMEA_PARSER
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 650+64
|
||||
#define STACK_SIZE_BYTES 650
|
||||
#endif // PIOS_GPS_MINIMAL
|
||||
#endif // PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
@ -114,14 +113,12 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
|
||||
|
||||
static GPSSettingsData gpsSettings;
|
||||
|
||||
//static uint32_t gpsPort;
|
||||
#define gpsPort PIOS_COM_GPS
|
||||
static bool gpsEnabled = false;
|
||||
|
||||
static xTaskHandle gpsTaskHandle;
|
||||
|
||||
static char *gps_rx_buffer;
|
||||
static uint16_t gps_rx_buffer_size;
|
||||
|
||||
static uint32_t timeOfLastCommandMs;
|
||||
static uint32_t timeOfLastUpdateMs;
|
||||
@ -140,12 +137,10 @@ static struct GPS_RX_STATS gpsRxStats;
|
||||
int32_t GPSStart(void)
|
||||
{
|
||||
if (gpsEnabled) {
|
||||
// if (gpsPort) {
|
||||
// Start gps task
|
||||
xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle);
|
||||
return 0;
|
||||
// }
|
||||
// Start gps task
|
||||
xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle);
|
||||
return 0;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
@ -158,8 +153,6 @@ int32_t GPSStart(void)
|
||||
|
||||
int32_t GPSInitialize(void)
|
||||
{
|
||||
// gpsPort = PIOS_COM_GPS;
|
||||
|
||||
HwSettingsInitialize();
|
||||
#ifdef MODULE_GPS_BUILTIN
|
||||
gpsEnabled = true;
|
||||
@ -199,7 +192,6 @@ int32_t GPSInitialize(void)
|
||||
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
|
||||
updateHwSettings(0);
|
||||
#else /* if defined(REVOLUTION) */
|
||||
// if (gpsPort && gpsEnabled) {
|
||||
if (gpsEnabled) {
|
||||
GPSPositionSensorInitialize();
|
||||
GPSVelocitySensorInitialize();
|
||||
@ -218,19 +210,15 @@ int32_t GPSInitialize(void)
|
||||
}
|
||||
#endif /* if defined(REVOLUTION) */
|
||||
|
||||
// if (gpsPort && gpsEnabled) {
|
||||
if (gpsEnabled) {
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
gps_rx_buffer_size = (sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH;
|
||||
gps_rx_buffer = pios_malloc(gps_rx_buffer_size);
|
||||
gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
|
||||
#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
gps_rx_buffer_size = sizeof(struct UBXPacket);
|
||||
gps_rx_buffer = pios_malloc(gps_rx_buffer_size);
|
||||
gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
|
||||
#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
|
||||
gps_rx_buffer_size = NMEA_MAX_PACKET_LENGTH;
|
||||
gps_rx_buffer = pios_malloc(gps_rx_buffer_size);
|
||||
gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
|
||||
#else
|
||||
gps_rx_buffer = NULL;
|
||||
gps_rx_buffer = NULL;
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
PIOS_Assert(gps_rx_buffer);
|
||||
@ -260,7 +248,6 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
// that is 1/180 = 0.005555556 seconds per packet
|
||||
// we must never wait more than 5ms since packet was last drained or it may overflow
|
||||
// 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible
|
||||
// portTickType xDelay = 100 / portTICK_RATE_MS;
|
||||
portTickType xDelay = 5 / portTICK_RATE_MS;
|
||||
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
|
||||
@ -283,121 +270,120 @@ 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+8];
|
||||
uint8_t c[GPS_READ_BUFFER];
|
||||
|
||||
// Loop forever
|
||||
while (1) {
|
||||
if (gpsPort) {
|
||||
if (gpsPort) {
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
// do autoconfig stuff for UBX GPS's
|
||||
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
|
||||
char *buffer = 0;
|
||||
uint16_t count = 0;
|
||||
// do autoconfig stuff for UBX GPS's
|
||||
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
|
||||
char *buffer = 0;
|
||||
uint16_t count = 0;
|
||||
|
||||
gps_ubx_autoconfig_run(&buffer, &count);
|
||||
// Something to send?
|
||||
if (count) {
|
||||
// clear ack/nak
|
||||
ubxLastAck.clsID = 0x00;
|
||||
ubxLastAck.msgID = 0x00;
|
||||
ubxLastNak.clsID = 0x00;
|
||||
ubxLastNak.msgID = 0x00;
|
||||
gps_ubx_autoconfig_run(&buffer, &count);
|
||||
// Something to send?
|
||||
if (count) {
|
||||
// clear ack/nak
|
||||
ubxLastAck.clsID = 0x00;
|
||||
ubxLastAck.msgID = 0x00;
|
||||
ubxLastNak.clsID = 0x00;
|
||||
ubxLastNak.msgID = 0x00;
|
||||
|
||||
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
|
||||
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// need to do this whether there is received data or not, or the status (e.g. gcs) will not always be correct
|
||||
int32_t ac_status = ubx_autoconfig_get_status();
|
||||
static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
|
||||
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
|
||||
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
|
||||
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
|
||||
gpspositionsensor.AutoConfigStatus =
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
|
||||
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
|
||||
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
|
||||
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
|
||||
lastStatus = gpspositionsensor.AutoConfigStatus;
|
||||
}
|
||||
// need to do this whether there is received data or not, or the status (e.g. gcs) will not always be correct
|
||||
int32_t ac_status = ubx_autoconfig_get_status();
|
||||
static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
|
||||
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
|
||||
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
|
||||
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
|
||||
gpspositionsensor.AutoConfigStatus =
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
|
||||
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
|
||||
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
|
||||
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
|
||||
lastStatus = gpspositionsensor.AutoConfigStatus;
|
||||
}
|
||||
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
|
||||
|
||||
uint16_t cnt;
|
||||
// This blocks the task until there is something on the buffer (or 100ms? passes)
|
||||
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
|
||||
if (cnt > 0) {
|
||||
PERF_TIMED_SECTION_START(counterParse);
|
||||
PERF_TRACK_VALUE(counterBytesIn, cnt);
|
||||
PERF_MEASURE_PERIOD(counterRate);
|
||||
int res;
|
||||
switch (gpsSettings.DataProtocol) {
|
||||
uint16_t cnt;
|
||||
// This blocks the task until there is something on the buffer (or 100ms? passes)
|
||||
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
|
||||
if (cnt > 0) {
|
||||
PERF_TIMED_SECTION_START(counterParse);
|
||||
PERF_TRACK_VALUE(counterBytesIn, cnt);
|
||||
PERF_MEASURE_PERIOD(counterRate);
|
||||
int res;
|
||||
switch (gpsSettings.DataProtocol) {
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
|
||||
case GPSSETTINGS_DATAPROTOCOL_NMEA:
|
||||
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
case GPSSETTINGS_DATAPROTOCOL_NMEA:
|
||||
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
case GPSSETTINGS_DATAPROTOCOL_UBX:
|
||||
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
case GPSSETTINGS_DATAPROTOCOL_UBX:
|
||||
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
res = NO_PARSER; // this should not happen
|
||||
break;
|
||||
}
|
||||
PERF_TIMED_SECTION_END(counterParse);
|
||||
default:
|
||||
res = NO_PARSER; // this should not happen
|
||||
break;
|
||||
}
|
||||
PERF_TIMED_SECTION_END(counterParse);
|
||||
|
||||
if (res == PARSER_COMPLETE) {
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
timeOfLastUpdateMs = timeNowMs;
|
||||
timeOfLastCommandMs = timeNowMs;
|
||||
if (res == PARSER_COMPLETE) {
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
timeOfLastUpdateMs = timeNowMs;
|
||||
timeOfLastCommandMs = timeNowMs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Check for GPS timeout
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
|
||||
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
|
||||
// we have not received any valid GPS sentences for a while.
|
||||
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
|
||||
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
|
||||
GPSPositionSensorStatusSet(&status);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else {
|
||||
// we appear to be receiving GPS sentences OK, we've had an update
|
||||
// criteria for GPS-OK taken from this post...
|
||||
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
|
||||
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
|
||||
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
|
||||
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
|
||||
// Check for GPS timeout
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
|
||||
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
|
||||
// we have not received any valid GPS sentences for a while.
|
||||
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
|
||||
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
|
||||
GPSPositionSensorStatusSet(&status);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else {
|
||||
// we appear to be receiving GPS sentences OK, we've had an update
|
||||
// criteria for GPS-OK taken from this post...
|
||||
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
|
||||
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
|
||||
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
|
||||
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
|
||||
#ifdef PIOS_GPS_SETS_HOMELOCATION
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
if (home.Set == HOMELOCATION_SET_FALSE) {
|
||||
if (homelocationSetDelay == 0) {
|
||||
homelocationSetDelay = xTaskGetTickCount();
|
||||
}
|
||||
if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) {
|
||||
setHomeLocation(&gpspositionsensor);
|
||||
if (home.Set == HOMELOCATION_SET_FALSE) {
|
||||
if (homelocationSetDelay == 0) {
|
||||
homelocationSetDelay = xTaskGetTickCount();
|
||||
}
|
||||
if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) {
|
||||
setHomeLocation(&gpspositionsensor);
|
||||
homelocationSetDelay = 0;
|
||||
}
|
||||
} else {
|
||||
homelocationSetDelay = 0;
|
||||
}
|
||||
} else {
|
||||
homelocationSetDelay = 0;
|
||||
}
|
||||
#endif
|
||||
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
|
||||
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
|
||||
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
}
|
||||
}
|
||||
} // if (gpsPort)
|
||||
} // if (gpsPort)
|
||||
vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
|
||||
} // while (1)
|
||||
}
|
||||
@ -512,7 +498,7 @@ void gps_set_fc_baud_from_arg(uint8_t baud)
|
||||
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
|
||||
static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
|
||||
{
|
||||
static uint32_t previousGpsPort=0xf0f0f0f0; // = 0 means deconfigured and at power up it is deconfigured
|
||||
static uint32_t previousGpsPort=0xf0f0f0f0; // = doesn't match gpsport
|
||||
// if GPS (UBX or NMEA) is enabled at all
|
||||
if (gpsPort && gpsEnabled) {
|
||||
// on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate
|
||||
|
||||
@ -65,21 +65,22 @@ typedef struct {
|
||||
struct {
|
||||
union {
|
||||
struct {
|
||||
UBXSentPacket_t working_packet; // outbound "buffer"
|
||||
// bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
|
||||
// failure mode otherwise:
|
||||
// - send message with baud rate change
|
||||
// - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly)
|
||||
// - change Revo port baud rate
|
||||
// sometimes fails (much worse for lowest baud rates)
|
||||
// FIXME: remove this and retest when someone has time
|
||||
uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
|
||||
UBXSentPacket_t working_packet; // outbound "buffer"
|
||||
// bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
|
||||
// failure mode otherwise:
|
||||
// - send message with baud rate change
|
||||
// - wait 1 second (even at 2400, the baud rate change command
|
||||
// - should clear even an initially full 31 byte PIOS buffer much more quickly)
|
||||
// - change Revo port baud rate
|
||||
// sometimes fails (much worse for lowest baud rates)
|
||||
// FIXME: remove this and retest when someone has time
|
||||
uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
|
||||
} __attribute__((packed));
|
||||
GPSSettingsData gpsSettings;
|
||||
GPSSettingsData gpsSettings;
|
||||
} __attribute__((packed));
|
||||
} __attribute__((packed));
|
||||
volatile ubx_autoconfig_settings_t currentSettings;
|
||||
int8_t lastConfigSent; // index of last configuration string sent
|
||||
int8_t lastConfigSent; // index of last configuration string sent
|
||||
struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
|
||||
uint8_t retryCount;
|
||||
} status_t;
|
||||
@ -163,6 +164,8 @@ static volatile status_t *volatile status = 0;
|
||||
static uint8_t hwsettings_baud;
|
||||
static uint8_t baud_to_try_index = 255;
|
||||
|
||||
// functions
|
||||
|
||||
static void append_checksum(UBXSentPacket_t *packet)
|
||||
{
|
||||
uint8_t i;
|
||||
@ -210,7 +213,7 @@ static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t mess
|
||||
static void set_current_step_if_untouched(initSteps_t new_steps)
|
||||
{
|
||||
// assume this one byte initSteps_t is atomic
|
||||
// take care of some but not all concurrency issues
|
||||
// take care of some concurrency issues
|
||||
|
||||
if (!current_step_touched) {
|
||||
status->currentStep = new_steps;
|
||||
@ -259,7 +262,6 @@ 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));
|
||||
@ -420,7 +422,7 @@ static void configure(uint16_t *bytes_to_send)
|
||||
// Skip and fall through to next step
|
||||
status->lastConfigSent++;
|
||||
}
|
||||
// in the else case we must fall through because we must send something each time because successful send is tested externally
|
||||
// in the else case we must fall through because we must send something each time because successful send is tested externally
|
||||
|
||||
case LAST_CONFIG_SENT_START + 3:
|
||||
config_sbas(bytes_to_send);
|
||||
@ -451,7 +453,7 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
|
||||
|
||||
|
||||
// permanently store our version of GPSSettings.UbxAutoConfig
|
||||
// we use this to disable after ConfigStoreAndDisable is complete
|
||||
// we use this to disable after AbConfigStoreAndDisable 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);
|
||||
@ -468,8 +470,6 @@ static void setGpsSettings()
|
||||
|
||||
// 9600 baud and lower are not usable, and are best left at factory default
|
||||
// if the user selects 9600
|
||||
|
||||
|
||||
void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
{
|
||||
*bytes_to_send = 0;
|
||||
@ -481,9 +481,11 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
return;
|
||||
}
|
||||
|
||||
// get UBX version whether autoconfig is enabled or not
|
||||
// this allows the user to try some baud rates and visibly see when it works
|
||||
// get UBX version whether autobaud / autoconfig is enabled or not
|
||||
// this allows the user to manually try some baud rates and visibly see when it works
|
||||
// it also is how the autobaud code determines when the baud rate is correct
|
||||
// ubxHwVersion is a global set externally by the caller of this function
|
||||
// it is set when the GPS responds to a MON_VER message
|
||||
if (ubxHwVersion <= 0) {
|
||||
// at low baud rates and high data rates the ubx gps simply must drop some outgoing data
|
||||
// this isn't really an error
|
||||
@ -521,8 +523,10 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
HWSETTINGS_GPSSPEED_2400
|
||||
};
|
||||
|
||||
// first try HwSettings.GPSSpeed and then
|
||||
// get the next baud rate to try from the table, but skip over the value of HwSettings.GPSSpeed
|
||||
do {
|
||||
// index is inited to be out of bounds, which is interpreted as "currently defined baud rate" (= HwSettigns.GPSSpeed)
|
||||
// index is inited to be out of bounds, which is interpreted as "currently defined baud rate" (= HwSettings.GPSSpeed)
|
||||
if (baud_to_try_index >= sizeof(baud_array)/sizeof(baud_array[0])) {
|
||||
HwSettingsGPSSpeedGet(&hwsettings_baud);
|
||||
baud_to_try = hwsettings_baud;
|
||||
@ -553,7 +557,9 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
return; // autoconfig not enabled
|
||||
}
|
||||
|
||||
////////
|
||||
// FSM
|
||||
////////
|
||||
switch (status->currentStep) {
|
||||
// if here, we have verified that the baud rates are in sync sometime in the past
|
||||
case INIT_STEP_START:
|
||||
@ -597,8 +603,8 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
#if !defined(ALWAYS_RESET)
|
||||
// ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore
|
||||
// but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore
|
||||
// if user requests a low baud rate then we just reset and leave it set to NMEA
|
||||
// because low baud and high OP data rate doesn't play nice
|
||||
// if user requests a low baud rate then we just reset and avoid adding navigation sentences
|
||||
// because low GPS baud and high OP data rate doesn't play nice
|
||||
// if user requests that settings be saved, we will reset here too
|
||||
// that makes sure that all strange settings are reset to factory default
|
||||
// else these strange settings may persist because we don't reset all settings by table
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user