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

OP-476: If AttitudeSettings.ZeroDuringArming is true, then while arming (1

Browse Source 
second) it will speed up the estimate of gyro bias.
This commit is contained in:
James Cotton 2011-05-07 15:29:21 -05:00
parent 8e06eb3162
commit 4ec0263dfc
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,6 +53,7 @@
#include "attituderaw.h"
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "flightstatus.h"
#include "CoordinateConversions.h"
#include "pios_flash_w25x.h"
@ -87,6 +88,7 @@ static int16_t accelbias[3];
static float q[4] = {1,0,0,0};
static float R[3][3];
static int8_t rotate = 0;
static bool zero_during_arming = false;
/**
* Initialise the module, called on startup
@ -133,15 +135,25 @@ static void AttitudeTask(void *parameters)
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
zero_during_arming = false;
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
yawBiasRate = 0.01 / (1 + xTaskGetTickCount() / 5000);
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKi = .01;
yawBiasRate = 0.1;
init = 0;
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
@ -313,6 +325,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];

1
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -7,6 +7,7 @@
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.01"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<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="FALSE"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>