From 1846806d1785861b5ad0721332a7c16f7be4012d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 22 Aug 2014 10:10:21 +0200 Subject: [PATCH] OP-1370 - Refactor AutoConfig code out of UBX.* --- flight/modules/GPS/GPS.c | 3 +- flight/modules/GPS/UBX.c | 86 +++------------------ flight/modules/GPS/inc/UBX.h | 60 +-------------- flight/modules/GPS/inc/ubx_autoconfig.h | 91 +++++++++++++++++++++++ flight/modules/GPS/ubx_autoconfig.c | 99 +++++++++++++++++++++++++ 5 files changed, 207 insertions(+), 132 deletions(-) create mode 100644 flight/modules/GPS/inc/ubx_autoconfig.h create mode 100644 flight/modules/GPS/ubx_autoconfig.c diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index d191ac8a0..9e0b7b622 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -48,6 +48,7 @@ #include "GPS.h" #include "NMEA.h" #include "UBX.h" +#include "inc/ubx_autoconfig.h" // **************** @@ -233,7 +234,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { char *buffer = 0; uint16_t count = 0; - ubx_run_management_tasks(&buffer, &count); + ubx_autoconfig_run(&buffer, &count); // Something to send? if (count) { PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count); diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 0bc4fb26c..0761c31b8 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -47,8 +47,7 @@ static float mag_transform[3][3] = { static bool useMag = false; static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; -static struct UBX_ACK_ACK last_ack; -static struct UBX_ACK_NAK last_nak; + static bool usePvt = false; static uint32_t lastPvtTime = 0; @@ -101,19 +100,12 @@ const ubx_message_handler ubx_handler_table[] = { }; #define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table) -// COnfiguration support -typedef enum { - INIT_STEP_ASK_VER = 0, - INIT_STEP_WAIT_VER = 1, - INIT_STEP_CONFIGURE = 2, - INIT_STEP_DONE = 3, -} initSteps; - -static initSteps currentConfigurationStep; -// timestamp of last operation -static uint32_t lastStepTimestampRaw = 0; // detected hw version -static int32_t hwVersion = -1; +int32_t ubxHwVersion = -1; + +// Last received Ack/Nak +struct UBX_ACK_ACK ubxLastAck; +struct UBX_ACK_NAK ubxLastNak; // If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC #define UBX_PVT_TIMEOUT (1000) @@ -452,24 +444,24 @@ static void parse_ubx_ack_ack(struct UBXPacket *ubx, __attribute__((unused)) GPS { struct UBX_ACK_ACK *ack_ack = &ubx->payload.ack_ack; - last_ack = *ack_ack; + ubxLastAck = *ack_ack; } static void parse_ubx_ack_nak(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) { struct UBX_ACK_NAK *ack_nak = &ubx->payload.ack_nak; - last_nak = *ack_nak; + ubxLastNak = *ack_nak; } static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) { struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver; - hwVersion = atoi(mon_ver->hwVersion); + ubxHwVersion = atoi(mon_ver->hwVersion); - sensorType = (hwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : - ((hwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); + sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : + ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); } @@ -569,61 +561,5 @@ void load_mag_settings() } } -UBXSentPacket_t working_packet; -void append_checksum(UBXSentPacket_t *packet) -{ - uint8_t i; - uint8_t ck_a = 0; - uint8_t ck_b = 0; - uint16_t len = packet->message.header.len + sizeof(UBXSentHeader_t); - - for (i = 2; i < len; i++) { - ck_a += packet->buffer[i]; - ck_b += ck_a; - } - - 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) -{ - packet->message.header.prolog[0] = UBX_SYNC1; - packet->message.header.prolog[1] = UBX_SYNC2; - packet->message.header.class = classID; - packet->message.header.id = messageID; - packet->message.header.len = len; - append_checksum(packet); -} - -void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count) -{ - prepare_packet(packet, classID, messageID, 0); - *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2; -} - -void ubx_run_management_tasks(char * *buffer, uint16_t *count) -{ - uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw); - - switch (currentConfigurationStep) { - 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; - break; - case INIT_STEP_WAIT_VER: - if (hwVersion > 0) { - currentConfigurationStep = INIT_STEP_CONFIGURE; - } else if (elapsed > UBX_REPLY_TIMEOUT) { - currentConfigurationStep = INIT_STEP_ASK_VER; - } - return; - - case INIT_STEP_CONFIGURE: - case INIT_STEP_DONE: - break; - } -} #endif // PIOS_INCLUDE_GPS_UBX_PARSER diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 973bc1bd6..2889d52cc 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -352,66 +352,14 @@ struct UBXPacket { UBXPayload payload; }; -// Sent messages for configuration support - -typedef struct { - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint8_t cnoThreshNumSVs; - uint8_t cnoThresh; - uint16_t reserved2; - uint32_t reserved3; - uint32_t reserved4; -} ubx_cfg_nav5_t; - -typedef struct { - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; -} ubx_cfg_rate_t; - -typedef struct { - uint8_t msgClass; - uint8_t msgID; - uint8_t rate; -} ubx_cfg_msg_t; - -typedef struct { - uint8_t prolog[2]; - uint8_t class; - uint8_t id; - uint16_t len; -} UBXSentHeader_t; - -typedef union { - uint8_t buffer[0]; - struct { - UBXSentHeader_t header; - union { - ubx_cfg_nav5_t cfg_nav5; - ubx_cfg_rate_t cfg_rate; - ubx_cfg_msg_t cfg_msg; - } payload; - uint8_t resvd[2]; // added space for checksum bytes - } message; -} UBXSentPacket_t; - +// Used by AutoConfig code +extern int32_t ubxHwVersion; +extern struct UBX_ACK_ACK ubxLastAck; +extern struct UBX_ACK_NAK ubxLastNak; bool checksum_ubx_message(struct UBXPacket *); uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *); -void ubx_run_management_tasks(char * *buffer, uint16_t *count); int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); void load_mag_settings(); diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h new file mode 100644 index 000000000..272f1fdda --- /dev/null +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -0,0 +1,91 @@ +/** + ****************************************************************************** + * + * @file %FILENAME% + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @addtogroup [Group] + * @{ + * @addtogroup %CLASS% + * @{ + * @brief [Brief] + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef UBX_AUTOCONFIG_H_ +#define UBX_AUTOCONFIG_H_ +#include +#include +#include "UBX.h" + +// Sent messages for configuration support + +typedef struct { + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint8_t cnoThreshNumSVs; + uint8_t cnoThresh; + uint16_t reserved2; + uint32_t reserved3; + uint32_t reserved4; +} ubx_cfg_nav5_t; + +typedef struct { + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; +} ubx_cfg_rate_t; + +typedef struct { + uint8_t msgClass; + uint8_t msgID; + uint8_t rate; +} ubx_cfg_msg_t; + +typedef struct { + uint8_t prolog[2]; + uint8_t class; + uint8_t id; + uint16_t len; +} UBXSentHeader_t; + +typedef union { + uint8_t buffer[0]; + struct { + UBXSentHeader_t header; + union { + ubx_cfg_nav5_t cfg_nav5; + ubx_cfg_rate_t cfg_rate; + ubx_cfg_msg_t cfg_msg; + } payload; + uint8_t resvd[2]; // added space for checksum bytes + } message; +} UBXSentPacket_t; + +void ubx_autoconfig_run(char * *buffer, uint16_t *count); + +#endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c new file mode 100644 index 000000000..a68039fe6 --- /dev/null +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -0,0 +1,99 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Support code for UBX AutoConfig + * @{ + * + * @file ubx_autoconfig.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Support code for UBX AutoConfig + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include "inc/ubx_autoconfig.h" + +typedef enum { + INIT_STEP_ASK_VER = 0, + INIT_STEP_WAIT_VER = 1, + INIT_STEP_CONFIGURE = 2, + INIT_STEP_DONE = 3, +} initSteps; + +static initSteps currentConfigurationStep; +// timestamp of last operation +static uint32_t lastStepTimestampRaw = 0; + +UBXSentPacket_t working_packet; +void append_checksum(UBXSentPacket_t *packet) +{ + uint8_t i; + uint8_t ck_a = 0; + uint8_t ck_b = 0; + uint16_t len = packet->message.header.len + sizeof(UBXSentHeader_t); + + for (i = 2; i < len; i++) { + ck_a += packet->buffer[i]; + ck_b += ck_a; + } + + 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) +{ + packet->message.header.prolog[0] = UBX_SYNC1; + packet->message.header.prolog[1] = UBX_SYNC2; + packet->message.header.class = classID; + packet->message.header.id = messageID; + packet->message.header.len = len; + append_checksum(packet); +} + +void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count) +{ + prepare_packet(packet, classID, messageID, 0); + *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2; +} + + +void ubx_autoconfig_run(char * *buffer, uint16_t *count) +{ + uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw); + + switch (currentConfigurationStep) { + 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; + break; + case INIT_STEP_WAIT_VER: + if (ubxHwVersion > 0) { + currentConfigurationStep = INIT_STEP_CONFIGURE; + } else if (elapsed > UBX_REPLY_TIMEOUT) { + currentConfigurationStep = INIT_STEP_ASK_VER; + } + return; + + case INIT_STEP_CONFIGURE: + case INIT_STEP_DONE: + break; + } +}