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-1149 changes in CC/CC3D code to use AccelGyroSettings for bias/scale instead of AttitudeSettings.

Browse Source 
Bias units of measure are now normalized between CC/CC3D/Revo.
This commit is contained in:
Alessio Morale 2013-12-29 14:08:53 +01:00
parent a340c06b01
commit 260db3446c
3 changed files with 93 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
flight/modules/Attitude/attitude.c
View File

@ -57,6 +57,7 @@
#include "accelstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "accelgyrosettings.h"
#include "flightstatus.h"
#include "manualcontrolcommand.h"
#include "taskinfo.h"
@ -70,7 +71,6 @@
#define SENSOR_PERIOD 4
#define UPDATE_RATE 25.0f
#define GYRO_NEUTRAL 1665
// Private types
@ -96,7 +96,6 @@ static float accels_filtered[3];
static float grot_filtered[3];
static float yawBiasRate = 0;
static float rollPitchBiasRate = 0.0f;
static float gyroGain = 0.42f;
static int16_t accelbias[3];
static float q[4] = { 1, 0, 0, 0 };
static float R[3][3];
@ -104,6 +103,17 @@ static int8_t rotate = 0;
static bool zero_during_arming = false;
static bool bias_correct_gyro = true;
// temp coefficient to calculate gyro bias
bool apply_gyro_temp = false;
bool apply_accel_temp = false;
static float gyro_temp_coeff[4] = {0};
static float accel_temp_coeff[4] = {0};
// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
static float gyro_scale[3] = {0};
static float accel_scale[3] = {0};
// For running trim flights
static volatile bool trim_requested = false;
static volatile int32_t trim_accels[3];
@ -111,9 +121,14 @@ static volatile int32_t trim_samples;
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define GRAV 9.81f
#define ACCEL_SCALE (GRAV * 0.004f)
#define STD_CC_ACCEL_SCALE (GRAV * 0.004f)
/* 0.004f is gravity / LSB */
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
// Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -136,6 +151,7 @@ int32_t AttitudeInitialize(void)
{
AttitudeStateInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
AccelStateInitialize();
GyroStateInitialize();
@ -166,7 +182,7 @@ int32_t AttitudeInitialize(void)
trim_requested = false;
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -190,9 +206,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool cc3d = bdinfo->board_rev == 0x02;
bool cc3d = BOARDISCC3D;
if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000)
@ -272,8 +286,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
}
}
float gyros_passed[3];
/**
* Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
@ -301,9 +313,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
}
// First sample is temperature
gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale[0];
gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale[1];
gyros->z = -(gyro[3] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale[2];
int32_t x = 0;
int32_t y = 0;
@ -317,9 +329,10 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
y += -accel_data.y;
z += -accel_data.z;
} while ((i < 32) && (samples_remaining > 0));
// gyros->temperature = samples_remaining;
float accel[3] = { (float)x / i, (float)y / i, (float)z / i };
float accel[3] = { accel_scale[0] * (float)x / i,
accel_scale[1] * (float)y / i,
accel_scale[2] * (float)z / i };
if (rotate) {
// TODO: rotate sensors too so stabilization is well behaved
@ -357,9 +370,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
}
// Scale accels and correct bias
accelState->x = (accelState->x - accelbias[0]) * ACCEL_SCALE;
accelState->y = (accelState->y - accelbias[1]) * ACCEL_SCALE;
accelState->z = (accelState->z - accelbias[2]) * ACCEL_SCALE;
accelState->x -= accelbias[0];
accelState->y -= accelbias[1];
accelState->z -= accelbias[2];
if (bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
@ -404,13 +417,13 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
return 0;
}
gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
gyros[0] = mpu6000_data.gyro_x * gyro_scale[0];
gyros[1] = mpu6000_data.gyro_y * gyro_scale[1];
gyros[2] = mpu6000_data.gyro_z * gyro_scale[2];
accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
accels[0] = mpu6000_data.accel_x * accel_scale[0];
accels[1] = mpu6000_data.accel_y * accel_scale[1];
accels[2] = mpu6000_data.accel_z * accel_scale[2];
// gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
@ -429,9 +442,9 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
gyros[2] = vec_out[2];
}
accelStateData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
accelStateData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
accelStateData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
accelStateData->x = accels[0] - accelbias[0];
accelStateData->y = accels[1] - accelbias[1];
accelStateData->z = accels[2] - accelbias[2];
gyrosData->x = gyros[0];
gyrosData->y = gyros[1];
@ -587,14 +600,14 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
AttitudeSettingsData attitudeSettings;
AccelGyroSettingsData accelGyroSettings;
AttitudeSettingsGet(&attitudeSettings);
AccelGyroSettingsGet(&accelGyroSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025f;
@ -609,13 +622,55 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias.X;
accelbias[1] = attitudeSettings.AccelBias.Y;
accelbias[2] = attitudeSettings.AccelBias.Z;
gyro_temp_coeff[0] = accelGyroSettings.gyro_temp_coeff.X;
gyro_temp_coeff[1] = accelGyroSettings.gyro_temp_coeff.Y;
gyro_temp_coeff[2] = accelGyroSettings.gyro_temp_coeff.Z;
gyro_temp_coeff[3] = accelGyroSettings.gyro_temp_coeff.Z2;
gyro_correct_int[0] = attitudeSettings.GyroBias.X / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias.Y / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias.Z / 100.0f;
accel_temp_coeff[0] = accelGyroSettings.accel_temp_coeff.X;
accel_temp_coeff[1] = accelGyroSettings.accel_temp_coeff.Y;
accel_temp_coeff[2] = accelGyroSettings.accel_temp_coeff.Z;
apply_gyro_temp = ( fabsf(gyro_temp_coeff[0])> 1e-6f ||
fabsf(gyro_temp_coeff[1])> 1e-6f ||
fabsf(gyro_temp_coeff[2])> 1e-6f ||
fabsf(gyro_temp_coeff[3])> 1e-6f);
apply_accel_temp = (fabsf(accel_temp_coeff[0])> 1e-6f ||
fabsf(accel_temp_coeff[1])> 1e-6f ||
fabsf(accel_temp_coeff[2])> 1e-6f);
gyro_correct_int[0] = accelGyroSettings.gyro_bias.X;
gyro_correct_int[1] = accelGyroSettings.gyro_bias.Y;
gyro_correct_int[2] = accelGyroSettings.gyro_bias.Z;
if(BOARDISCC3D) {
accelbias[0] = accelGyroSettings.accel_bias.X;
accelbias[1] = accelGyroSettings.accel_bias.Y;
accelbias[2] = accelGyroSettings.accel_bias.Z;
gyro_scale[0] = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale ();
gyro_scale[1] = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale ();
gyro_scale[2] = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale ();
accel_scale[0] = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
accel_scale[1] = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
accel_scale[2] = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
} else {
// Original CC with analog gyros and ADXL accel
accelbias[0] = accelGyroSettings.accel_bias.X;
accelbias[1] = accelGyroSettings.accel_bias.Y;
accelbias[2] = accelGyroSettings.accel_bias.Z;
gyro_scale[0] = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale[1] = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale[2] = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN;
accel_scale[0] = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE;
accel_scale[1] = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE;
accel_scale[2] = accelGyroSettings.accel_scale.Z * STD_CC_ACCEL_SCALE;
}
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
@ -643,10 +698,10 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false;
attitudeSettings.AccelBias.X = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples;
accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples;
accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
// Z should average -grav
attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
} else {

1
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -88,6 +88,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
SRC += $(OPUAVSYNTHDIR)/accelstate.c
SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c
SRC += $(OPUAVSYNTHDIR)/gyrostate.c
SRC += $(OPUAVSYNTHDIR)/attitudestate.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c

3
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -1,10 +1,7 @@
<xml>
<object name="AttitudeSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="AccelBias" units="lsb" type="int16" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="GyroBias" units="deg/s * 100" type="int16" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="BoardRotation" units="deg" type="int16" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/>