Skip to content

Commit

Permalink
Refactoring of IMU and ACC
Browse files Browse the repository at this point in the history
  • Loading branch information
@daleckystepan
daleckystepan committed Jun 28, 2022
1 parent b6b24ba commit f85ebba
Show file tree
Hide file tree
Showing 24 changed files with 141 additions and 195 deletions.
3 changes: 2 additions & 1 deletion src/main/build/debug.c
View file
Expand Up @@ -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 change: 1 addition & 0 deletions src/main/build/debug.h
View file
Expand Up @@ -101,6 +101,7 @@ typedef enum {
DEBUG_GPS_RESCUE_VELOCITY,
DEBUG_GPS_RESCUE_HEADING,
DEBUG_GPS_RESCUE_TRACKING,
DEBUG_ATTITUDE,
DEBUG_COUNT
} debugType_e;

Expand Down
3 changes: 2 additions & 1 deletion src/main/cli/settings.c
View file
Expand Up @@ -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) },

Expand Down Expand Up @@ -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) },
Expand Down
4 changes: 2 additions & 2 deletions src/main/fc/core.c
View file
Expand Up @@ -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)) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/tasks.c
View file
Expand Up @@ -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

Expand Down
40 changes: 13 additions & 27 deletions src/main/flight/imu.c
View file
Expand Up @@ -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;
Expand All @@ -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)
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand All @@ -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));

Expand Down Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/imu.h
View file
Expand Up @@ -27,7 +27,6 @@

// Exported symbols
extern bool canUseGPSHeading;
extern float accAverage[XYZ_AXIS_COUNT];

typedef struct {
float w,x,y,z;
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit f85ebba

Please sign in to comment.