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 - handle mag rotation

Browse Source
This commit is contained in:
Alessio Morale 2014-08-20 21:05:56 +02:00
parent 8df34b01d7
commit 85be5ba7af
3 changed files with 30 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -99,7 +99,7 @@ static uint32_t timeOfLastUpdateMs;
static struct GPS_RX_STATS gpsRxStats;
#endif
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
void AuxMagCalibrationUpdatedCb(UAVObjEvent *ev);
void AuxMagSettingsUpdatedCb(UAVObjEvent *ev);
#endif
// ****************
/**
@ -157,12 +157,12 @@ int32_t GPSInitialize(void)
HomeLocationInitialize();
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
AuxMagSensorInitialize();
AuxMagCalibrationInitialize();
AuxMagSettingsInitialize();
GPSExtendedStatusInitialize();
// Initialize mag parameters
AuxMagCalibrationUpdatedCb(NULL);
AuxMagCalibrationConnectCallback(AuxMagCalibrationUpdatedCb);
AuxMagSettingsUpdatedCb(NULL);
AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
#endif
updateSettings();
#else
@ -393,7 +393,7 @@ static void updateSettings()
}
}
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
void AuxMagCalibrationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
load_mag_settings();
}

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

@ -35,12 +35,13 @@
#include "inc/UBX.h"
#include "inc/GPS.h"
#include "CoordinateConversions.h"
#include "auxmagcalibration.h"
#include "auxmagsettings.h"
static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
static bool useMag = false;
// 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)
@ -316,6 +317,9 @@ void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo)
#endif
void parse_ubx_op_mag(struct UBX_OP_MAG *mag)
{
if (!useMag) {
return;
}
float mags[3] = { mag->x - mag_bias[0],
mag->y - mag_bias[1],
mag->z - mag_bias[2] };
@ -465,7 +469,24 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
void load_mag_settings()
{
AuxMagCalibrationmag_transformArrayGet((float *)mag_transform);
AuxMagCalibrationmag_biasArrayGet(mag_bias);
AuxMagSettingsTypeOptions option;
AuxMagSettingsTypeGet(&option);
if (option != AUXMAGSETTINGS_TYPE_GPSV9) {
useMag = false;
const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
// next sample from other external mags will provide the right status if present
AuxMagSensorStatusSet((uint8_t *)&status);
} else {
float a[3][3];
float b[3][3];
float rotz;
AuxMagSettingsmag_transformArrayGet((float *)a);
AuxMagSettingsOrientationGet(&rotz);
rot_about_axis_z(rotz, b);
matrix_mult_3x3f(a, b, mag_transform);
AuxMagSettingsmag_biasArrayGet(mag_bias);
useMag = true;
}
}
#endif // PIOS_INCLUDE_GPS_UBX_PARSER

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

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