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

Added debug IMU speed output

This commit is contained in:
ProDrone 2015-09-22 16:32:12 +02:00
parent 242409a443
commit 57fc0f0a57
2 changed files with 48 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,6 +48,7 @@
#include "config/runtime_config.h" #include "config/runtime_config.h"
//#define DEBUG_IMU //#define DEBUG_IMU
#define DEBUG_IMU_SPEED
int16_t accSmooth[XYZ_AXIS_COUNT]; int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT]; int32_t accSum[XYZ_AXIS_COUNT];
@ -182,10 +183,17 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
return head; return head;
} }
#if 0
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims) void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{ {
#ifdef DEBUG_IMU_SPEED
uint32_t time = micros();
#endif
gyroUpdate(); gyroUpdate();
#ifdef DEBUG_IMU_SPEED
debug[0] = micros() - time;
time = micros();
#endif
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
qAccProcessingStateMachine(accelerometerTrims); qAccProcessingStateMachine(accelerometerTrims);
} else { } else {
@ -193,8 +201,37 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
accADC[Y] = 0; accADC[Y] = 0;
accADC[Z] = 0; accADC[Z] = 0;
} }
#ifdef DEBUG_IMU_SPEED
debug[2] = debug[0] + debug[1];
#endif
} }
#else
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
#ifdef DEBUG_IMU_SPEED
uint32_t time = micros();
#endif
gyroUpdate();
#ifdef DEBUG_IMU_SPEED
debug[0] = micros() - time;
time = micros();
#endif
if (sensors(SENSOR_ACC)) {
qAccProcessingStateMachine(accelerometerTrims);
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
#ifdef DEBUG_IMU_SPEED
debug[2] = debug[0] + debug[1];
#endif
}
#endif
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{ {
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);

10
src/main/sensors/acceleration.c
View File

@ -19,11 +19,13 @@
#include <stdint.h> #include <stdint.h>
#include "platform.h" #include "platform.h"
#include "debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/system.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
@ -34,6 +36,8 @@
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#define DEBUG_IMU_SPEED
int16_t accADC[XYZ_AXIS_COUNT]; int16_t accADC[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions acc_t acc; // acc access functions
@ -171,9 +175,15 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{ {
#ifdef DEBUG_IMU_SPEED
uint32_t time = micros();
#endif
if (!acc.read(accADC)) { if (!acc.read(accADC)) {
return; return;
} }
#ifdef DEBUG_IMU_SPEED
debug[1] = micros() - time;
#endif
alignSensors(accADC, accADC, accAlign); alignSensors(accADC, accADC, accAlign);
if (!isAccelerationCalibrationComplete()) { if (!isAccelerationCalibrationComplete()) {