From 6bd6ef5be18fb01908232ca2d7b2b65171fa0f55 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 3 May 2016 01:00:06 +0200 Subject: [PATCH 1/2] LP-150 - Run attitude estimation at a configurable fraction of gyro sample rate for CC/CC3D --- flight/modules/Attitude/attitude.c | 49 ++++++++++++++----- .../coptercontrol/firmware/inc/pios_config.h | 3 +- 2 files changed, 39 insertions(+), 13 deletions(-) diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index f2d024d24..98d903f3b 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -332,11 +332,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) } else { // Do not update attitude data in simulation mode if (!AttitudeStateReadOnly()) { - PERF_TIMED_SECTION_START(counterAtt); updateAttitude(&accelState, &gyros); - PERF_TIMED_SECTION_END(counterAtt); } - PERF_MEASURE_PERIOD(counterPeriod); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_PERIOD_MS); @@ -606,17 +603,41 @@ static inline void apply_accel_filter(const float *raw, float *filtered) __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData) { - float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig); + static uint32_t samples = 0; + static float gyros_accum[3]; + static float accels_accum[3]; + // Bad practice to assume structure order, but saves memory float *gyros = &gyrosData->x; float *accels = &accelStateData->x; + if (samples < ATTITUDE_SENSORS_DOWNSAMPLE - 1) { + gyros_accum[0] += gyros[0]; + gyros_accum[1] += gyros[1]; + gyros_accum[2] += gyros[2]; + accels_accum[0] += accels[0]; + accels_accum[1] += accels[1]; + accels_accum[2] += accels[2]; + samples++; + return; + } + float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig); + PERF_TIMED_SECTION_START(counterAtt); + float inv_samples_count = 1.0f / (float)samples; + samples = 0; + gyros_accum[0] *= inv_samples_count; + gyros_accum[1] *= inv_samples_count; + gyros_accum[2] *= inv_samples_count; + accels_accum[0] *= inv_samples_count; + accels_accum[1] *= inv_samples_count; + accels_accum[2] *= inv_samples_count; + float grot[3]; float accel_err[3]; // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter(accels, accels_filtered); + apply_accel_filter(accels_accum, accels_filtered); // Rotate gravity unit vector to body frame, filter and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); @@ -658,18 +679,18 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel // Correct rates based on error, integral component dealt with in updateSensors const float kpInvdT = accelKp / dT; - gyros[0] += accel_err[0] * kpInvdT; - gyros[1] += accel_err[1] * kpInvdT; - gyros[2] += accel_err[2] * kpInvdT; + gyros_accum[0] += accel_err[0] * kpInvdT; + gyros_accum[1] += accel_err[1] * kpInvdT; + gyros_accum[2] += accel_err[2] * kpInvdT; { // scoping variables to save memory // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; - qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); - qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[0] = (-q[1] * gyros_accum[0] - q[2] * gyros_accum[1] - q[3] * gyros_accum[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[1] = (q[0] * gyros_accum[0] - q[3] * gyros_accum[1] + q[2] * gyros_accum[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[2] = (q[3] * gyros_accum[0] + q[0] * gyros_accum[1] - q[1] * gyros_accum[2]) * dT * (M_PI_F / 180.0f / 2.0f); + qdot[3] = (-q[2] * gyros_accum[0] + q[1] * gyros_accum[1] + q[0] * gyros_accum[2]) * dT * (M_PI_F / 180.0f / 2.0f); // Take a time step q[0] = q[0] + qdot[0]; @@ -711,6 +732,10 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll); AttitudeStateSet(&attitudeState); + gyros_accum[0] = gyros_accum[1] = gyros_accum[2] = 0.0f; + accels_accum[0] = accels_accum[1] = accels_accum[2] = 0.0f; + PERF_TIMED_SECTION_END(counterAtt); + PERF_MEASURE_PERIOD(counterPeriod); } static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index b207bd0c2..122b0fcc8 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -93,7 +93,8 @@ /* #define PIOS_INCLUDE_ETASV3 */ /* #define PIOS_INCLUDE_HCSR04 */ -#define PIOS_SENSOR_RATE 500.0f +#define PIOS_SENSOR_RATE 500.0f +#define ATTITUDE_SENSORS_DOWNSAMPLE 4 /* PIOS receiver drivers */ #define PIOS_INCLUDE_PWM From e14e5febe2c67fa0755ef92febf81b6235b86be1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 3 May 2016 01:00:28 +0200 Subject: [PATCH 2/2] LP-150 - prevent stab error conditions --- flight/modules/Stabilization/inc/stabilization.h | 5 ++++- flight/modules/Stabilization/outerloop.c | 13 +++++++++---- .../boards/coptercontrol/firmware/inc/pios_config.h | 1 + 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index b1ff2fc13..fbf8dbf9b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -80,9 +80,12 @@ extern StabilizationData stabSettings; // must be same as eventdispatcher to avoid needing additional mutexes #define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED // outer loop only executes every 4th uavobject update to save CPU #define OUTERLOOP_SKIPCOUNT 4 - +#else +#define OUTERLOOP_SKIPCOUNT ATTITUDE_SENSORS_DOWNSAMPLE +#endif // STABILIZATION_ATTITUDE_DOWNSAMPLED #endif // STABILIZATION_H /** diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 31c81e86c..9fc3c6e4a 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -365,14 +365,19 @@ static void stabilizationOuterloopTask() static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { +#ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED // to reduce CPU utilization, outer loop is not executed on every state update static uint8_t cpusaver = 0; if ((cpusaver++ % OUTERLOOP_SKIPCOUNT) == 0) { - // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! - AttitudeStateGet(&attitude); - PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); - } +#endif + // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! + AttitudeStateGet(&attitude); + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + +#ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED +} +#endif } /** diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 122b0fcc8..6c617d06f 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -94,6 +94,7 @@ /* #define PIOS_INCLUDE_HCSR04 */ #define PIOS_SENSOR_RATE 500.0f +#define STABILIZATION_ATTITUDE_DOWNSAMPLED #define ATTITUDE_SENSORS_DOWNSAMPLE 4 /* PIOS receiver drivers */