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

OP-1434 - Switch to common aux mag support library

Browse Source
This commit is contained in:
Alessio Morale 2014-09-13 16:15:02 +02:00
parent 76b9b1c40f
commit 3fd5ca08cd
1 changed files with 5 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,20 +30,14 @@
#include "openpilot.h" #include "openpilot.h"
#include "pios.h" #include "pios.h"
#include "CoordinateConversions.h"
#include "pios_math.h" #include "pios_math.h"
#include <pios_helpers.h> #include <pios_helpers.h>
#include <pios_delay.h> #include <pios_delay.h>
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "inc/UBX.h" #include "inc/UBX.h"
#include "inc/GPS.h" #include "inc/GPS.h"
#include "auxmagsettings.h"
#include <string.h> #include <string.h>
#include <auxmagsupport.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; static bool useMag = false;
static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
@ -485,20 +479,8 @@ static void parse_ubx_op_mag(struct UBXPacket *ubx, __attribute__((unused)) GPSP
return; return;
} }
struct UBX_OP_MAG *mag = &ubx->payload.op_mag; struct UBX_OP_MAG *mag = &ubx->payload.op_mag;
float mags[3] = { mag->x - mag_bias[0], float mags[3] = { mag->x, mag->y, mag->z };
mag->y - mag_bias[1], auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK);
mag->z - mag_bias[2] };
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);
} }
@ -537,30 +519,17 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
return id; return id;
} }
void load_mag_settings() void load_mag_settings()
{ {
AuxMagSettingsTypeOptions option; auxmagsupport_reload_settings();
AuxMagSettingsTypeGet(&option); if (auxmagsupport_get_type() != AUXMAGSETTINGS_TYPE_GPSV9) {
if (option != AUXMAGSETTINGS_TYPE_GPSV9) {
useMag = false; useMag = false;
const uint8_t status = AUXMAGSENSOR_STATUS_NONE; const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
// next sample from other external mags will provide the right status if present // next sample from other external mags will provide the right status if present
AuxMagSensorStatusSet((uint8_t *)&status); AuxMagSensorStatusSet((uint8_t *)&status);
} else { } else {
float a[3][3];
float b[3][3];
float rotz;
AuxMagSettingsmag_transformArrayGet((float *)a);
AuxMagSettingsOrientationGet(&rotz);
rotz = DEG2RAD(rotz);
rot_about_axis_z(rotz, b);
matrix_mult_3x3f(a, b, mag_transform);
AuxMagSettingsmag_biasArrayGet(mag_bias);
useMag = true; useMag = true;
} }
} }
#endif // PIOS_INCLUDE_GPS_UBX_PARSER #endif // PIOS_INCLUDE_GPS_UBX_PARSER