From ea390d0b909356d4fbf6ef231c04408b6e7df48b Mon Sep 17 00:00:00 2001 From: peabody124 Date: Sat, 26 Feb 2011 22:21:44 +0000 Subject: [PATCH] CC-26: Add accel bias term. Note it's units are LSB, not m/s^s git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2895 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/Modules/Attitude/attitude.c | 9 +++++++++ shared/uavobjectdefinition/attitudesettings.xml | 1 + 2 files changed, 10 insertions(+) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 0565e654e..1e1d66ad0 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -84,6 +84,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv); static float accelKi = 0; static float accelKp = 0; static float gyroGain = 0.42; +static int16_t accelbias[3]; /** * Initialise the module, called on startup @@ -215,6 +216,9 @@ static void updateSensors(AttitudeRawData * attitudeRaw) } while ( (i < 32) && (samples_remaining > 0) ); attitudeRaw->gyrotemp[0] = samples_remaining; attitudeRaw->gyrotemp[1] = i; + x -= accelbias[0]; + y -= accelbias[1]; + z -= accelbias[2]; attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = ((float)x * 0.004f * 9.81f) / i; attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = ((float)y * 0.004f * 9.81f) / i; attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = ((float)z * 0.004f * 9.81f) / i; @@ -311,6 +315,11 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) { accelKp = attitudeSettings.AccelKp; accelKi = attitudeSettings.AccelKi; gyroGain = attitudeSettings.GyroGain; + + accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; + accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; + accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; + } /** * @} diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index b8b911f50..4f533b3db 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -1,6 +1,7 @@ Settings for the @ref Attitude module used on CopterControl +