rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Use the scale and bias terms for hte mag and accel

Browse Source
This commit is contained in:
James Cotton 2011-12-24 15:56:37 -06:00
parent 710f95feeb
commit 1092ac3184
2 changed files with 33 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
flight/Modules/Sensors/sensors.c
View File

@ -54,6 +54,7 @@
#include "gyrosbias.h" #include "gyrosbias.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attitudesettings.h" #include "attitudesettings.h"
#include "revocalibration.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
@ -72,17 +73,13 @@ static xTaskHandle sensorsTaskHandle;
static void SensorsTask(void *parameters); static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent * objEv); static void settingsUpdatedCb(UAVObjEvent * objEv);
static float gyroGain = 0.42;
static int16_t accelbias[3];
static float R[3][3];
static int8_t rotate = 0;
static bool zero_during_arming = false;
// These values are initialized by settings but can be updated by the attitude algorithm // These values are initialized by settings but can be updated by the attitude algorithm
static bool bias_correct_gyro = true; static bool bias_correct_gyro = true;
static float gyro_bias[3] = {0,0,0};
float mag_bias[3] = {-140.0f,150.0f,0}; static float mag_bias[3] = {0,0,0};
static float mag_scale[3] = {0,0,0};
static float accel_bias[3] = {0,0,0};
static float accel_scale[3] = {0,0,0};
/** /**
* API for sensor fusion algorithms: * API for sensor fusion algorithms:
@ -103,15 +100,10 @@ int32_t SensorsInitialize(void)
GyrosBiasInitialize(); GyrosBiasInitialize();
AccelsInitialize(); AccelsInitialize();
MagnetometerInitialize(); MagnetometerInitialize();
AttitudeSettingsInitialize(); RevoCalibrationInitialize();
AttitudeSettingsInitialize();
RevoCalibrationConnectCallback(&settingsUpdatedCb);
for(uint8_t i = 0; i < 3; i++)
for(uint8_t j = 0; j < 3; j++)
R[i][j] = 0;
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
return 0; return 0;
} }
@ -204,9 +196,9 @@ static void SensorsTask(void *parameters)
// Not the swaping of channel orders // Not the swaping of channel orders
scaling = PIOS_BMA180_GetScale(); scaling = PIOS_BMA180_GetScale();
AccelsData accelsData; // Skip get as we set all the fields AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = (accels[0] - accelbias[0]) * scaling; accelsData.x = accels[0] * scaling * accel_scale[0] - accel_bias[0];
accelsData.y = (accels[1] - accelbias[1]) * scaling; accelsData.y = accels[1] * scaling * accel_scale[1] - accel_bias[1];
accelsData.z = (accels[2] - accelbias[2]) * scaling; accelsData.z = accels[2] * scaling * accel_scale[2] - accel_bias[2];
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
AccelsSet(&accelsData); AccelsSet(&accelsData);
@ -252,9 +244,9 @@ static void SensorsTask(void *parameters)
int16_t values[3]; int16_t values[3];
PIOS_HMC5883_ReadMag(values); PIOS_HMC5883_ReadMag(values);
MagnetometerData mag; // Skip get as we set all the fields MagnetometerData mag; // Skip get as we set all the fields
mag.x = values[1] - mag_bias[0]; mag.x = values[1] * mag_scale[0] - mag_bias[0];
mag.y = values[0] - mag_bias[1]; mag.y = values[0] * mag_scale[1] - mag_bias[1];
mag.z = -values[2] - mag_bias[2]; mag.z = -values[2] * mag_scale[2] - mag_bias[2];
MagnetometerSet(&mag); MagnetometerSet(&mag);
} }
@ -267,35 +259,24 @@ static void SensorsTask(void *parameters)
* Locally cache some variables from the AtttitudeSettings object * Locally cache some variables from the AtttitudeSettings object
*/ */
static void settingsUpdatedCb(UAVObjEvent * objEv) { static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings; if(objEv->obj == RevoCalibrationHandle()) {
AttitudeSettingsGet(&attitudeSettings); RevoCalibrationData cal;
RevoCalibrationGet(&cal);
gyroGain = attitudeSettings.GyroGain;
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y];
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z];
mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X];
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X];
accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y];
// Indicates not to expend cycles on rotation accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z];
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
attitudeSettings.BoardRotation[2] == 0) { accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
rotate = 0; accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
// Shouldn't be used but to be safe
float rotationQuat[4] = {1,0,0,0};
Quaternion2R(rotationQuat, R);
} else {
float rotationQuat[4];
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
} }
} }
/** /**
* @} * @}

1
flight/Revolution/UAVObjects.inc
View File

@ -60,6 +60,7 @@ UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += positiondesired UAVOBJSRCFILENAMES += positiondesired
UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += sonaraltitude UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings UAVOBJSRCFILENAMES += stabilizationsettings