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-1434 - Support for mag calibration

Browse Source
This commit is contained in:
Alessio Morale 2014-07-17 20:43:16 +02:00
parent 397bfdc08b
commit ce1e4e728e
4 changed files with 49 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -98,7 +98,9 @@ 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 AuxMagCalibrationUpdatedCb(UAVObjEvent *ev);
#endif
// ****************
/**
* Initialise the gps module
@ -153,10 +155,16 @@ int32_t GPSInitialize(void)
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
AuxMagSensorInitialize();
AuxMagCalibrationInitialize();
GPSExtendedStatusInitialize();
updateSettings();
// Initialize mag parameters
AuxMagCalibrationUpdatedCb(NULL);
AuxMagCalibrationConnectCallback(AuxMagCalibrationUpdatedCb);
#endif
updateSettings();
#else
if (gpsPort && gpsEnabled) {
GPSPositionSensorInitialize();
@ -381,7 +389,12 @@ static void updateSettings()
}
}
}
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
void AuxMagCalibrationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
load_mag_settings();
}
#endif
/**
* @}
* @}

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

@ -34,6 +34,13 @@
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "inc/UBX.h"
#include "inc/GPS.h"
#include "CoordinateConversions.h"
#include "auxmagcalibration.h"
static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
// 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)
@ -293,7 +300,7 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi
}
#endif
}
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo)
{
GPSExtendedStatusData data;
@ -306,13 +313,21 @@ void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo)
data.Status = GPSEXTENDEDSTATUS_STATUS_GPSV9;
GPSExtendedStatusSet(&data);
}
#endif
void parse_ubx_op_mag(struct UBX_OP_MAG *mag)
{
AuxMagSensorData data;
float mags[3] = { mag->x - mag_bias[0],
mag->y - mag_bias[1],
mag->z - mag_bias[2] };
data.x = mag->x;
data.y = mag->y;
data.z = mag->z;
float mag_out[3];
rot_mult(mag_transform, mags, mag_out);
AuxMagSensorData data;
data.x = mag_out[0];
data.y = mag_out[1];
data.z = mag_out[2];
data.Status = mag->Status;
AuxMagSensorSet(&data);
}
@ -427,9 +442,11 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
break;
case UBX_CLASS_OP_CUST:
switch (ubx->header.id) {
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_SYS:
parse_ubx_op_sys(&ubx->payload.op_sysinfo);
break;
#endif
case UBX_ID_MAG:
parse_ubx_op_mag(&ubx->payload.op_mag);
break;
@ -445,4 +462,10 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
return id;
}
void load_mag_settings()
{
AuxMagCalibrationmag_transformArrayGet((float *)mag_transform);
AuxMagCalibrationmag_biasArrayGet(mag_bias);
}
#endif // PIOS_INCLUDE_GPS_UBX_PARSER

1
flight/modules/GPS/inc/GPS.h
View File

@ -38,6 +38,7 @@
#include "gpssatellites.h"
#include "gpspositionsensor.h"
#include "gpstime.h"
#include "auxmagcalibration.h"
#define NO_PARSER -3 // no parser available
#define PARSER_OVERRUN -2 // message buffer overrun before completing the message

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

@ -261,9 +261,9 @@ struct UBX_OP_SYSINFO {
// OP custom messages
struct UBX_OP_MAG {
uint16_t x;
uint16_t y;
uint16_t z;
int16_t x;
int16_t y;
int16_t z;
uint16_t Status;
};
@ -299,5 +299,6 @@ struct UBXPacket {
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
void load_mag_settings();
#endif /* UBX_H */