Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 13:55:18 +03:00
Code Issues Wiki Activity

Added debug IMU speed output

Browse source
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
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"
//#define DEBUG_IMU
#define DEBUG_IMU_SPEED
int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT];
@ -182,10 +183,17 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
return head;
}
#if 0
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 {
@ -193,8 +201,37 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
accADC[Y] = 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)
{
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);