Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Cleanup // Reorganisation

Browse source
This commit is contained in:
borisbstyle 2015-10-22 17:27:26 +02:00
parent fbfc9ee9ca
commit d076a60eb5
3 changed files with 41 additions and 75 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -50,9 +50,6 @@
#include "config/runtime_config.h"
//#define DEBUG_IMU
//#define DEBUG_IMU_SPEED
int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT];
@ -411,19 +408,10 @@ static void imuCalculateEstimatedAttitude(void)
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors)
{
#ifdef DEBUG_IMU_SPEED
uint32_t time = micros();
#endif
if (imuUpdateSensors == ONLY_GYRO || imuUpdateSensors == ACC_AND_GYRO) {
gyroUpdate();
#ifdef DEBUG_IMU_SPEED
debug[0] = micros() - time; // gyro read time
#endif
}
if (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
#ifdef DEBUG_IMU_SPEED
time = micros();
#endif
updateAccelerationReadings(accelerometerTrims);
imuCalculateEstimatedAttitude();
} else {
@ -431,12 +419,6 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors
accADC[Y] = 0;
accADC[Z] = 0;
}
#ifdef DEBUG_IMU_SPEED
debug[1] = micros() - time; // acc read time
if (imuUpdateSensors == ACC_AND_GYRO) {
debug[2] = debug[0] + debug[1]; // gyro + acc read time
}
#endif
}
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)

95
src/main/mw.c
View file

@ -90,8 +90,6 @@ 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 */
@ -102,7 +100,7 @@ enum {
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
float dT;
float dT = 0.000001f; // dT set for 1khz mode
int16_t magHold;
int16_t headFreeModeHold;
@ -166,6 +164,41 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
void filterRc(void){
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static filterStatePt1_t filteredCycleTimeState;
uint16_t rxRefreshRate, filteredCycleTime;
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
isRXDataNew = false;
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
} else {
factor = 0;
}
}
void annexCode(void)
{
int32_t tmp, tmp2;
@ -242,6 +275,8 @@ void annexCode(void)
rcCommand[PITCH] = rcCommand_PITCH;
}
filterRc(); // rcCommand smoothing function
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
@ -673,42 +708,6 @@ void processRx(void)
}
void filterRc(void){
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static filterStatePt1_t filteredCycleTimeState;
uint16_t rxRefreshRate, filteredCycleTime;
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
isRXDataNew = false;
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
} else {
factor = 0;
}
}
void loop(void)
{
static uint32_t loopTime;
@ -776,26 +775,8 @@ void loop(void)
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);
annexCode();
filterRc();
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
#endif

3
src/main/sensors/gyro.c
View file

@ -22,6 +22,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -122,6 +123,8 @@ void gyroUpdate(void)
}
alignSensors(gyroADC, gyroADC, gyroAlign);
filterApply7TapFIR(gyroADC); // Apply filter to gyro
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}