mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +03:00
7592316c04
changed gyro/acc to allow different drivers without recompile, this allowed adding mpu6050 support w/autodetect moved sensor orientation code into driver - each driver provides its own added support for mpu6050 - desolder mpu3050 and adxl345, replace with mpu6050 for instant ??? merged multiwii 2.0pre3 code changes (none that mattered except mag calibration and typo in baro stuff) changes to sensor autodetect for new dynamic drivers more of ledring stuff done (still doesn't work, so dont try) git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@115 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
327 lines
11 KiB
C
Executable file
327 lines
11 KiB
C
Executable file
#include "board.h"
|
|
#include "mw.h"
|
|
|
|
uint8_t calibratedACC = 0;
|
|
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.
|
|
uint16_t calibratingG = 0;
|
|
uint8_t calibratingM = 0;
|
|
uint16_t acc_1G = 256; // this is the 1G measured acceleration
|
|
int16_t heading, magHold;
|
|
|
|
extern uint16_t InflightcalibratingA;
|
|
extern int16_t AccInflightCalibrationArmed;
|
|
extern uint16_t AccInflightCalibrationMeasurementDone;
|
|
extern uint16_t AccInflightCalibrationSavetoEEProm;
|
|
extern uint16_t AccInflightCalibrationActive;
|
|
|
|
sensor_t acc; // acc access functions
|
|
sensor_t gyro; // gyro access functions
|
|
|
|
void sensorsAutodetect(void)
|
|
{
|
|
// Detect what's available
|
|
if (!adxl345Detect(&acc))
|
|
sensorsClear(SENSOR_ACC);
|
|
if (!bmp085Init())
|
|
sensorsClear(SENSOR_BARO);
|
|
if (!hmc5883lDetect())
|
|
sensorsClear(SENSOR_MAG);
|
|
|
|
// Init sensors
|
|
if (sensors(SENSOR_ACC))
|
|
acc.init();
|
|
if (sensors(SENSOR_BARO))
|
|
bmp085Init();
|
|
|
|
// special case for supported gyros - MPU3050 and MPU6050
|
|
if (mpu6050Detect(&acc, &gyro)) { // first, try MPU6050, and re-enable acc (if ADXL345 is missing) since this chip has it built in
|
|
sensorsSet(SENSOR_ACC);
|
|
acc.init();
|
|
} else if (!mpu3050Detect(&gyro)) {
|
|
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
|
|
failureMode(3);
|
|
}
|
|
// this is safe because either mpu6050 or mpu3050 sets it, and in case of fail, none do.
|
|
gyro.init();
|
|
}
|
|
|
|
static void ACC_Common(void)
|
|
{
|
|
static int32_t a[3];
|
|
uint8_t axis;
|
|
|
|
if (calibratingA > 0) {
|
|
for (axis = 0; axis < 3; axis++) {
|
|
// Reset a[axis] at start of calibration
|
|
if (calibratingA == 400)
|
|
a[axis] = 0;
|
|
// Sum up 400 readings
|
|
a[axis] += accADC[axis];
|
|
// Clear global variables for next reading
|
|
accADC[axis] = 0;
|
|
cfg.accZero[axis] = 0;
|
|
}
|
|
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
|
if (calibratingA == 1) {
|
|
cfg.accZero[ROLL] = a[ROLL] / 400;
|
|
cfg.accZero[PITCH] = a[PITCH] / 400;
|
|
cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
|
|
cfg.accTrim[ROLL] = 0;
|
|
cfg.accTrim[PITCH] = 0;
|
|
writeParams(); // 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 accTrim_saved[2] = { 0, 0 };
|
|
//Saving old zeropoints before measurement
|
|
if (InflightcalibratingA == 50) {
|
|
accZero_saved[ROLL] = cfg.accZero[ROLL];
|
|
accZero_saved[PITCH] = cfg.accZero[PITCH];
|
|
accZero_saved[YAW] = cfg.accZero[YAW];
|
|
accTrim_saved[ROLL] = cfg.accTrim[ROLL];
|
|
accTrim_saved[PITCH] = cfg.accTrim[PITCH];
|
|
}
|
|
if (InflightcalibratingA > 0) {
|
|
uint8_t axis;
|
|
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;
|
|
cfg.accZero[axis] = 0;
|
|
}
|
|
//all values are measured
|
|
if (InflightcalibratingA == 1) {
|
|
AccInflightCalibrationActive = 0;
|
|
AccInflightCalibrationMeasurementDone = 1;
|
|
blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight
|
|
// recover saved values to maintain current flight behavior until new values are transferred
|
|
cfg.accZero[ROLL] = accZero_saved[ROLL];
|
|
cfg.accZero[PITCH] = accZero_saved[PITCH];
|
|
cfg.accZero[YAW] = accZero_saved[YAW];
|
|
cfg.accTrim[ROLL] = accTrim_saved[ROLL];
|
|
cfg.accTrim[PITCH] = accTrim_saved[PITCH];
|
|
}
|
|
InflightcalibratingA--;
|
|
}
|
|
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
|
if (AccInflightCalibrationSavetoEEProm == 1) { //the copter is landed, disarmed and the combo has been done again
|
|
AccInflightCalibrationSavetoEEProm = 0;
|
|
cfg.accZero[ROLL] = b[ROLL] / 50;
|
|
cfg.accZero[PITCH] = b[PITCH] / 50;
|
|
cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
|
|
cfg.accTrim[ROLL] = 0;
|
|
cfg.accTrim[PITCH] = 0;
|
|
writeParams(); // write accZero in EEPROM
|
|
}
|
|
}
|
|
|
|
accADC[ROLL] -= cfg.accZero[ROLL];
|
|
accADC[PITCH] -= cfg.accZero[PITCH];
|
|
accADC[YAW] -= cfg.accZero[YAW];
|
|
}
|
|
|
|
|
|
void ACC_getADC(void)
|
|
{
|
|
acc.read(accADC);
|
|
acc.orient(accADC);
|
|
|
|
ACC_Common();
|
|
}
|
|
|
|
static uint32_t baroDeadline = 0;
|
|
static uint8_t baroState = 0;
|
|
static uint16_t baroUT = 0;
|
|
static uint32_t baroUP = 0;
|
|
static int16_t baroTemp = 0;
|
|
|
|
void Baro_update(void)
|
|
{
|
|
int32_t pressure;
|
|
|
|
if (currentTime < baroDeadline)
|
|
return;
|
|
|
|
baroDeadline = currentTime;
|
|
|
|
switch (baroState) {
|
|
case 0:
|
|
bmp085_start_ut();
|
|
baroState++;
|
|
baroDeadline += 4600;
|
|
break;
|
|
case 1:
|
|
baroUT = bmp085_get_ut();
|
|
baroState++;
|
|
break;
|
|
case 2:
|
|
bmp085_start_up();
|
|
baroState++;
|
|
baroDeadline += 14000;
|
|
break;
|
|
case 3:
|
|
baroUP = bmp085_get_up();
|
|
baroTemp = bmp085_get_temperature(baroUT);
|
|
pressure = bmp085_get_pressure(baroUP);
|
|
|
|
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
|
|
baroState = 0;
|
|
baroDeadline += 5000;
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void GYRO_Common(void)
|
|
{
|
|
static int16_t previousGyroADC[3] = { 0, 0, 0 };
|
|
static int32_t g[3];
|
|
uint8_t axis;
|
|
|
|
#if defined MMGYRO
|
|
// Moving Average Gyros by Magnetron1
|
|
//---------------------------------------------------
|
|
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGTH];
|
|
static int32_t mediaMobileGyroADCSum[3];
|
|
static uint8_t mediaMobileGyroIDX;
|
|
//---------------------------------------------------
|
|
#endif
|
|
|
|
if (calibratingG > 0) {
|
|
for (axis = 0; axis < 3; axis++) {
|
|
// Reset g[axis] at start of calibration
|
|
if (calibratingG == 400)
|
|
g[axis] = 0;
|
|
// Sum up 400 readings
|
|
g[axis] += gyroADC[axis];
|
|
// Clear global variables for next reading
|
|
gyroADC[axis] = 0;
|
|
gyroZero[axis] = 0;
|
|
if (calibratingG == 1) {
|
|
gyroZero[axis] = g[axis] / 400;
|
|
blinkLED(10, 15, 1);
|
|
}
|
|
}
|
|
calibratingG--;
|
|
}
|
|
|
|
#ifdef MMGYRO
|
|
mediaMobileGyroIDX = ++mediaMobileGyroIDX % MMGYROVECTORLENGTH;
|
|
for (axis = 0; axis < 3; axis++) {
|
|
gyroADC[axis] -= gyroZero[axis];
|
|
mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX];
|
|
//anti gyro glitch, limit the variation between two consecutive readings
|
|
mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
|
|
mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX];
|
|
gyroADC[axis] = mediaMobileGyroADCSum[axis] / MMGYROVECTORLENGTH;
|
|
previousGyroADC[axis] = gyroADC[axis];
|
|
}
|
|
#else
|
|
for (axis = 0; axis < 3; axis++) {
|
|
gyroADC[axis] -= gyroZero[axis];
|
|
//anti gyro glitch, limit the variation between two consecutive readings
|
|
gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
|
|
previousGyroADC[axis] = gyroADC[axis];
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void Gyro_getADC(void)
|
|
{
|
|
// range: +/- 8192; +/- 2000 deg/sec
|
|
gyro.read(gyroADC);
|
|
gyro.orient(gyroADC);
|
|
|
|
GYRO_Common();
|
|
}
|
|
|
|
static float magCal[3] = { 1.0, 1.0, 1.0 }; // gain for each axis, populated at sensor init
|
|
static uint8_t magInit = 0;
|
|
|
|
static void Mag_getRawADC(void)
|
|
{
|
|
static int16_t rawADC[3];
|
|
hmc5883lRead(rawADC);
|
|
|
|
// no way? is THIS finally the proper orientation?? (by GrootWitBaas)
|
|
magADC[ROLL] = rawADC[2]; // X
|
|
magADC[PITCH] = -rawADC[0]; // Y
|
|
magADC[YAW] = -rawADC[1]; // Z
|
|
}
|
|
|
|
void Mag_init(void)
|
|
{
|
|
// initial calibration
|
|
hmc5883lInit();
|
|
delay(100);
|
|
Mag_getRawADC();
|
|
delay(10);
|
|
|
|
magCal[ROLL] = 1000.0 / abs(magADC[ROLL]);
|
|
magCal[PITCH] = 1000.0 / abs(magADC[PITCH]);
|
|
magCal[YAW] = 1000.0 / abs(magADC[YAW]);
|
|
|
|
hmc5883lFinishCal();
|
|
magInit = 1;
|
|
}
|
|
|
|
void Mag_getADC(void)
|
|
{
|
|
static uint32_t t, tCal = 0;
|
|
static int16_t magZeroTempMin[3];
|
|
static int16_t magZeroTempMax[3];
|
|
uint8_t axis;
|
|
|
|
if (currentTime < t)
|
|
return; //each read is spaced by 100ms
|
|
t = currentTime + 100000;
|
|
|
|
// Read mag sensor
|
|
Mag_getRawADC();
|
|
|
|
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
|
|
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
|
|
magADC[YAW] = magADC[YAW] * magCal[YAW];
|
|
|
|
if (calibratingM == 1) {
|
|
tCal = t;
|
|
for (axis = 0; axis < 3; axis++) {
|
|
cfg.magZero[axis] = 0;
|
|
magZeroTempMin[axis] = magADC[axis];
|
|
magZeroTempMax[axis] = magADC[axis];
|
|
}
|
|
calibratingM = 0;
|
|
}
|
|
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
|
|
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
|
|
magADC[YAW] = magADC[YAW] * magCal[YAW];
|
|
if (magInit) { // we apply offset only once mag calibration is done
|
|
magADC[ROLL] -= cfg.magZero[ROLL];
|
|
magADC[PITCH] -= cfg.magZero[PITCH];
|
|
magADC[YAW] -= cfg.magZero[YAW];
|
|
}
|
|
|
|
if (tCal != 0) {
|
|
if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
|
|
LED0_TOGGLE;
|
|
for (axis = 0; axis < 3; axis++) {
|
|
if (magADC[axis] < magZeroTempMin[axis])
|
|
magZeroTempMin[axis] = magADC[axis];
|
|
if (magADC[axis] > magZeroTempMax[axis])
|
|
magZeroTempMax[axis] = magADC[axis];
|
|
}
|
|
} else {
|
|
tCal = 0;
|
|
for (axis = 0; axis < 3; axis++)
|
|
cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2;
|
|
writeParams();
|
|
}
|
|
}
|
|
}
|