rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1370 - Add configuration support for nav rate and dynamic model.

Browse Source 
Added stub for sentence rate configuration (enable_sentences(...))
This commit is contained in:
Alessio Morale 2014-08-22 19:00:46 +02:00
parent 1846806d17
commit f430af83c5
4 changed files with 218 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 */
/**
* @}
* @}

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

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

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

@ -30,6 +30,32 @@
#include <stdint.h>
#include <stdbool.h>
#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_ */

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

@ -28,20 +28,39 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/ubx_autoconfig.h"
#include <pios_mem.h>
// 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;
}
}