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

Merge branch 'trim_flight_rebase' into next

Browse Source
This commit is contained in:
James Cotton 2011-12-25 22:14:41 -06:00
commit fdec2019e6
2 changed files with 106 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
flight/Modules/Attitude/attitude.c
View File

@ -54,6 +54,7 @@
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "flightstatus.h"
#include "manualcontrolcommand.h"
#include "CoordinateConversions.h"
#include "pios_flash_w25x.h"
@ -91,6 +92,16 @@ static int8_t rotate = 0;
static bool zero_during_arming = false;
static bool bias_correct_gyro = true;
// For running trim flights
static volatile bool trim_requested = false;
static volatile int32_t trim_accels[3];
static volatile int32_t trim_samples;
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define GRAV 9.81f
#define ACCEL_SCALE (GRAV * 0.004f)
/* 0.004f is gravity / LSB */
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -138,6 +149,8 @@ int32_t AttitudeInitialize(void)
for(uint8_t j = 0; j < 3; j++)
R[i][j] = 0;
trim_requested = false;
// Create queue for passing gyro data, allow 2 back samples in case
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
if(gyro_queue == NULL)
@ -277,10 +290,28 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
attitudeRaw->accels[2] = accel[2];
}
if (trim_requested) {
if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) {
trim_requested = false;
} else {
uint8_t armed;
float throttle;
FlightStatusArmedGet(&armed);
ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
trim_samples++;
// Store the digitally scaled version since that is what we use for bias
trim_accels[0] += attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
trim_accels[1] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Y];
trim_accels[2] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Z];
}
}
}
// Scale accels and correct bias
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * ACCEL_SCALE;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * ACCEL_SCALE;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * ACCEL_SCALE;
if(bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
@ -332,7 +363,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
//gyro_correct_int[2] += accel_err[2] * accelKi;
// Correct rates based on error, integral component dealt with in updateSensors
gyro[0] += accel_err[0] * accelKp / dT;
@ -428,8 +459,25 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
Quaternion2R(rotationQuat, R);
rotate = 1;
}
if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) {
trim_accels[0] = 0;
trim_accels[1] = 0;
trim_accels[2] = 0;
trim_samples = 0;
trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false;
attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y] = trim_accels[1] / trim_samples;
// Z should average -grav
attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
} else
trim_requested = false;
}
/**
* @}
* @}
*/
* @}
* @}
*/

1
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -10,6 +10,7 @@
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
<field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="BiasCorrectGyro" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="TrimFlight" units="channel" type="enum" elements="1" options="NORMAL,START,LOAD" defaultvalue="NORMAL"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>