rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

Merged in alessiomorale/librepilot/LP-327_gyro_cal_steady (pull request #270)

Browse Source 
Lp 327_gyro_cal_steady
This commit is contained in:
Lalanne Laurent 2016-07-05 19:44:25 +02:00
commit 4aba28c296
5 changed files with 126 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
flight/libraries/math/mathmisc.c
View File

@ -5,8 +5,9 @@
* @addtogroup Reuseable math functions
* @{
*
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @file mathmisc.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @see The GNU Public License (GPL) Version 3
@ -28,5 +29,17 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <mathmisc.h>
// space deliberately left empty, any non inline misc math functions can go here
void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size)
{
variance->new_sma = 0.0f;
variance->new_smsa = 0.0f;
variance->p1 = 1.0f / (float)window_size;
variance->p2 = 1.0f - variance->p1;
}
float pseudo_windowed_variance_get(pw_variance_t *variance)
{
return variance->new_smsa - variance->new_sma * variance->new_sma;
}

35
flight/libraries/math/mathmisc.h
View File

@ -6,7 +6,8 @@
* @{
*
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @see The GNU Public License (GPL) Version 3
@ -34,6 +35,38 @@
#include <math.h>
#include <stdint.h>
typedef struct {
float p1;
float p2;
float new_sma;
float new_smsa;
} pw_variance_t;
/***
* initialize pseudo windowed
* @param variance the instance to be initialized
* @param window_size size of the sample window
*/
void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size);
/***
* Push a new sample
* @param variance the working instance
* @param sample the new sample
*/
static inline void pseudo_windowed_variance_push_sample(pw_variance_t *variance, float sample)
{
variance->new_sma = variance->new_sma * variance->p2 + sample * variance->p1;
variance->new_smsa = variance->new_smsa * variance->p2 + sample * sample * variance->p1;
}
/***
* Get the current variance value
* @param variance the working instance
* @return
*/
float pseudo_windowed_variance_get(pw_variance_t *variance);
// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)

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

@ -8,7 +8,8 @@
* @{
*
* @file attitude.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
@ -83,25 +84,27 @@ PERF_DEFINE_COUNTER(counterAtt);
// - 0xA7710004 number of accel samples read for each loop (cc only).
// Private constants
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
// Attitude module loop interval (defined by sensor rate in pios_config.h)
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
#define UPDATE_RATE 25.0f
#define UPDATE_RATE 25.0f
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30
#define TEMP_CALIB_INTERVAL 30
// LPF
#define TEMP_DT (1.0f / PIOS_SENSOR_RATE)
#define TEMP_LPF_FC 5.0f
#define TEMP_DT (1.0f / PIOS_SENSOR_RATE)
#define TEMP_LPF_FC 5.0f
static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define VARIANCE_WINDOW_SIZE 40
// Private types
@ -152,6 +155,9 @@ static uint8_t temp_calibration_count = 0;
static AccelGyroSettingsgyro_scaleData gyro_scale;
static AccelGyroSettingsaccel_scaleData accel_scale;
static pw_variance_t gyro_var[3];
static bool initialZeroWhenBoardSteady = true;
static float boardSteadyMaxVariance;
// For running trim flights
static volatile bool trim_requested = false;
@ -243,6 +249,14 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
bool cc3d = BOARDISCC3D;
AccelStateData accelState;
GyroStateData gyros;
int32_t retval = 0;
gyros.x = 0.0f;
gyros.y = 0.0f;
gyros.z = 0.0f;
if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000)
@ -280,12 +294,31 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
portTickType lastSysTime = xTaskGetTickCount();
portTickType startTime = xTaskGetTickCount();
pseudo_windowed_variance_init(&gyro_var[0], VARIANCE_WINDOW_SIZE);
pseudo_windowed_variance_init(&gyro_var[1], VARIANCE_WINDOW_SIZE);
pseudo_windowed_variance_init(&gyro_var[2], VARIANCE_WINDOW_SIZE);
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if ((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
if (init == 0 && initialZeroWhenBoardSteady) {
pseudo_windowed_variance_push_sample(&gyro_var[0], gyros.x);
pseudo_windowed_variance_push_sample(&gyro_var[1], gyros.y);
pseudo_windowed_variance_push_sample(&gyro_var[2], gyros.z);
float const gyrovarx = pseudo_windowed_variance_get(&gyro_var[0]);
float const gyrovary = pseudo_windowed_variance_get(&gyro_var[1]);
float const gyrovarz = pseudo_windowed_variance_get(&gyro_var[2]);
if ((fabsf(gyrovarx) + fabsf(gyrovary) + fabsf(gyrovarz)) > boardSteadyMaxVariance) {
startTime = xTaskGetTickCount();
}
}
if (xTaskGetTickCount() - startTime < 1000) {
PIOS_NOTIFY_StartNotification(NOTIFY_OK, NOTIFY_PRIORITY_REGULAR);
} else if ((xTaskGetTickCount() - startTime < 7000)) {
// Use accels to initialise attitude and calculate gyro bias
accelKp = 1.0f;
accelKi = 0.0f;
@ -316,9 +349,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
#endif
AccelStateData accelState;
GyroStateData gyros;
int32_t retval = 0;
if (cc3d) {
retval = updateSensorsCC3D(&accelState, &gyros);
@ -327,7 +357,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
}
// Only update attitude when sensor data is good
if (retval != 0) {
// raise alarm if gyro has not been yet calibrated to prevent arming
if (retval != 0 || init == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
// Do not update attitude data in simulation mode
@ -750,6 +781,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
initialZeroWhenBoardSteady = (attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE);
boardSteadyMaxVariance = attitudeSettings.BoardSteadyMaxVariance;
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025f;
if (attitudeSettings.AccelTau < 0.0001f) {

27
flight/modules/StateEstimation/filtercf.c
View File

@ -40,7 +40,7 @@
#include <flightstatus.h>
#include <homelocation.h>
#include <revocalibration.h>
#include <mathmisc.h>
#include <CoordinateConversions.h>
#include <pios_notify.h>
// Private constants
@ -49,7 +49,7 @@
#define CALIBRATION_DELAY_MS 4000
#define CALIBRATION_DURATION_MS 6000
#define VARIANCE_WINDOW_SIZE 40
// Private types
struct data {
AttitudeSettingsData attitudeSettings;
@ -70,6 +70,7 @@ struct data {
int32_t starttime;
uint8_t init;
bool magCalibrated;
pw_variance_t gyro_var[3];
};
// Private variables
@ -158,7 +159,6 @@ static int32_t maininit(stateFilter *self)
this->gyroBias[0] = 0.0f;
this->gyroBias[1] = 0.0f;
this->gyroBias[2] = 0.0f;
return 0;
}
@ -231,6 +231,11 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
mag[1] = 0.0f;
mag[2] = 0.0f;
#endif
pseudo_windowed_variance_init(&this->gyro_var[0], VARIANCE_WINDOW_SIZE);
pseudo_windowed_variance_init(&this->gyro_var[1], VARIANCE_WINDOW_SIZE);
pseudo_windowed_variance_init(&this->gyro_var[2], VARIANCE_WINDOW_SIZE);
float magBias[3];
RevoCalibrationmag_biasArrayGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
@ -282,6 +287,22 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion
}
// check whether copter is steady
if (this->init == 0 && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) {
pseudo_windowed_variance_push_sample(&this->gyro_var[0], gyro[0]);
pseudo_windowed_variance_push_sample(&this->gyro_var[1], gyro[1]);
pseudo_windowed_variance_push_sample(&this->gyro_var[2], gyro[2]);
float const gyrovarx = pseudo_windowed_variance_get(&this->gyro_var[0]);
float const gyrovary = pseudo_windowed_variance_get(&this->gyro_var[1]);
float const gyrovarz = pseudo_windowed_variance_get(&this->gyro_var[2]);
if ((fabsf(gyrovarx) + fabsf(gyrovary) + fabsf(gyrovarz)) > this->attitudeSettings.BoardSteadyMaxVariance) {
this->starttime = xTaskGetTickCount();
this->first_run = 1;
return FILTERRESULT_WARNING;
}
}
if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) {
// wait 4 seconds for the user to get his hands off in case the board was just powered

2
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -11,6 +11,8 @@
<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="InitialZeroWhenBoardSteady" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
<field name="BoardSteadyMaxVariance" units="(deg/s)^2" type="float" elements="1" defaultvalue="2"/>
<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"/>