diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 9e0b7b622..b9e6ee2fe 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -55,13 +55,18 @@ // Private functions static void gpsTask(void *parameters); -static void updateSettings(); +static void updateHwSettings(); #ifdef PIOS_GPS_SETS_HOMELOCATION static void setHomeLocation(GPSPositionSensorData *gpsData); static float GravityAccel(float latitude, float longitude, float altitude); #endif +#ifdef PIOS_INCLUDE_GPS_UBX_PARSER +void AuxMagSettingsUpdatedCb(UAVObjEvent *ev); +void updateGpsSettings(UAVObjEvent *ev); +#endif + // **************** // Private constants @@ -100,9 +105,7 @@ static uint32_t timeOfLastUpdateMs; #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) static struct GPS_RX_STATS gpsRxStats; #endif -#ifdef PIOS_INCLUDE_GPS_UBX_PARSER -void AuxMagSettingsUpdatedCb(UAVObjEvent *ev); -#endif + // **************** /** * Initialise the gps module @@ -166,7 +169,7 @@ int32_t GPSInitialize(void) AuxMagSettingsUpdatedCb(NULL); AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb); #endif - updateSettings(); + updateHwSettings(); #else if (gpsPort && gpsEnabled) { GPSPositionSensorInitialize(); @@ -178,7 +181,7 @@ int32_t GPSInitialize(void) #if defined(PIOS_GPS_SETS_HOMELOCATION) HomeLocationInitialize(); #endif - updateSettings(); + updateHwSettings(); } #endif /* if defined(REVOLUTION) */ @@ -196,7 +199,7 @@ int32_t GPSInitialize(void) gps_rx_buffer = NULL; } PIOS_Assert(gps_rx_buffer); - + GPSSettingsConnectCallback(updateGpsSettings); return 0; } @@ -227,6 +230,9 @@ static void gpsTask(__attribute__((unused)) void *parameters) timeOfLastCommandMs = timeNowMs; GPSPositionSensorGet(&gpspositionsensor); +#ifdef PIOS_INCLUDE_GPS_UBX_PARSER + updateGpsSettings(0); +#endif // Loop forever while (1) { uint8_t c; @@ -368,7 +374,7 @@ static void setHomeLocation(GPSPositionSensorData *gpsData) * like protocol, etc. Thus the HwSettings object which contains the * GPS port speed is used for now. */ -static void updateSettings() +static void updateHwSettings() { if (gpsPort) { // Retrieve settings @@ -404,12 +410,38 @@ static void updateSettings() } } } + #ifdef PIOS_INCLUDE_GPS_UBX_PARSER void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { load_mag_settings(); } -#endif + +void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) +{ + uint8_t ubxAutoConfig; + uint8_t ubxDynamicModel; + + ubx_autoconfig_settings_t newconfig; + + GPSSettingsUBXAutoConfigGet(&ubxAutoConfig); + GPSSettingsUBXRateGet(&newconfig.navRate); + GPSSettingsUBXDynamicModelGet(&ubxDynamicModel); + + newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_TRUE ? true : false; + newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : + ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY : + ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN : + ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE : + ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA : + ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G : + ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G : + ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G : + UBX_DYNMODEL_AIRBORNE1G; + ubx_autoconfig_set(newconfig); +} + +#endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */ /** * @} * @} diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 2889d52cc..fd9e3a77d 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -36,6 +36,9 @@ #include "auxmagsensor.h" #include "GPS.h" +#define UBX_HW_VERSION_8 80000 +#define UBX_HW_VERSION_7 70000 + #define UBX_REPLY_TIMEOUT (500 * 1000) #define UBX_SYNC1 0xb5 // UBX protocol synchronization characters diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 272f1fdda..379979916 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -30,6 +30,32 @@ #include #include #include "UBX.h" +// defines +// TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that. +#define UBX_MAX_RATE_VER8 18 +#define UBX_MAX_RATE_VER7 10 +#define UBX_MAX_RATE 5 + +// types + +// Enumeration options for field UBXDynamicModel + +typedef enum { + UBX_DYNMODEL_PORTABLE = 0, + UBX_DYNMODEL_STATIONARY = 2, + UBX_DYNMODEL_PEDESTRIAN = 3, + UBX_DYNMODEL_AUTOMOTIVE = 4, + UBX_DYNMODEL_SEA = 5, + UBX_DYNMODEL_AIRBORNE1G = 6, + UBX_DYNMODEL_AIRBORNE2G = 7, + UBX_DYNMODEL_AIRBORNE4G = 8 +} ubx_config_dynamicmodel_t; + +typedef struct { + bool autoconfigEnabled; + int8_t navRate; + ubx_config_dynamicmodel_t dynamicModel; +} ubx_autoconfig_settings_t; // Sent messages for configuration support @@ -52,26 +78,26 @@ typedef struct { uint16_t reserved2; uint32_t reserved3; uint32_t reserved4; -} ubx_cfg_nav5_t; +} __attribute__((packed)) ubx_cfg_nav5_t; typedef struct { uint16_t measRate; uint16_t navRate; uint16_t timeRef; -} ubx_cfg_rate_t; +} __attribute__((packed)) ubx_cfg_rate_t; typedef struct { uint8_t msgClass; uint8_t msgID; uint8_t rate; -} ubx_cfg_msg_t; +} __attribute__((packed)) ubx_cfg_msg_t; typedef struct { uint8_t prolog[2]; uint8_t class; uint8_t id; uint16_t len; -} UBXSentHeader_t; +} __attribute__((packed)) UBXSentHeader_t; typedef union { uint8_t buffer[0]; @@ -84,8 +110,9 @@ typedef union { } payload; uint8_t resvd[2]; // added space for checksum bytes } message; -} UBXSentPacket_t; +} __attribute__((packed)) UBXSentPacket_t; -void ubx_autoconfig_run(char * *buffer, uint16_t *count); +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send); +void ubx_autoconfig_set(ubx_autoconfig_settings_t config); #endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index a68039fe6..b45f8eccb 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -28,20 +28,39 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "inc/ubx_autoconfig.h" - +#include +// private type definitions typedef enum { - INIT_STEP_ASK_VER = 0, - INIT_STEP_WAIT_VER = 1, - INIT_STEP_CONFIGURE = 2, - INIT_STEP_DONE = 3, -} initSteps; + INIT_STEP_DISABLED = 0, + INIT_STEP_START, + INIT_STEP_ASK_VER, + INIT_STEP_WAIT_VER, + INIT_STEP_CONFIGURE, + INIT_STEP_ENABLE_SENTENCES, + INIT_STEP_DONE, +} initSteps_t; -static initSteps currentConfigurationStep; -// timestamp of last operation -static uint32_t lastStepTimestampRaw = 0; +typedef struct { + initSteps_t currentConfigurationStep; // Current configuration "fsm" status + uint32_t lastStepTimestampRaw; // timestamp of last operation + UBXSentPacket_t working_packet; // outbound "buffer" + ubx_autoconfig_settings_t currentSettings; + int8_t lastConfigSent; // index of last configuration string sent +} ubx_autoconfig_status_t; -UBXSentPacket_t working_packet; -void append_checksum(UBXSentPacket_t *packet) + +// private defines +#define LAST_CONFIG_SENT_START (-1) +#define LAST_CONFIG_SENT_COMPLETED (-2) + +// private variables + +// enable the autoconfiguration system +static bool enabled; + +static ubx_autoconfig_status_t *status = 0; + +static void append_checksum(UBXSentPacket_t *packet) { uint8_t i; uint8_t ck_a = 0; @@ -56,7 +75,11 @@ void append_checksum(UBXSentPacket_t *packet) packet->buffer[len] = ck_a; packet->buffer[len + 1] = ck_b; } -void prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len) +/** + * prepare a packet to be sent, fill the header and appends the checksum. + * return the total packet lenght comprising header and checksum + */ +static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len) { packet->message.header.prolog[0] = UBX_SYNC1; packet->message.header.prolog[1] = UBX_SYNC2; @@ -64,36 +87,126 @@ void prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, packet->message.header.id = messageID; packet->message.header.len = len; append_checksum(packet); + return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum } -void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count) +static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send) { - prepare_packet(packet, classID, messageID, 0); - *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2; + *bytes_to_send = prepare_packet(packet, classID, messageID, 0); } - -void ubx_autoconfig_run(char * *buffer, uint16_t *count) +void config_rate(uint16_t *bytes_to_send) { - uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw); + memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t)); + // if rate is less than 1 uses the highest rate for current hardware + uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99; + if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) { + rate = UBX_MAX_RATE; + } else if (ubxHwVersion < UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER7) { + rate = UBX_MAX_RATE_VER7; + } else if (ubxHwVersion >= UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER8) { + rate = UBX_MAX_RATE_VER8; + } + uint16_t period = 1000 / rate; - switch (currentConfigurationStep) { + status->working_packet.message.payload.cfg_rate.measRate = period; + 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 + *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); +} + +void config_nav(uint16_t *bytes_to_send) +{ + memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t)); + + status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel; + status->working_packet.message.payload.cfg_nav5.fixMode = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D + // mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB + + 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)); +} + +static void configure(uint16_t *bytes_to_send) +{ + switch (status->lastConfigSent) { + case LAST_CONFIG_SENT_START: + config_rate(bytes_to_send); + break; + case LAST_CONFIG_SENT_START + 1: + config_nav(bytes_to_send); + status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; + return; + + default: + status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; + return; + } + status->lastConfigSent++; +} + +static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) {} +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) +{ + *bytes_to_send = 0; + *buffer = (char *)status->working_packet.buffer; + if (!status || !enabled) { + return; // autoconfig not enabled + } + switch (status->currentConfigurationStep) { + case INIT_STEP_DISABLED: + case INIT_STEP_DONE: + return; + + case INIT_STEP_START: case INIT_STEP_ASK_VER: - lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - build_request(&working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, count); - *buffer = (char *)working_packet.buffer; - currentConfigurationStep = INIT_STEP_WAIT_VER; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); + status->currentConfigurationStep = INIT_STEP_WAIT_VER; break; case INIT_STEP_WAIT_VER: + if (ubxHwVersion > 0) { - currentConfigurationStep = INIT_STEP_CONFIGURE; - } else if (elapsed > UBX_REPLY_TIMEOUT) { - currentConfigurationStep = INIT_STEP_ASK_VER; + status->lastConfigSent = LAST_CONFIG_SENT_START; + status->currentConfigurationStep = INIT_STEP_CONFIGURE; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + return; + } + + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) { + status->currentConfigurationStep = INIT_STEP_ASK_VER; } return; case INIT_STEP_CONFIGURE: - case INIT_STEP_DONE: - break; + configure(bytes_to_send); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + status->currentConfigurationStep = INIT_STEP_ENABLE_SENTENCES; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + return; + + case INIT_STEP_ENABLE_SENTENCES: + enable_sentences(bytes_to_send); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + status->currentConfigurationStep = INIT_STEP_DONE; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + return; + } +} + +void ubx_autoconfig_set(ubx_autoconfig_settings_t config) +{ + enabled = false; + if (config.autoconfigEnabled) { + if (!status) { + status = (ubx_autoconfig_status_t *)pios_malloc(sizeof(ubx_autoconfig_status_t)); + memset(status, 0, sizeof(ubx_autoconfig_status_t)); + status->currentConfigurationStep = INIT_STEP_DISABLED; + } + status->currentSettings = config; + status->currentConfigurationStep = INIT_STEP_START; + enabled = true; } }