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

More Jitter Enhancements for Naze target 

remove debug

Anti Jitter Enhancements for Naze target

Jitter Optimalisation without emf avoidance
This commit is contained in:
borisbstyle 2015-09-23 11:59:51 +02:00
parent 9ed1c46065
commit 4a492c611a
3 changed files with 64 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,7 +48,10 @@
#include "config/runtime_config.h"
//#define DEBUG_IMU
#define DEBUG_IMU_SPEED
//#define DEBUG_IMU_SPEED
#define MAX_ACC_PROCESSING 360 // Anti jitter equal acc processing each cycle
#define MAX_GYRO_PROCESSING 100 // Anti jitter equal gyro processing each cycle
int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT];
@ -186,23 +189,40 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
#if 0
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
#ifdef DEBUG_IMU_SPEED
uint32_t time = micros();
#if defined(NAZE)
uint32_t accProcessTime, gyroProcessTime;
#endif
#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
uint32_t time = micros();
#endif
gyroUpdate();
#if defined(NAZE)
while (gyroProcessTime < MAX_GYRO_PROCESSING ) {
gyroProcessTime = micros() - time;
}
#endif
#ifdef DEBUG_IMU_SPEED
debug[0] = micros() - time;
time = micros();
debug[0] = micros() - time; // gyro read time
#endif
if (sensors(SENSOR_ACC)) {
#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
time = micros();
#endif
qAccProcessingStateMachine(accelerometerTrims);
#if defined(NAZE)
while (accProcessTime < MAX_ACC_PROCESSING) {
accProcessTime = micros() - time;
}
#endif
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
#ifdef DEBUG_IMU_SPEED
debug[2] = debug[0] + debug[1];
debug[1] = micros() - time; // acc read time
debug[2] = debug[0] + debug[1]; // gyro + acc read time
#endif
}
@ -210,23 +230,40 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
#ifdef DEBUG_IMU_SPEED
uint32_t time = micros();
#if defined(NAZE)
uint32_t accProcessTime, gyroProcessTime;
#endif
#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
uint32_t time = micros();
#endif
gyroUpdate();
#if defined(NAZE)
while (gyroProcessTime < MAX_GYRO_PROCESSING ) {
gyroProcessTime = micros() - time;
}
#endif
#ifdef DEBUG_IMU_SPEED
debug[0] = micros() - time;
time = micros();
debug[0] = micros() - time; // gyro read time
#endif
if (sensors(SENSOR_ACC)) {
#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
time = micros();
#endif
qAccProcessingStateMachine(accelerometerTrims);
#if defined(NAZE)
while (accProcessTime < MAX_ACC_PROCESSING) {
accProcessTime = micros() - time;
}
#endif
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
#ifdef DEBUG_IMU_SPEED
debug[2] = debug[0] + debug[1];
debug[1] = micros() - time; // acc read time
debug[2] = debug[0] + debug[1]; // gyro + acc read time
#endif
}

16
src/main/mw.c
View File

@ -20,6 +20,7 @@
#include <stdint.h>
#include <math.h>
#include "debug.h"
#include "platform.h"
#include "common/maths.h"
@ -88,6 +89,8 @@ enum {
ALIGN_MAG = 2
};
//#define DEBUG_JITTER 3
/* VBAT monitoring interval (in microseconds) - 1s*/
#define VBATINTERVAL (6 * 3500)
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
@ -776,12 +779,23 @@ void loop(void)
loopTime = currentTime + targetLooptime;
imuUpdate(&currentProfile->accelerometerTrims);
// Measure loop rate just after reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
#ifdef DEBUG_JITTER
static uint32_t previousCycleTime;
if (previousCycleTime > cycleTime) {
debug[DEBUG_JITTER] = previousCycleTime - cycleTime;
} else {
debug[DEBUG_JITTER] = cycleTime - previousCycleTime;
}
previousCycleTime = cycleTime;
#endif
dT = (float)targetLooptime * 0.000001f;
filterApply7TapFIR(gyroADC);

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

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