rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/amorale/OP-1499_autoconfig_retries' into next

Browse Source
This commit is contained in:
Alessio Morale 2014-09-29 21:30:16 +02:00
commit b2aa73acf6
4 changed files with 73 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -245,7 +245,9 @@ static void gpsTask(__attribute__((unused)) void *parameters)
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0;
uint16_t count = 0;
ubx_autoconfig_run(&buffer, &count);
uint8_t status;
GPSPositionSensorStatusGet(&status);
ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS);
// Something to send?
if (count) {
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
@ -266,11 +268,17 @@ static void gpsTask(__attribute__((unused)) void *parameters)
{
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
int32_t ac_status = ubx_autoconfig_get_status();
static uint8_t lastStatus = 0;
gpspositionsensor.AutoConfigStatus =
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
lastStatus = gpspositionsensor.AutoConfigStatus;
}
#endif
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
}
@ -290,7 +298,8 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_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.
uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;

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

@ -515,6 +515,14 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;
id = GPSPOSITIONSENSOR_OBJID;
} else {
uint8_t status;
GPSPositionSensorStatusGet(&status);
if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
// Some ubx thing has been received so GPS is there
status = GPSPOSITIONSENSOR_STATUS_NOFIX;
GPSPositionSensorStatusSet(&status);
}
}
return id;
}

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

@ -37,9 +37,16 @@
#define UBX_MAX_RATE_VER7 10
#define UBX_MAX_RATE 5
// time to wait before reinitializing the fsm due to disconnection
#define UBX_CONNECTION_TIMEOUT (2000 * 1000)
// times between retries in case an error does occurs
#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000)
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (500 * 1000)
// max retries in case of timeout
#define UBX_MAX_RETRIES 5
// pause between each configuration step
#define UBX_STEP_WAIT_TIME (10 * 1000)
// types
typedef enum {
UBX_AUTOCONFIG_STATUS_DISABLED = 0,
@ -179,7 +186,7 @@ typedef union {
} message;
} __attribute__((packed)) UBXSentPacket_t;
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected);
void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
int32_t ubx_autoconfig_get_status();
#endif /* UBX_AUTOCONFIG_H_ */

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

@ -46,6 +46,7 @@ typedef enum {
typedef struct {
initSteps_t currentStep; // Current configuration "fsm" status
uint32_t lastStepTimestampRaw; // timestamp of last operation
uint32_t lastConnectedRaw; // timestamp of last time gps was connected
UBXSentPacket_t working_packet; // outbound "buffer"
ubx_autoconfig_settings_t currentSettings;
int8_t lastConfigSent; // index of last configuration string sent
@ -227,7 +228,7 @@ void config_save(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
// mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
status->working_packet.message.payload.cfg_cfg.saveMask = 0x01 | 0x08; // msgConf + navConf
status->working_packet.message.payload.cfg_cfg.saveMask = 0x02 | 0x08; // msgConf + navConf
status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
@ -279,21 +280,50 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
}
}
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected)
{
*bytes_to_send = 0;
*buffer = (char *)status->working_packet.buffer;
if (!status || !enabled) {
if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) {
return; // autoconfig not enabled
}
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) {
return;
}
// when gps is disconnected it will replay the autoconfig sequence.
if (!gps_connected) {
if (status->currentStep == INIT_STEP_DONE) {
// if some operation is in progress and it is not running into issues it maybe that
// the disconnection is only due to wrong message rates, so reinit only when done.
// errors caused by disconnection are handled by error retry logic
if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) {
status->currentStep = INIT_STEP_START;
return;
}
}
} else {
// reset connection timer
status->lastConnectedRaw = PIOS_DELAY_GetRaw();
}
switch (status->currentStep) {
case INIT_STEP_ERROR: // TODO: what to do? retries after a while? maybe gps was disconnected (this can be detected)?
case INIT_STEP_ERROR:
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_ERROR_RETRY_TIMEOUT) {
status->currentStep = INIT_STEP_START;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
}
return;
case INIT_STEP_DISABLED:
case INIT_STEP_DONE:
return;
case INIT_STEP_START:
case INIT_STEP_ASK_VER:
// clear ack
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
status->currentStep = INIT_STEP_WAIT_VER;
@ -348,6 +378,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES) {
status->currentStep = INIT_STEP_ERROR;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
}
@ -374,12 +405,16 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
status->currentSettings = config;
status->currentStep = INIT_STEP_START;
enabled = true;
} else {
if (!status) {
status->currentStep = INIT_STEP_DISABLED;
}
}
}
int32_t ubx_autoconfig_get_status()
{
if (!status) {
if (!status || !enabled) {
return UBX_AUTOCONFIG_STATUS_DISABLED;
}
switch (status->currentStep) {