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 - Refactor AutoConfig code out of UBX.*

Browse Source
This commit is contained in:
Alessio Morale 2014-08-22 10:10:21 +02:00
parent af9dce3569
commit 1846806d17
5 changed files with 207 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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);

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

@ -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

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

@ -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();

91
flight/modules/GPS/inc/ubx_autoconfig.h Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#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_ */

99
flight/modules/GPS/ubx_autoconfig.c Normal file
View File

@ -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;
}
}