rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1370 - Add automatic sentence rate configuration based on HW version (llh+velned etc. vs PVT)

Browse Source
This commit is contained in:
Alessio Morale 2014-08-22 21:27:50 +02:00
parent f430af83c5
commit 69b76b665c
3 changed files with 104 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,8 +39,6 @@
#define UBX_HW_VERSION_8 80000 #define UBX_HW_VERSION_8 80000
#define UBX_HW_VERSION_7 70000 #define UBX_HW_VERSION_7 70000
#define UBX_REPLY_TIMEOUT (500 * 1000)
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters #define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62 #define UBX_SYNC2 0x62

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

@ -36,10 +36,10 @@
#define UBX_MAX_RATE_VER7 10 #define UBX_MAX_RATE_VER7 10
#define UBX_MAX_RATE 5 #define UBX_MAX_RATE 5
// types #define UBX_REPLY_TIMEOUT (500 * 1000)
#define UBX_MAX_RETRIES 5
// Enumeration options for field UBXDynamicModel // Enumeration options for field UBXDynamicModel
typedef enum { typedef enum {
UBX_DYNMODEL_PORTABLE = 0, UBX_DYNMODEL_PORTABLE = 0,
UBX_DYNMODEL_STATIONARY = 2, UBX_DYNMODEL_STATIONARY = 2,

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

@ -36,19 +36,43 @@ typedef enum {
INIT_STEP_ASK_VER, INIT_STEP_ASK_VER,
INIT_STEP_WAIT_VER, INIT_STEP_WAIT_VER,
INIT_STEP_CONFIGURE, INIT_STEP_CONFIGURE,
INIT_STEP_CONFIGURE_WAIT_ACK,
INIT_STEP_ENABLE_SENTENCES, INIT_STEP_ENABLE_SENTENCES,
INIT_STEP_ENABLE_SENTENCES_WAIT_ACK,
INIT_STEP_DONE, INIT_STEP_DONE,
INIT_STEP_ERROR,
} initSteps_t; } initSteps_t;
typedef struct { typedef struct {
initSteps_t currentConfigurationStep; // Current configuration "fsm" status initSteps_t currentStep; // Current configuration "fsm" status
uint32_t lastStepTimestampRaw; // timestamp of last operation uint32_t lastStepTimestampRaw; // timestamp of last operation
UBXSentPacket_t working_packet; // outbound "buffer" UBXSentPacket_t working_packet; // outbound "buffer"
ubx_autoconfig_settings_t currentSettings; 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;
} ubx_autoconfig_status_t; } ubx_autoconfig_status_t;
ubx_cfg_msg_t msg_config_ubx6[] = {
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 },
};
ubx_cfg_msg_t msg_config_ubx7[] = {
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 0 },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 },
};
// private defines // private defines
#define LAST_CONFIG_SENT_START (-1) #define LAST_CONFIG_SENT_START (-1)
#define LAST_CONFIG_SENT_COMPLETED (-2) #define LAST_CONFIG_SENT_COMPLETED (-2)
@ -113,6 +137,8 @@ void config_rate(uint16_t *bytes_to_send)
status->working_packet.message.payload.cfg_rate.navRate = 1; // must be set to 1 status->working_packet.message.payload.cfg_rate.navRate = 1; // must be set to 1
status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_RATE;
} }
void config_nav(uint16_t *bytes_to_send) void config_nav(uint16_t *bytes_to_send)
@ -125,6 +151,8 @@ void config_nav(uint16_t *bytes_to_send)
status->working_packet.message.payload.cfg_nav5.mask = 0x01 + 0x04; // Dyn Model | posFixMode configuration status->working_packet.message.payload.cfg_nav5.mask = 0x01 + 0x04; // Dyn Model | posFixMode configuration
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t)); *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_NAV5;
} }
static void configure(uint16_t *bytes_to_send) static void configure(uint16_t *bytes_to_send)
@ -142,10 +170,26 @@ static void configure(uint16_t *bytes_to_send)
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
return; return;
} }
status->lastConfigSent++;
} }
static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) {} static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
{
int8_t msg = status->lastConfigSent + 1;
uint8_t msg_count = (ubxHwVersion >= UBX_HW_VERSION_7) ?
NELEMENTS(msg_config_ubx7) : NELEMENTS(msg_config_ubx6);
ubx_cfg_msg_t *msg_config = (ubxHwVersion >= UBX_HW_VERSION_7) ?
&msg_config_ubx7[0] : &msg_config_ubx6[0];
if (msg >= 0 && msg < msg_count) {
status->working_packet.message.payload.cfg_msg = msg_config[msg];
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_MSG;
} else {
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
}
}
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
{ {
*bytes_to_send = 0; *bytes_to_send = 0;
@ -153,47 +197,80 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
if (!status || !enabled) { if (!status || !enabled) {
return; // autoconfig not enabled return; // autoconfig not enabled
} }
switch (status->currentConfigurationStep) { 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_DISABLED: case INIT_STEP_DISABLED:
case INIT_STEP_DONE: case INIT_STEP_DONE:
return; return;
case INIT_STEP_START: case INIT_STEP_START:
case INIT_STEP_ASK_VER: case INIT_STEP_ASK_VER:
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
status->currentConfigurationStep = INIT_STEP_WAIT_VER; status->currentStep = INIT_STEP_WAIT_VER;
break; break;
case INIT_STEP_WAIT_VER:
case INIT_STEP_WAIT_VER:
if (ubxHwVersion > 0) { if (ubxHwVersion > 0) {
status->lastConfigSent = LAST_CONFIG_SENT_START; status->lastConfigSent = LAST_CONFIG_SENT_START;
status->currentConfigurationStep = INIT_STEP_CONFIGURE; status->currentStep = INIT_STEP_CONFIGURE;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return; return;
} }
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) { if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
status->currentConfigurationStep = INIT_STEP_ASK_VER; status->currentStep = INIT_STEP_ASK_VER;
}
return;
case INIT_STEP_CONFIGURE:
configure(bytes_to_send);
if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
status->currentConfigurationStep = INIT_STEP_ENABLE_SENTENCES;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
} }
return; return;
case INIT_STEP_ENABLE_SENTENCES: case INIT_STEP_ENABLE_SENTENCES:
enable_sentences(bytes_to_send); case INIT_STEP_CONFIGURE:
{
bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE);
if (step_configure) {
configure(bytes_to_send);
} else {
enable_sentences(bytes_to_send);
}
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
status->currentConfigurationStep = INIT_STEP_DONE; status->lastConfigSent = LAST_CONFIG_SENT_START;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); status->currentStep = step_configure ? INIT_STEP_ENABLE_SENTENCES : INIT_STEP_DONE;
} else {
status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK;
} }
return; return;
} }
case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK:
case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS
{
bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK);
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
// clear ack
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
// Continue with next configuration option
status->retryCount = 0;
status->lastConfigSent++;
} else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
// timeout, resend the message or abort
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES) {
status->currentStep = INIT_STEP_ERROR;
return;
}
}
// no abort condition, continue or retries.,
if (step_configure) {
status->currentStep = INIT_STEP_CONFIGURE;
} else {
status->currentStep = INIT_STEP_ENABLE_SENTENCES;
}
return;
}
}
} }
void ubx_autoconfig_set(ubx_autoconfig_settings_t config) void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
@ -203,10 +280,10 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
if (!status) { if (!status) {
status = (ubx_autoconfig_status_t *)pios_malloc(sizeof(ubx_autoconfig_status_t)); status = (ubx_autoconfig_status_t *)pios_malloc(sizeof(ubx_autoconfig_status_t));
memset(status, 0, sizeof(ubx_autoconfig_status_t)); memset(status, 0, sizeof(ubx_autoconfig_status_t));
status->currentConfigurationStep = INIT_STEP_DISABLED; status->currentStep = INIT_STEP_DISABLED;
} }
status->currentSettings = config; status->currentSettings = config;
status->currentConfigurationStep = INIT_STEP_START; status->currentStep = INIT_STEP_START;
enabled = true; enabled = true;
} }
} }