mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 05:15:25 +03:00
2baf385b99
of determining a revision code. Previously there was mpu6050 specific code in cli.c (the status command). Finally this commit has removed all non-serial port configuration settings from core_t so that a future commit can refactor core_t to reduce dependencies on serial port code. In doing this I also noted from other source code that the MPU6050 accelerometer trim for some revisions appeared to be incorrectly set to 255 * 8 instead of 256 * 8.
107 lines
4.4 KiB
C
107 lines
4.4 KiB
C
#include "board.h"
|
|
|
|
#include "flight_common.h"
|
|
|
|
#include "mw.h"
|
|
|
|
#include "sensors_acceleration.h"
|
|
|
|
sensor_t acc; // acc access functions
|
|
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
|
|
|
|
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
|
|
|
|
extern uint16_t InflightcalibratingA;
|
|
extern bool AccInflightCalibrationArmed;
|
|
extern bool AccInflightCalibrationMeasurementDone;
|
|
extern bool AccInflightCalibrationSavetoEEProm;
|
|
extern bool AccInflightCalibrationActive;
|
|
|
|
void ACC_Common(void)
|
|
{
|
|
static int32_t a[3];
|
|
int axis;
|
|
|
|
if (calibratingA > 0) {
|
|
for (axis = 0; axis < 3; axis++) {
|
|
// Reset a[axis] at start of calibration
|
|
if (calibratingA == CALIBRATING_ACC_CYCLES)
|
|
a[axis] = 0;
|
|
// Sum up CALIBRATING_ACC_CYCLES readings
|
|
a[axis] += accADC[axis];
|
|
// Clear global variables for next reading
|
|
accADC[axis] = 0;
|
|
mcfg.accZero[axis] = 0;
|
|
}
|
|
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
|
if (calibratingA == 1) {
|
|
mcfg.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
|
|
mcfg.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
|
|
mcfg.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
|
|
cfg.angleTrim[AI_ROLL] = 0;
|
|
cfg.angleTrim[AI_PITCH] = 0;
|
|
writeEEPROM(1, true); // write accZero in EEPROM
|
|
}
|
|
calibratingA--;
|
|
}
|
|
|
|
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
|
static int32_t b[3];
|
|
static int16_t accZero_saved[3] = { 0, 0, 0 };
|
|
static int16_t angleTrim_saved[2] = { 0, 0 };
|
|
// Saving old zeropoints before measurement
|
|
if (InflightcalibratingA == 50) {
|
|
accZero_saved[GI_ROLL] = mcfg.accZero[GI_ROLL];
|
|
accZero_saved[GI_PITCH] = mcfg.accZero[GI_PITCH];
|
|
accZero_saved[GI_YAW] = mcfg.accZero[GI_YAW];
|
|
angleTrim_saved[AI_ROLL] = cfg.angleTrim[AI_ROLL];
|
|
angleTrim_saved[AI_PITCH] = cfg.angleTrim[AI_PITCH];
|
|
}
|
|
if (InflightcalibratingA > 0) {
|
|
for (axis = 0; axis < 3; axis++) {
|
|
// Reset a[axis] at start of calibration
|
|
if (InflightcalibratingA == 50)
|
|
b[axis] = 0;
|
|
// Sum up 50 readings
|
|
b[axis] += accADC[axis];
|
|
// Clear global variables for next reading
|
|
accADC[axis] = 0;
|
|
mcfg.accZero[axis] = 0;
|
|
}
|
|
// all values are measured
|
|
if (InflightcalibratingA == 1) {
|
|
AccInflightCalibrationActive = false;
|
|
AccInflightCalibrationMeasurementDone = true;
|
|
toggleBeep = 2; // buzzer for indicatiing the end of calibration
|
|
// recover saved values to maintain current flight behavior until new values are transferred
|
|
mcfg.accZero[GI_ROLL] = accZero_saved[GI_ROLL];
|
|
mcfg.accZero[GI_PITCH] = accZero_saved[GI_PITCH];
|
|
mcfg.accZero[GI_YAW] = accZero_saved[GI_YAW];
|
|
cfg.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL];
|
|
cfg.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH];
|
|
}
|
|
InflightcalibratingA--;
|
|
}
|
|
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
|
if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again
|
|
AccInflightCalibrationSavetoEEProm = false;
|
|
mcfg.accZero[GI_ROLL] = b[GI_ROLL] / 50;
|
|
mcfg.accZero[GI_PITCH] = b[GI_PITCH] / 50;
|
|
mcfg.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G; // for nunchuk 200=1G
|
|
cfg.angleTrim[AI_ROLL] = 0;
|
|
cfg.angleTrim[AI_PITCH] = 0;
|
|
writeEEPROM(1, true); // write accZero in EEPROM
|
|
}
|
|
}
|
|
|
|
accADC[GI_ROLL] -= mcfg.accZero[GI_ROLL];
|
|
accADC[GI_PITCH] -= mcfg.accZero[GI_PITCH];
|
|
accADC[GI_YAW] -= mcfg.accZero[GI_YAW];
|
|
}
|
|
|
|
void ACC_getADC(void)
|
|
{
|
|
acc.read(accADC);
|
|
ACC_Common();
|
|
}
|
|
|