Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Migrate IMU and PID to fastram

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-09-10 00:04:41 +10:00
parent 3da0ab1862
commit 49f10730a0
2 changed files with 34 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -75,22 +75,22 @@
#define MAX_ACC_SQ_NEARNESS 25 // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
#define MAX_GPS_HEADING_ERROR_DEG 60 // Amount of error between GPS CoG and estimated Yaw at witch we stop trusting GPS and fallback to MAG
t_fp_vector imuMeasuredAccelBF;
t_fp_vector imuMeasuredRotationBF;
static float smallAngleCosZ = 0;
FASTRAM t_fp_vector imuMeasuredAccelBF;
FASTRAM t_fp_vector imuMeasuredRotationBF;
STATIC_FASTRAM float smallAngleCosZ;
static bool isAccelUpdatedAtLeastOnce = false;
STATIC_FASTRAM bool isAccelUpdatedAtLeastOnce;
STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame
STATIC_UNIT_TESTED float rMat[3][3];
STATIC_FASTRAM_UNIT_TESTED float q0, q1, q2, q3; // quaternion of sensor frame relative to earth frame
STATIC_FASTRAM_UNIT_TESTED float rMat[3][3];
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
FASTRAM attitudeEulerAngles_t attitude; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
static imuRuntimeConfig_t imuRuntimeConfig;
STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;
static const float gyroScale = (M_PIf / 180.0f); // gyro output scaled to rad per second
static const float gyroScale = (M_PIf / 180.0f); // gyro output scaled to rad per second
static bool gpsHeadingInitialized = false;
STATIC_FASTRAM bool gpsHeadingInitialized;
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
@ -100,15 +100,14 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.dcm_kp_mag = 10000, // 1.00 * 10000
.dcm_ki_mag = 0, // 0.00 * 10000
.small_angle = 25
);
#ifdef ASYNC_GYRO_PROCESSING
/* Asynchronous update accumulators */
static float imuAccumulatedRate[XYZ_AXIS_COUNT];
static timeUs_t imuAccumulatedRateTimeUs;
static float imuAccumulatedAcc[XYZ_AXIS_COUNT];
static int imuAccumulatedAccCount;
STATIC_FASTRAM float imuAccumulatedRate[XYZ_AXIS_COUNT];
STATIC_FASTRAM timeUs_t imuAccumulatedRateTimeUs;
STATIC_FASTRAM float imuAccumulatedAcc[XYZ_AXIS_COUNT];
STATIC_FASTRAM int imuAccumulatedAccCount;
#endif
#ifdef ASYNC_GYRO_PROCESSING
@ -168,6 +167,14 @@ void imuInit(void)
imuMeasuredAccelBF.A[axis] = 0;
}
// Explicitly initialize FASTRAM statics
isAccelUpdatedAtLeastOnce = false;
gpsHeadingInitialized = false;
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
imuComputeRotationMatrix();
}
@ -284,8 +291,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useMag, float mx, float my, float mz,
bool useCOG, float courseOverGround)
{
static float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; // integral error terms scaled by Ki
static float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; // integral error terms scaled by Ki
STATIC_FASTRAM float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; // integral error terms scaled by Ki
STATIC_FASTRAM float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; // integral error terms scaled by Ki
float ex, ey, ez;
/* Calculate general spin rate (rad/s) */

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

@ -87,24 +87,24 @@ typedef struct {
} pidState_t;
#ifdef USE_DTERM_NOTCH
static filterApplyFnPtr notchFilterApplyFn;
STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
#endif
extern float dT;
float headingHoldCosZLimit;
int16_t headingHoldTarget;
static pt1Filter_t headingHoldRateFilter;
FASTRAM float headingHoldCosZLimit;
FASTRAM int16_t headingHoldTarget;
STATIC_FASTRAM pt1Filter_t headingHoldRateFilter;
// Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
static bool pidGainsUpdateRequired = false;
int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
STATIC_FASTRAM bool pidGainsUpdateRequired;
FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
#ifdef BLACKBOX
int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT];
#endif
static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 3);
@ -206,6 +206,8 @@ void pidInit(void)
// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
pidGainsUpdateRequired = false;
}
#ifdef USE_DTERM_NOTCH
@ -313,7 +315,7 @@ void schedulePidGainsUpdate(void)
void updatePIDCoefficients(void)
{
static uint16_t prevThrottle = 0;
STATIC_FASTRAM uint16_t prevThrottle = 0;
// Check if throttle changed
if (rcCommand[THROTTLE] != prevThrottle) {