Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Refactoring of IMU and ACC

Browse source
This commit is contained in:
Štěpán Dalecký 2022-01-20 20:37:23 +01:00
parent b6b24ba946
commit f85ebba6a4
24 changed files with 141 additions and 195 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/build/debug.c
View file

@ -102,5 +102,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RX_STATE_TIME",
"GPS_RESCUE_VELOCITY",
"GPS_RESCUE_HEADING",
"GPS_RESCUE_TRACKING"
"GPS_RESCUE_TRACKING",
"ATTITUDE",
};

1
src/main/build/debug.h
View file

@ -101,6 +101,7 @@ typedef enum {
DEBUG_GPS_RESCUE_VELOCITY,
DEBUG_GPS_RESCUE_HEADING,
DEBUG_GPS_RESCUE_TRACKING,
DEBUG_ATTITUDE,
DEBUG_COUNT
} debugType_e;

3
src/main/cli/settings.c
View file

@ -709,7 +709,7 @@ const clivalue_t valueTable[] = {
#if defined(USE_GYRO_SPI_ICM20649)
{ "acc_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_high_fsr) },
#endif
{ PARAM_NAME_ACC_LPF_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
{ PARAM_NAME_ACC_LPF_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
{ "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) },
{ "acc_trim_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) },
@ -1000,6 +1000,7 @@ const clivalue_t valueTable[] = {
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp) },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki) },
{ "small_angle", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
{ "imu_process_denom", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) },
// PG_ARMING_CONFIG
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_ARMING_CONFIG, offsetof(armingConfig_t, auto_disarm_delay) },

4
src/main/fc/core.c
View file

@ -996,10 +996,10 @@ void processRxModes(timeUs_t currentTimeUs)
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
LED1_ON;
// increase frequency of attitude task to reduce drift when in angle or horizon mode
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(acc.sampleRateHz / (float)imuConfig()->imu_process_denom));
} else {
LED1_OFF;
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(acc.sampleRateHz / 10.0f));
}
if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {

2
src/main/fc/tasks.c
View file

@ -161,7 +161,7 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
#ifdef USE_ACC
static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{
accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims);
accUpdate(currentTimeUs);
}
#endif

40
src/main/flight/imu.c
View file

@ -88,13 +88,10 @@ static bool imuUpdated = false;
#define GPS_COG_MIN_GROUNDSPEED 200 // 200cm/s minimum groundspeed for a gps based IMU heading to be considered valid
// Better to have some update than none for GPS Rescue at slow return speeds
float accAverage[XYZ_AXIS_COUNT];
bool canUseGPSHeading = true;
static float throttleAngleScale;
static int throttleAngleValue;
static float fc_acc;
static float smallAngleCosZ = 0;
static imuRuntimeConfig_t imuRuntimeConfig;
@ -113,12 +110,13 @@ quaternion offset = QUATERNION_INITIALIZE;
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
attitudeEulerAngles_t attitude = EULER_INITIALIZE;
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.dcm_kp = 2500, // 1.0 * 10000
.dcm_ki = 0, // 0.003 * 10000
.small_angle = 25,
.imu_process_denom = 2
);
static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
@ -156,14 +154,6 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
#endif
}
/*
* Calculate RC time constant used in the accZ lpf.
*/
static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
return 0.5f / (M_PIf * accz_lpf_cutoff);
}
static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
@ -176,7 +166,6 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle));
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
throttleAngleValue = throttle_correction_value;
@ -501,16 +490,10 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
#if defined(USE_GPS)
if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
// Use GPS course over ground to correct attitude.values.yaw
if (isFixedWing()) {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
} else {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
useCOG = true;
}
if (useCOG && shouldInitializeGPSHeading()) {
if (shouldInitializeGPSHeading()) {
// Reset our reference and reinitialize quaternion. This will likely ideally happen more than once per flight, but for now,
// shouldInitializeGPSHeading() returns true only once.
imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
@ -537,15 +520,15 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
deltaT = imuDeltaT;
#endif
float gyroAverage[XYZ_AXIS_COUNT];
gyroGetAccumulationAverage(gyroAverage);
if (accGetAccumulationAverage(accAverage)) {
useAcc = imuIsAccelerometerHealthy(accAverage);
for (int axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
gyroAverage[axis] = gyroGetFilteredDownsampled(axis);
}
useAcc = imuIsAccelerometerHealthy(acc.accADC);
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, accAverage[X], accAverage[Y], accAverage[Z],
useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z],
useMag,
useCOG, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
@ -596,6 +579,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
acc.accADC[Z] = 0;
schedulerIgnoreTaskStateTime();
}
DEBUG_SET(DEBUG_ATTITUDE, X, acc.accADC[X]);
DEBUG_SET(DEBUG_ATTITUDE, Y, acc.accADC[Y]);
}
#endif // USE_ACC

2
src/main/flight/imu.h
View file

@ -27,7 +27,6 @@
// Exported symbols
extern bool canUseGPSHeading;
extern float accAverage[XYZ_AXIS_COUNT];
typedef struct {
float w,x,y,z;
@ -57,6 +56,7 @@ typedef struct imuConfig_s {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
uint8_t small_angle;
uint8_t imu_process_denom;
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);

115
src/main/flight/pid.c
View file

@ -369,7 +369,7 @@ static float getLevelModeRcDeflection(uint8_t axis)
}
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength()
{
// start with 1.0 at center stick, 0.0 at max stick deflection:
float horizonLevelStrength = 1.0f - MAX(fabsf(getLevelModeRcDeflection(FD_ROLL)), fabsf(getLevelModeRcDeflection(FD_PITCH)));
@ -421,29 +421,33 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1;
}
}
return constrainf(horizonLevelStrength, 0, 1);
}
// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
// The impact is possibly slightly slower performance on F7/H7 but they have more than enough
// processing power that it should be a non-issue.
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim,
float currentPidSetpoint, float horizonLevelStrength) {
const float levelAngleLimit = pidProfile->levelAngleLimit;
// calculate error angle and limit the angle to the max inclination
// rcDeflection is in range [-1.0, 1.0]
float angle = pidProfile->levelAngleLimit * getLevelModeRcDeflection(axis);
float angle = levelAngleLimit * getLevelModeRcDeflection(axis);
#ifdef USE_GPS_RESCUE
angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
#endif
angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
angle = constrainf(angle, -levelAngleLimit, levelAngleLimit);
const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
// ANGLE mode - control is angle based
currentPidSetpoint = errorAngle * pidRuntime.levelGain;
const float setpointCorrection = errorAngle * pidRuntime.levelGain;
currentPidSetpoint = pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
} else {
// HORIZON mode - mix of ANGLE and ACRO modes
// mix in errorAngle to currentPidSetpoint to add a little auto-level feel
const float horizonLevelStrength = calcHorizonLevelStrength();
currentPidSetpoint = currentPidSetpoint + (errorAngle * pidRuntime.horizonGain * horizonLevelStrength);
const float setpointCorrection = errorAngle * pidRuntime.horizonGain * horizonLevelStrength;
currentPidSetpoint += pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
}
return currentPidSetpoint;
}
@ -821,18 +825,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
#if defined(USE_ACC)
static timeUs_t levelModeStartTimeUs = 0;
static bool gpsRescuePreviousState = false;
#endif
#if defined(USE_ACC)
const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
#else
UNUSED(pidProfile);
UNUSED(currentTimeUs);
#endif
#ifdef USE_TPA_MODE
const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
#else
@ -846,6 +838,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
const bool launchControlActive = isLaunchControlActive();
#if defined(USE_ACC)
static timeUs_t levelModeStartTimeUs = 0;
static bool gpsRescuePreviousState = false;
const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
float horizonLevelStrength = 0.0f;
const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
levelMode_e levelMode;
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive) {
@ -854,23 +851,30 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
} else {
levelMode = LEVEL_MODE_RP;
}
} else {
levelMode = LEVEL_MODE_OFF;
}
// Keep track of when we entered a self-level mode so that we can
// add a guard time before crash recovery can activate.
// Also reset the guard time whenever GPS Rescue is activated.
if (levelMode) {
// Keep track of when we entered a self-level mode so that we can
// add a guard time before crash recovery can activate.
// Also reset the guard time whenever GPS Rescue is activated.
if ((levelModeStartTimeUs == 0) || (gpsRescueIsActive && !gpsRescuePreviousState)) {
levelModeStartTimeUs = currentTimeUs;
}
// Calc horizonLevelStrength if needed
if (FLIGHT_MODE(HORIZON_MODE)) {
horizonLevelStrength = calcHorizonLevelStrength();
}
} else {
levelMode = LEVEL_MODE_OFF;
levelModeStartTimeUs = 0;
}
gpsRescuePreviousState = gpsRescueIsActive;
#else
UNUSED(pidProfile);
UNUSED(currentTimeUs);
#endif
// Dynamic i component,
if ((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled) {
// traditional itermAccelerator factor for iTerm
@ -887,7 +891,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000));
float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI;
const float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI;
// gradually scale back integration when above windup point
float dynCi = pidRuntime.dT;
@ -906,15 +910,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// This is done to avoid DTerm spikes that occur with dynamically
// calculated deltaT whenever another task causes the PID
// loop execution to be delayed.
const float delta =
- (gyroRateDterm[axis] - previousRawGyroRateDterm[axis]) * pidRuntime.pidFrequency / D_LPF_RAW_SCALE;
previousRawGyroRateDterm[axis] = gyroRateDterm[axis];
// Log the unfiltered D
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_LPF, 0, lrintf(delta));
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_LPF, 1, lrintf(delta));
// Log the unfiltered D for ROLL and PITCH
if (axis != FD_YAW) {
const float delta = (previousRawGyroRateDterm[axis] - gyroRateDterm[axis]) * pidRuntime.pidFrequency / D_LPF_RAW_SCALE;
DEBUG_SET(DEBUG_D_LPF, axis, lrintf(delta));
}
gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
@ -929,10 +930,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#endif
#ifdef USE_FEEDFORWARD
bool newRcFrame = false;
if (getShouldUpdateFeedforward()) {
newRcFrame = true;
}
const bool newRcFrame = getShouldUpdateFeedforward();
#endif
// ----------PID controller----------
@ -945,21 +943,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// Yaw control is GYRO based, direct sticks control is applied to rate PID
// When Race Mode is active PITCH control is also GYRO based in level or horizon mode
#if defined(USE_ACC)
switch (levelMode) {
case LEVEL_MODE_OFF:
break;
case LEVEL_MODE_R:
if (axis == FD_PITCH) {
break;
}
FALLTHROUGH;
case LEVEL_MODE_RP:
if (axis == FD_YAW) {
break;
}
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
if ((levelMode == LEVEL_MODE_R && axis == FD_ROLL)
|| (levelMode == LEVEL_MODE_RP && (axis == FD_ROLL || axis == FD_PITCH)) ) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength);
DEBUG_SET(DEBUG_ATTITUDE, axis - FD_ROLL + 2, currentPidSetpoint);
}
#endif
@ -999,7 +986,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
const float previousIterm = pidData[axis].I;
float itermErrorRate = errorRate;
#ifdef USE_ABSOLUTE_CONTROL
float uncorrectedSetpoint = currentPidSetpoint;
const float uncorrectedSetpoint = currentPidSetpoint;
#endif
#if defined(USE_ITERM_RELAX)
@ -1009,7 +996,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
#endif
#ifdef USE_ABSOLUTE_CONTROL
float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
const float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
#endif
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
@ -1092,18 +1079,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// Log the value of D pre application of TPA
preTpaD *= D_LPF_FILT_SCALE;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_LPF, 2, lrintf(preTpaD));
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaD));
if (axis != FD_YAW) {
DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, lrintf(preTpaD));
}
} else {
pidData[axis].D = 0;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_LPF, 2, 0);
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_LPF, 3, 0);
if (axis != FD_YAW) {
DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, 0);
}
}
@ -1176,7 +1158,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
const float agBoost = 1.0f + (pidRuntime.antiGravityPBoost / agBoostAttenuator);
if (axis != FD_YAW) {
pidData[axis].P *= agBoost;
DEBUG_SET(DEBUG_ANTI_GRAVITY, axis + 2, lrintf(agBoost * 1000));
DEBUG_SET(DEBUG_ANTI_GRAVITY, axis - FD_ROLL + 2, lrintf(agBoost * 1000));
}
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
@ -1277,8 +1259,7 @@ void dynLpfDTermUpdate(float throttle)
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
const float expof = expo / 10.0f;
static float curve;
curve = throttle * (1 - throttle) * expof + throttle;
const float curve = throttle * (1 - throttle) * expof + throttle;
return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
}

8
src/main/flight/pid.h
View file

@ -391,6 +391,10 @@ typedef struct pidRuntime_s {
float feedforwardJitterFactor;
float feedforwardBoostFactor;
#endif
#ifdef USE_ACC
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
#endif
} pidRuntime_t;
extern pidRuntime_t pidRuntime;
@ -437,8 +441,8 @@ void applyItermRelax(const int axis, const float iterm,
void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
void rotateItermAndAxisError();
float pidLevel(int axis, const pidProfile_t *pidProfile,
const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);
float calcHorizonLevelStrength(void);
const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint, float horizonLevelStrength);
float calcHorizonLevelStrength();
#endif
void dynLpfDTermUpdate(float throttle);
void pidSetItermReset(bool enabled);

8
src/main/flight/pid_init.c
View file

@ -54,6 +54,7 @@
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
#define ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF 3 // The anti gravity P smoothing filter cutoff
#define ATTITUDE_CUTOFF_HZ 250
static void pidSetTargetLooptime(uint32_t pidLooptime)
{
@ -238,6 +239,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
#ifdef USE_ACC
const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT);
for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only
pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
}
#endif
}
void pidInit(const pidProfile_t *pidProfile)

2
src/main/osd/osd.c
View file

@ -1122,7 +1122,7 @@ void osdProcessStats3()
&& (VISIBLE(osdElementConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) {
// only calculate the G force if the element is visible or the stat is enabled
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
const float a = accAverage[axis];
const float a = acc.accADC[axis];
osdGForce += a * a;
}
osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;

39
src/main/sensors/acceleration.c
View file

@ -20,6 +20,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
@ -47,7 +48,7 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim
acc.accADC[Z] -= accelerationTrims->raw[Z];
}
void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
void accUpdate(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
@ -57,14 +58,8 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
acc.isAccelUpdatedAtLeastOnce = true;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[axis]);
acc.accADC[axis] = acc.dev.ADCRaw[axis];
}
if (accelerationRuntime.accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADC[axis] = biquadFilterApply(&accelerationRuntime.accFilter[axis], acc.accADC[axis]);
}
const int16_t val = acc.dev.ADCRaw[axis];
acc.accADC[axis] = val;
}
if (acc.dev.accAlign == ALIGN_CUSTOM) {
@ -74,36 +69,18 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
}
if (!accIsCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims);
performAcclerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
}
if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) {
performInflightAccelerationCalibration(rollAndPitchTrims);
performInflightAccelerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
}
applyAccelerationTrims(accelerationRuntime.accelerationTrims);
++accelerationRuntime.accumulatedMeasurementCount;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accelerationRuntime.accumulatedMeasurements[axis] += acc.accADC[axis];
}
}
bool accGetAccumulationAverage(float *accumulationAverage)
{
if (accelerationRuntime.accumulatedMeasurementCount > 0) {
// If we have gyro data accumulated, calculate average rate that will yield the same rotation
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulationAverage[axis] = accelerationRuntime.accumulatedMeasurements[axis] / accelerationRuntime.accumulatedMeasurementCount;
accelerationRuntime.accumulatedMeasurements[axis] = 0.0f;
}
accelerationRuntime.accumulatedMeasurementCount = 0;
return true;
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulationAverage[axis] = 0.0f;
}
return false;
const int16_t val = acc.accADC[axis];
acc.accADC[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val;
}
}

3
src/main/sensors/acceleration.h
View file

@ -86,8 +86,7 @@ bool accIsCalibrationComplete(void);
bool accHasBeenCalibrated(void);
void accStartCalibration(void);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims);
bool accGetAccumulationAverage(float *accumulation);
void accUpdate(timeUs_t currentTimeUs);
union flightDynamicsTrims_u;
void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse);
void accInitFilters(void);

6
src/main/sensors/acceleration_init.c
View file

@ -124,7 +124,7 @@ static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero)
void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
{
RESET_CONFIG_2(accelerometerConfig_t, instance,
.acc_lpf_hz = 10,
.acc_lpf_hz = 25, // ATTITUDE/IMU runs at 100Hz (acro) or 500Hz (level modes) so we need to set 50 Hz (or lower) to avoid aliasing
.acc_hardware = ACC_DEFAULT,
.acc_high_fsr = false,
);
@ -352,9 +352,9 @@ void accInitFilters(void)
// the filter initialization is not defined (sample rate = 0)
accelerationRuntime.accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
if (accelerationRuntime.accLpfCutHz) {
const uint32_t accSampleTimeUs = 1e6 / acc.sampleRateHz;
const float k = pt2FilterGain(accelerationRuntime.accLpfCutHz, 1.0f / acc.sampleRateHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accelerationRuntime.accFilter[axis], accelerationRuntime.accLpfCutHz, accSampleTimeUs);
pt2FilterInit(&accelerationRuntime.accFilter[axis], k);
}
}
}

4
src/main/sensors/acceleration_init.h
View file

@ -27,10 +27,8 @@
typedef struct accelerationRuntime_s {
uint16_t accLpfCutHz;
biquadFilter_t accFilter[XYZ_AXIS_COUNT];
pt2Filter_t accFilter[XYZ_AXIS_COUNT];
flightDynamicsTrims_t *accelerationTrims;
int accumulatedMeasurementCount;
float accumulatedMeasurements[XYZ_AXIS_COUNT];
uint16_t calibratingA; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
} accelerationRuntime_t;

27
src/main/sensors/gyro.c
View file

@ -77,9 +77,7 @@ static FAST_DATA_ZERO_INIT bool yawSpinDetected;
static FAST_DATA_ZERO_INIT timeUs_t yawSpinTimeUs;
#endif
static FAST_DATA_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
static FAST_DATA_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT];
static FAST_DATA_ZERO_INIT int accumulatedMeasurementCount;
static FAST_DATA_ZERO_INIT float gyroFilteredDownsampled[XYZ_AXIS_COUNT];
static FAST_DATA_ZERO_INIT int16_t gyroSensorTemperature;
@ -526,11 +524,8 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
if (!overflowDetected) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
gyroPrevious[axis] = gyro.gyroADCf[axis];
gyroFilteredDownsampled[axis] = pt1FilterApply(&gyro.imuGyroFilter[axis], gyro.gyroADCf[axis]);
}
accumulatedMeasurementCount++;
}
#if !defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)
@ -538,23 +533,9 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
#endif
}
bool gyroGetAccumulationAverage(float *accumulationAverage)
float gyroGetFilteredDownsampled(int axis)
{
if (accumulatedMeasurementCount) {
// If we have gyro data accumulated, calculate average rate that will yield the same rotation
const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount * gyro.targetLooptime;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
accumulatedMeasurements[axis] = 0.0f;
}
accumulatedMeasurementCount = 0;
return true;
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulationAverage[axis] = 0.0f;
}
return false;
}
return gyroFilteredDownsampled[axis];
}
int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor)

6
src/main/sensors/gyro.h
View file

@ -49,6 +49,8 @@
#define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
#endif
#define GYRO_IMU_DOWNSAMPLE_CUTOFF_HZ 200
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
@ -127,7 +129,7 @@ typedef struct gyro_s {
#ifdef USE_GYRO_OVERFLOW_CHECK
uint8_t overflowAxisMask;
#endif
pt1Filter_t imuGyroFilter[XYZ_AXIS_COUNT];
} gyro_t;
extern gyro_t gyro;
@ -202,7 +204,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
void gyroUpdate(void);
void gyroFiltering(timeUs_t currentTimeUs);
bool gyroGetAccumulationAverage(float *accumulation);
float gyroGetFilteredDownsampled(int axis);
void gyroStartCalibration(bool isFirstArmingCalibration);
bool isFirstArmingGyroCalibrationRunning(void);
bool gyroIsCalibrationComplete(void);

5
src/main/sensors/gyro_init.c
View file

@ -266,6 +266,11 @@ void gyroInitFilters(void)
#ifdef USE_DYN_NOTCH_FILTER
dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
#endif
const float k = pt1FilterGain(GYRO_IMU_DOWNSAMPLE_CUTOFF_HZ, gyro.targetLooptime);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&gyro.imuGyroFilter[axis], k);
}
}
#if defined(USE_GYRO_SLEW_LIMITER)

1
src/test/unit/arming_prevention_unittest.cc
View file

@ -57,6 +57,7 @@ extern "C" {
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
float rcCommand[4];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];

1
src/test/unit/flight_imu_unittest.cc
View file

@ -254,4 +254,5 @@ void schedulerIgnoreTaskExecTime(void) {}
void schedulerIgnoreTaskStateTime(void) {}
void schedulerSetNextStateTime(timeDelta_t) {}
bool schedulerGetIgnoreTaskExecTime() { return false; }
float gyroGetFilteredDownsampled(int) { return 0.0f; }
}

1
src/test/unit/link_quality_unittest.cc
View file

@ -79,7 +79,6 @@ extern "C" {
gpsSolutionData_t gpsSol;
float motor[8];
acc_t acc;
float accAverage[XYZ_AXIS_COUNT];
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);

1
src/test/unit/osd_unittest.cc
View file

@ -85,7 +85,6 @@ extern "C" {
linkQualitySource_e linkQualitySource;
acc_t acc;
float accAverage[XYZ_AXIS_COUNT];
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);

50
src/test/unit/pid_unittest.cc
View file

@ -375,39 +375,39 @@ TEST(pidControllerTest, testPidLevel) {
float currentPidSetpoint = 30;
rollAndPitchTrims_t angleTrim = { { 0, 0 } };
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
// Test attitude response
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(275, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(-275, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(244.07211, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-244.07211, currentPidSetpoint);
setStickPosition(FD_ROLL, -0.5f);
setStickPosition(FD_PITCH, 0.5f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(-137.5, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(137.5, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-93.487915, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(93.487915, currentPidSetpoint);
attitude.values.roll = -275;
attitude.values.pitch = 275;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-12.047981, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
// Disable ANGLE_MODE
disableFlightMode(ANGLE_MODE);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(11.07958, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
// Test level mode expo
enableFlightMode(ANGLE_MODE);
@ -417,10 +417,10 @@ TEST(pidControllerTest, testPidLevel) {
setStickPosition(FD_PITCH, -0.5f);
currentControlRateProfile->levelExpo[FD_ROLL] = 50;
currentControlRateProfile->levelExpo[FD_PITCH] = 26;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(85.9375, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(-110.6875, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(76.208672, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-98.175163, currentPidSetpoint);
}
@ -433,18 +433,18 @@ TEST(pidControllerTest, testPidHorizon) {
// Test full stick response
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
EXPECT_FLOAT_EQ(0, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
// Expect full rate output on full stick
// Test zero stick response
setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0);
EXPECT_FLOAT_EQ(1, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(1.0f, calcHorizonLevelStrength());
// Test small stick response
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.82, calcHorizonLevelStrength(), calculateTolerance(0.82));
EXPECT_NEAR(0.811f, calcHorizonLevelStrength(), calculateTolerance(0.82));
}
TEST(pidControllerTest, testMixerSaturation) {

2
src/test/unit/vtx_unittest.cc
View file

@ -60,6 +60,7 @@ extern "C" {
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
float rcCommand[4];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
@ -75,6 +76,7 @@ extern "C" {
bool cmsInMenu = false;
float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
rxRuntimeState_t rxRuntimeState = {};
acc_t acc;
}
uint32_t simulationFeatureFlags = 0;