mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
Merge branch 'trim_flight_rebase' into next
This commit is contained in:
James Cotton
commit
fdec2019e6
@ -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,18 +92,28 @@ 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
|
||||
*/
|
||||
int32_t AttitudeStart(void)
|
||||
{
|
||||
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -115,7 +126,7 @@ int32_t AttitudeInitialize(void)
|
||||
AttitudeActualInitialize();
|
||||
AttitudeRawInitialize();
|
||||
AttitudeSettingsInitialize();
|
||||
|
||||
|
||||
// Initialize quaternion
|
||||
AttitudeActualData attitude;
|
||||
AttitudeActualGet(&attitude);
|
||||
@ -124,12 +135,12 @@ int32_t AttitudeInitialize(void)
|
||||
attitude.q3 = 0;
|
||||
attitude.q4 = 0;
|
||||
AttitudeActualSet(&attitude);
|
||||
|
||||
|
||||
// Cannot trust the values to init right above if BL runs
|
||||
gyro_correct_int[0] = 0;
|
||||
gyro_correct_int[1] = 0;
|
||||
gyro_correct_int[2] = 0;
|
||||
|
||||
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 0;
|
||||
@ -137,17 +148,19 @@ int32_t AttitudeInitialize(void)
|
||||
for(uint8_t i = 0; i < 3; i++)
|
||||
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)
|
||||
return -1;
|
||||
|
||||
|
||||
|
||||
|
||||
PIOS_ADC_SetQueue(gyro_queue);
|
||||
|
||||
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -160,9 +173,9 @@ static void AttitudeTask(void *parameters)
|
||||
{
|
||||
uint8_t init = 0;
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
|
||||
|
||||
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
|
||||
|
||||
|
||||
// Keep flash CS pin high while talking accel
|
||||
PIOS_FLASH_DISABLE;
|
||||
PIOS_ADXL345_Init();
|
||||
@ -175,13 +188,13 @@ static void AttitudeTask(void *parameters)
|
||||
|
||||
// Force settings update to make sure rotation loaded
|
||||
settingsUpdatedCb(AttitudeSettingsHandle());
|
||||
|
||||
|
||||
// Main task loop
|
||||
while (1) {
|
||||
|
||||
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
|
||||
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
|
||||
// For first 7 seconds use accels to get gyro bias
|
||||
accelKp = 1;
|
||||
@ -201,9 +214,9 @@ static void AttitudeTask(void *parameters)
|
||||
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
|
||||
init = 1;
|
||||
}
|
||||
|
||||
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
|
||||
|
||||
|
||||
AttitudeRawData attitudeRaw;
|
||||
AttitudeRawGet(&attitudeRaw);
|
||||
if(updateSensors(&attitudeRaw) != 0)
|
||||
@ -214,7 +227,7 @@ static void AttitudeTask(void *parameters)
|
||||
AttitudeRawSet(&attitudeRaw);
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -227,22 +240,22 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
{
|
||||
struct pios_adxl345_data accel_data;
|
||||
float gyro[4];
|
||||
|
||||
|
||||
// Only wait the time for two nominal updates before setting an alarm
|
||||
if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
// No accel data available
|
||||
if(PIOS_ADXL345_FifoElements() == 0)
|
||||
return -1;
|
||||
|
||||
|
||||
// First sample is temperature
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
|
||||
|
||||
|
||||
int32_t x = 0;
|
||||
int32_t y = 0;
|
||||
int32_t z = 0;
|
||||
@ -257,9 +270,9 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
} while ( (i < 32) && (samples_remaining > 0) );
|
||||
attitudeRaw->gyrotemp[0] = samples_remaining;
|
||||
attitudeRaw->gyrotemp[1] = i;
|
||||
|
||||
|
||||
float accel[3] = {(float) x / i, (float) y / i, (float) z / i};
|
||||
|
||||
|
||||
if(rotate) {
|
||||
// TODO: rotate sensors too so stabilization is well behaved
|
||||
float vec_out[3];
|
||||
@ -276,19 +289,37 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
|
||||
attitudeRaw->accels[1] = accel[1];
|
||||
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
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
|
||||
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
|
||||
}
|
||||
|
||||
|
||||
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
|
||||
// and make it average zero (weakly)
|
||||
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
|
||||
@ -301,45 +332,45 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
float dT;
|
||||
portTickType thisSysTime = xTaskGetTickCount();
|
||||
static portTickType lastSysTime = 0;
|
||||
|
||||
|
||||
dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
|
||||
// Bad practice to assume structure order, but saves memory
|
||||
float gyro[3];
|
||||
gyro[0] = attitudeRaw->gyros[0];
|
||||
gyro[1] = attitudeRaw->gyros[1];
|
||||
gyro[2] = attitudeRaw->gyros[2];
|
||||
|
||||
|
||||
{
|
||||
float * accels = attitudeRaw->accels;
|
||||
float grot[3];
|
||||
float accel_err[3];
|
||||
|
||||
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
|
||||
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
|
||||
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
|
||||
CrossProduct((const float *) accels, (const float *) grot, accel_err);
|
||||
|
||||
|
||||
// Account for accel magnitude
|
||||
float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
|
||||
accel_err[0] /= accel_mag;
|
||||
accel_err[1] /= accel_mag;
|
||||
accel_err[2] /= accel_mag;
|
||||
|
||||
|
||||
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
||||
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;
|
||||
gyro[1] += accel_err[1] * accelKp / dT;
|
||||
gyro[2] += accel_err[2] * accelKp / dT;
|
||||
}
|
||||
|
||||
|
||||
{ // scoping variables to save memory
|
||||
// Work out time derivative from INSAlgo writeup
|
||||
// Also accounts for the fact that gyros are in deg/s
|
||||
@ -348,7 +379,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||
|
||||
|
||||
// Take a time step
|
||||
q[0] = q[0] + qdot[0];
|
||||
q[1] = q[1] + qdot[1];
|
||||
@ -362,14 +393,14 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
q[3] = -q[3];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Renomalize
|
||||
float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
q[0] = q[0] / qmag;
|
||||
q[1] = q[1] / qmag;
|
||||
q[2] = q[2] / qmag;
|
||||
q[3] = q[3] / qmag;
|
||||
|
||||
|
||||
// If quaternion has become inappropriately short or is nan reinit.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
|
||||
@ -378,44 +409,44 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
|
||||
q[2] = 0;
|
||||
q[3] = 0;
|
||||
}
|
||||
|
||||
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
|
||||
|
||||
quat_copy(q, &attitudeActual.q1);
|
||||
|
||||
|
||||
// Convert into eueler degrees (makes assumptions about RPY order)
|
||||
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
|
||||
|
||||
|
||||
AttitudeActualSet(&attitudeActual);
|
||||
}
|
||||
|
||||
static void settingsUpdatedCb(UAVObjEvent * objEv) {
|
||||
AttitudeSettingsData attitudeSettings;
|
||||
AttitudeSettingsGet(&attitudeSettings);
|
||||
|
||||
|
||||
|
||||
|
||||
accelKp = attitudeSettings.AccelKp;
|
||||
accelKi = attitudeSettings.AccelKi;
|
||||
yawBiasRate = attitudeSettings.YawBiasRate;
|
||||
gyroGain = attitudeSettings.GyroGain;
|
||||
|
||||
|
||||
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
|
||||
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
|
||||
|
||||
|
||||
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
|
||||
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
|
||||
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
|
||||
|
||||
|
||||
gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
|
||||
gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
|
||||
gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
|
||||
|
||||
|
||||
// Indicates not to expend cycles on rotation
|
||||
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
|
||||
attitudeSettings.BoardRotation[2] == 0) {
|
||||
rotate = 0;
|
||||
|
||||
|
||||
// Shouldn't be used but to be safe
|
||||
float rotationQuat[4] = {1,0,0,0};
|
||||
Quaternion2R(rotationQuat, R);
|
||||
@ -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;
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
@ -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"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user