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

Moved acc accumulation code out of IMU into acc sensor

Browse source
This commit is contained in:
Martin Budden 2017-11-28 11:12:04 +00:00
parent 44b76bae97
commit 76efc482d3
4 changed files with 40 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -103,8 +103,6 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
/* Asynchronous update accumulators */
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
@ -550,25 +548,6 @@ static void imuUpdateMeasuredRotationRate(void)
#endif
}
/* Calculate measured acceleration in body frame cm/s/s */
static void imuUpdateMeasuredAcceleration(void)
{
int axis;
#ifdef ASYNC_GYRO_PROCESSING
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuMeasuredAccelBF.A[axis] = imuAccumulatedAcc[axis] * GRAVITY_CMSS / imuAccumulatedAccCount;
imuAccumulatedAcc[axis] = 0;
}
imuAccumulatedAccCount = 0;;
#else
/* Convert acceleration to cm/s/s */
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuMeasuredAccelBF.A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
}
#endif
}
#ifdef HIL
void imuHILUpdate(void)
{
@ -602,10 +581,7 @@ void imuUpdateAccelerometer(void)
#endif
#ifdef ASYNC_GYRO_PROCESSING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuAccumulatedAcc[axis] += acc.accADCf[axis];
}
imuAccumulatedAccCount++;
accUpdateAccumulatedMeasurements();
#endif
}
@ -620,7 +596,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
#ifdef HIL
if (!hilActive) {
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
}
else {
@ -629,7 +605,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
}
#else
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
#endif
} else {

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

@ -22,9 +22,6 @@
#include "common/time.h"
#include "config/parameter_group.h"
#define GRAVITY_CMSS 980.665f
#define GRAVITY_MSS 9.80665f
extern t_fp_vector imuMeasuredAccelBF; // cm/s/s
extern t_fp_vector imuMeasuredRotationBF; // rad/s

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

@ -454,6 +454,37 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
}
#ifdef ASYNC_GYRO_PROCESSING
STATIC_FASTRAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
STATIC_FASTRAM int accumulatedMeasurementCount;
void accUpdateAccumulatedMeasurements(void)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulatedMeasurements[axis] += acc.accADCf[axis];
}
accumulatedMeasurementCount++;
}
#endif
/*
* Calculate measured acceleration in body frame in g
*/
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc)
{
#ifdef ASYNC_GYRO_PROCESSING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
measuredAcc->A[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount;
accumulatedMeasurements[axis] = 0;
}
accumulatedMeasurementCount = 0;;
#else
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
}
#endif
}
void accUpdate(void)
{
if (!acc.dev.readFn(&acc.dev)) {

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

@ -18,10 +18,14 @@
#pragma once
#include "common/axis.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "drivers/accgyro/accgyro.h"
#include "sensors/sensors.h"
#define GRAVITY_CMSS 980.665f
#define GRAVITY_MSS 9.80665f
// Type of accelerometer used/detected
typedef enum {
ACC_NONE = 0,
@ -61,6 +65,8 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(uint32_t accTargetLooptime);
bool accIsCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void accUpdateAccumulatedMeasurements(void);
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc);
void accUpdate(void);
void accSetCalibrationValues(void);
void accInitFilters(void);