rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tidy of IMU header

This commit is contained in:
Martin Budden 2017-02-05 20:37:20 +00:00
parent 9a8124ffc4
commit 3d1e42d1aa
14 changed files with 78 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/blackbox/blackbox.c
View File

@ -32,6 +32,7 @@
#include "common/axis.h"
#include "common/encoding.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/config_profile.h"

1
src/main/blackbox/blackbox_io.c
View File

@ -15,6 +15,7 @@
#include "build/build_config.h"
#include "common/encoding.h"
#include "common/maths.h"
#include "common/printf.h"
#include "config/config_profile.h"

1
src/main/config/config_master.h
View File

@ -37,6 +37,7 @@
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/fc_core.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"

8
src/main/fc/fc_core.h
View File

@ -18,11 +18,19 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
extern int16_t magHold;
extern bool isRXDataNew;
extern int16_t headFreeModeHold;
typedef struct throttleCorrectionConfig_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
} throttleCorrectionConfig_t;
PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
union rollAndPitchTrims_u;
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
void handleInflightCalibrationStickPosition();

106
src/main/flight/imu.c
View File

@ -21,40 +21,40 @@
#include <stdint.h>
#include <math.h>
#include "common/maths.h"
#include "platform.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "io/gps.h"
#include "fc/runtime_config.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
// the limit (in degrees/second) beyond which we stop integrating
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
// which results in false gyro drift. See
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT 20
int32_t accSum[XYZ_AXIS_COUNT];
@ -63,11 +63,11 @@ uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
float throttleAngleScale;
float fc_acc;
float smallAngleCosZ = 0;
static float throttleAngleScale;
static float fc_acc;
static float smallAngleCosZ = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
static float magneticDeclination = 0.0f; // calculated at startup from config
static imuRuntimeConfig_t imuRuntimeConfig;
static pidProfile_t *pidProfile;
@ -103,6 +103,19 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
/*
* 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);
}
void imuConfigure(
imuConfig_t *imuConfig,
pidProfile_t *initialPidProfile,
@ -127,19 +140,6 @@ void imuInit(void)
imuComputeRotationMatrix();
}
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
/*
* Calculate RC time constant used in the accZ lpf.
*/
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
return 0.5f / (M_PIf * accz_lpf_cutoff);
}
void imuResetAccelerationSum(void)
{
accSum[0] = 0;
@ -149,14 +149,12 @@ void imuResetAccelerationSum(void)
accTimeSum = 0;
}
void imuTransformVectorBodyToEarth(t_fp_vector * v)
static void imuTransformVectorBodyToEarth(t_fp_vector * v)
{
float x,y,z;
/* From body frame to earth frame */
x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
v->V.X = x;
v->V.Y = -y;
@ -164,16 +162,15 @@ void imuTransformVectorBodyToEarth(t_fp_vector * v)
}
// rotate acc into Earth frame and calculate acceleration in it
void imuCalculateAcceleration(uint32_t deltaT)
static void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
float dT;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
const float dT = (float)deltaT * 1e-6f;
t_fp_vector accel_ned;
accel_ned.V.X = acc.accSmooth[X];
accel_ned.V.Y = acc.accSmooth[Y];
accel_ned.V.Z = acc.accSmooth[Z];
@ -186,8 +183,9 @@ void imuCalculateAcceleration(uint32_t deltaT)
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
} else {
accel_ned.V.Z -= acc.dev.acc_1G;
}
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
@ -227,15 +225,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useYaw, float yawError)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float hx, hy, bx;
float ex = 0, ey = 0, ez = 0;
float qa, qb, qc;
// Calculate general spin rate (rad/s)
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
float ez = 0;
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
@ -244,7 +239,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Use measured magnetic field vector
recipNorm = sq(mx) + sq(my) + sq(mz);
float ex = 0, ey = 0;
float recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
@ -257,12 +253,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
bx = sqrtf(hx * hx + hy * hy);
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
const float bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
const float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
@ -289,7 +285,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
if(imuRuntimeConfig.dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
float dcmKiGain = imuRuntimeConfig.dcm_ki;
const float dcmKiGain = imuRuntimeConfig.dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
@ -302,7 +298,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
const float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
@ -314,9 +310,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
gy *= (0.5f * dt);
gz *= (0.5f * dt);
qa = q0;
qb = q1;
qc = q2;
const float qa = q0;
const float qb = q1;
const float qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
@ -353,10 +349,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
static bool imuIsAccelerometerHealthy(void)
{
int32_t axis;
int32_t accMagnitude = 0;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
}

37
src/main/flight/imu.h
View File

@ -18,13 +18,10 @@
#pragma once
#include "common/axis.h"
#include "common/maths.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "sensors/acceleration.h"
// Exported symbols
extern uint32_t accTimeSum;
extern int accSumCount;
@ -49,13 +46,6 @@ typedef struct accDeadband_s {
uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations
} accDeadband_t;
typedef struct throttleCorrectionConfig_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
} throttleCorrectionConfig_t;
PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
typedef struct imuConfig_s {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
@ -74,38 +64,13 @@ typedef struct imuRuntimeConfig_s {
accDeadband_t accDeadband;
} imuRuntimeConfig_t;
typedef enum {
ACCPROC_READ = 0,
ACCPROC_CHUNK_1,
ACCPROC_CHUNK_2,
ACCPROC_CHUNK_3,
ACCPROC_CHUNK_4,
ACCPROC_CHUNK_5,
ACCPROC_CHUNK_6,
ACCPROC_CHUNK_7,
ACCPROC_COPY
} accProcessorState_e;
typedef struct accProcessor_s {
accProcessorState_e state;
} accProcessor_t;
struct pidProfile_s;
void imuConfigure(
imuConfig_t *imuConfig,
struct pidProfile_s *initialPidProfile,
uint16_t throttle_correction_angle
);
void imuConfigure(imuConfig_t *imuConfig, struct pidProfile_s *initialPidProfile, uint16_t throttle_correction_angle);
float getCosTiltAngle(void);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
void imuUpdateAttitude(timeUs_t currentTimeUs);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
union u_fp_vector;
int16_t imuCalculateHeading(union u_fp_vector *vec);
void imuResetAccelerationSum(void);
void imuInit(void);

1
src/main/flight/servos.c
View File

@ -27,6 +27,7 @@
#include "build/build_config.h"
#include "common/filter.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"

1
src/main/io/osd.c
View File

@ -38,6 +38,7 @@
#include "build/version.h"
#include "common/printf.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/config_profile.h"

2
src/main/io/vtx.c
View File

@ -26,6 +26,8 @@
#include "io/osd.h"
//External dependencies
#include "common/maths.h"
#include "config/config_eeprom.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"

1
src/main/sensors/battery.c
View File

@ -23,6 +23,7 @@
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/feature.h"

4
src/main/sensors/esc_sensor.c
View File

@ -21,12 +21,15 @@
#include <platform.h>
#ifdef USE_DSHOT
#include "build/debug.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/pwm_output.h"
@ -71,7 +74,6 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli
*/
#ifdef USE_DSHOT
enum {
DEBUG_ESC_MOTOR_INDEX = 0,
DEBUG_ESC_NUM_TIMEOUTS = 1,

10
src/main/target/COLIBRI_RACE/config.c
View File

@ -15,19 +15,21 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <platform.h>
#include "common/maths.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "io/ledstrip.h"
#include "io/motors.h"
#include "sensors/battery.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "io/ledstrip.h"
void targetApplyDefaultLedStripConfig(ledConfig_t *ledConfigs)
{

1
src/main/target/MULTIFLITEPICO/config.c
View File

@ -21,6 +21,7 @@
#include <platform.h>
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/sensor.h"
#include "drivers/compass.h"

1
src/main/telemetry/crsf.c
View File

@ -35,6 +35,7 @@
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "common/maths.h"
#include "common/streambuf.h"
#include "common/utils.h"