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

Merge branch 'master' into serial-cleanup

This commit is contained in:
Dominic Clifton 2015-02-19 21:57:54 +00:00
parent e0b8803503 242a7e65a8
commit 021496b244
23 changed files with 282 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
src/main/flight/pid.c
View File

@ -60,6 +60,7 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorAngleI[2] = { 0, 0 };
static float errorAngleIf[2] = { 0.0f, 0.0f };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -73,6 +74,9 @@ void resetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
errorAngleIf[ROLL] = 0.0f;
errorAngleIf[PITCH] = 0.0f;
}
void resetErrorGyro(void)
@ -81,9 +85,9 @@ void resetErrorGyro(void)
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorGyroIf[ROLL] = 0;
errorGyroIf[PITCH] = 0;
errorGyroIf[YAW] = 0;
errorGyroIf[ROLL] = 0.0f;
errorGyroIf[PITCH] = 0.0f;
errorGyroIf[YAW] = 0.0f;
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
@ -518,14 +522,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
float delta, RCfactor, rcCommandAxis, MainDptCut;
float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0;
static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0};
float tmp0flt;
int32_t tmp0;
float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant;
float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f;
static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f };
uint8_t axis;
float ACCDeltaTimeINS = 0;
float FLOATcycleTime = 0;
float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
@ -538,6 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
}
for (axis = 0; axis < 2; axis++) {
int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroDataQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS
@ -552,21 +555,21 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
}
#endif
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt);
errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -limitf, +limitf);
errorAngleIf[axis] = constrain(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
}
if (!FLIGHT_MODE(ANGLE_MODE)) {
if (ABS((int16_t)gyroData[axis]) > 2560) {
errorGyroI[axis] = 0.0f;
errorGyroIf[axis] = 0.0f;
} else {
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
}
ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
ITermGYRO = errorGyroIf[axis] * (float)pidProfile->I8[axis] * 0.01f;
if (FLIGHT_MODE(HORIZON_MODE)) {
PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
@ -580,10 +583,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
ITerm = ITermACC;
}
PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
PTerm -= gyroDataQuant * dynP8[axis] * 0.003f;
delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroData[axis];
lastGyro[axis] = gyroDataQuant;
lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
@ -596,37 +599,37 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
#endif
}
tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
tmp0flt /= 3000.0f;
Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
Mwii3msTimescale /= 3000.0f;
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
errorGyroI[FD_YAW] = 0;
} else {
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp;
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp
ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
}
} else {
tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp0) > 50) {
if (ABS(tmp) > 50) {
errorGyroI[FD_YAW] = 0;
} else {
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * Mwii3msTimescale), -268435454, +268435454);
}
ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
tmp0 = 300;
if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
PTerm = constrain(PTerm, -tmp0, tmp0);
int32_t limit = 300;
if (pidProfile->D8[FD_YAW]) limit -= (int32_t)pidProfile->D8[FD_YAW];
PTerm = constrain(PTerm, -limit, limit);
}
}
axisPID[FD_YAW] = PTerm + ITerm;

52
src/main/io/serial_cli.c
View File

@ -148,14 +148,23 @@ static const char * const featureNames[] = {
"BLACKBOX", NULL
};
#ifndef CJMCU
// sync this with sensors_e
static const char * const sensorNames[] = {
static const char * const sensorTypeNames[] = {
"GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
};
static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL
// FIXME the next time the EEPROM is bumped change the order of acc and gyro names so that "None" is second.
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
static const char * const sensorHardwareNames[4][11] = {
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL },
{ "", "None", "BMP085", "MS5611", NULL },
{ "", "None", "HMC5883", "AK8975", NULL }
};
#endif
typedef struct {
const char *name;
@ -1353,27 +1362,38 @@ static void cliGet(char *cmdline)
static void cliStatus(char *cmdline)
{
uint8_t i;
uint32_t mask;
UNUSED(cmdline);
printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
millis() / 1000, vbat, batteryCellCount);
mask = sensorsMask();
printf("CPU %dMHz, detected sensors: ", (SystemCoreClock / 1000000));
printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
#ifndef CJMCU
uint8_t i;
uint32_t mask;
uint32_t detectedSensorsMask = sensorsMask();
for (i = 0; ; i++) {
if (sensorNames[i] == NULL)
if (sensorTypeNames[i] == NULL)
break;
if (mask & (1 << i))
printf("%s ", sensorNames[i]);
}
if (sensors(SENSOR_ACC)) {
printf("ACCHW: %s", accNames[accHardware]);
if (acc.revisionCode)
printf(".%c", acc.revisionCode);
mask = (1 << i);
if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) {
const char *sensorHardware;
uint8_t sensorHardwareIndex = detectedSensors[i];
sensorHardware = sensorHardwareNames[i][sensorHardwareIndex];
printf(", %s=%s", sensorTypeNames[i], sensorHardware);
if (mask == SENSOR_ACC && acc.revisionCode) {
printf(".%c", acc.revisionCode);
}
}
}
#endif
cliPrint("\r\n");
#ifdef USE_I2C

3
src/main/main.c
View File

@ -296,9 +296,6 @@ void init(void)
}
#endif
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
// if gyro was not detected due to whatever reason, we give up now.

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

@ -37,7 +37,6 @@
int16_t accADC[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
sensor_align_e accAlign = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.

5
src/main/sensors/acceleration.h
View File

@ -18,7 +18,7 @@
#pragma once
// Type of accelerometer used/detected
typedef enum AccelSensors {
typedef enum {
ACC_DEFAULT = 0,
ACC_ADXL345 = 1,
ACC_MPU6050 = 2,
@ -29,9 +29,8 @@ typedef enum AccelSensors {
ACC_SPI_MPU6500 = 7,
ACC_FAKE = 8,
ACC_NONE = 9
} accelSensor_e;
} accelerationSensor_e;
extern uint8_t accHardware;
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint16_t acc_1G;

7
src/main/sensors/barometer.h
View File

@ -17,6 +17,13 @@
#pragma once
typedef enum {
BARO_NONE = 0,
BARO_DEFAULT = 1,
BARO_BMP085 = 2,
BARO_MS5611 = 3
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48
typedef struct barometerConfig_s {

1
src/main/sensors/compass.c
View File

@ -40,7 +40,6 @@
#endif
mag_t mag; // mag access functions
uint8_t magHardware = MAG_DEFAULT;
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.

9
src/main/sensors/compass.h
View File

@ -18,11 +18,11 @@
#pragma once
// Type of accelerometer used/detected
typedef enum MagSensors {
typedef enum {
MAG_DEFAULT = 0,
MAG_HMC5883 = 1,
MAG_AK8975 = 2,
MAG_NONE = 3
MAG_NONE = 1,
MAG_HMC5883 = 2,
MAG_AK8975 = 3,
} magSensor_e;
#ifdef MAG
@ -32,6 +32,5 @@ void updateCompass(flightDynamicsTrims_t *magZero);
extern int16_t magADC[XYZ_AXIS_COUNT];
extern uint8_t magHardware;
extern sensor_align_e magAlign;
extern mag_t mag;

12
src/main/sensors/gyro.h
View File

@ -17,6 +17,18 @@
#pragma once
typedef enum {
GYRO_NONE = 0,
GYRO_DEFAULT,
GYRO_MPU6050,
GYRO_L3G4200D,
GYRO_MPU3050,
GYRO_L3GD20,
GYRO_SPI_MPU6000,
GYRO_SPI_MPU6500,
GYRO_FAKE
} gyroSensor_e;
extern gyro_t gyro;
extern sensor_align_e gyroAlign;

288
src/main/sensors/initialisation.c
View File

@ -71,6 +71,8 @@ extern gyro_t gyro;
extern baro_t baro;
extern acc_t acc;
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
const mpu6050Config_t *selectMPU6050Config(void)
{
#ifdef NAZE
@ -141,82 +143,121 @@ bool fakeAccDetect(acc_t *acc)
bool detectGyro(uint16_t gyroLpf)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
#ifdef GYRO_MPU6050_ALIGN
gyroAlign = GYRO_MPU6050_ALIGN;
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroAlign = GYRO_L3G4200D_ALIGN;
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef GYRO_MPU3050_ALIGN
gyroAlign = GYRO_MPU3050_ALIGN;
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
if (l3gd20Detect(&gyro, gyroLpf)) {
#ifdef GYRO_L3GD20_ALIGN
gyroAlign = GYRO_L3GD20_ALIGN;
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
gyroHardware = GYRO_SPI_MPU6000;
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6500:
#ifdef USE_GYRO_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
return true;
}
break;
}
#else
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
return true;
}
break;
}
#endif
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
return true;
}
if (fakeGyroDetect(&gyro, gyroLpf)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
return false;
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
static void detectAcc(uint8_t accHardwareToUse)
static void detectAcc(accelerationSensor_e accHardwareToUse)
{
#ifdef USE_ACC_ADXL345
accelerationSensor_e accHardware;
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;
#endif
@ -224,20 +265,18 @@ retry:
accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) {
case ACC_DEFAULT:
; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
default:
if (fakeAccDetect(&acc)) {
accHardware = ACC_FAKE;
if (accHardwareToUse == ACC_FAKE)
break;
break;
}
#endif
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
#ifdef USE_ACC_ADXL345
; // fallthrough
case ACC_ADXL345: // ADXL345
#ifdef USE_ACC_ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
#ifdef NAZE
@ -249,37 +288,34 @@ retry:
accAlign = ACC_ADXL345_ALIGN;
#endif
accHardware = ACC_ADXL345;
if (accHardwareToUse == ACC_ADXL345)
break;
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_LSM303DLHC
; // fallthrough
case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
if (lsm303dlhcAccDetect(&acc)) {
#ifdef ACC_LSM303DLHC_ALIGN
accAlign = ACC_LSM303DLHC_ALIGN;
#endif
accHardware = ACC_LSM303DLHC;
if (accHardwareToUse == ACC_LSM303DLHC)
break;
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_MPU6050
; // fallthrough
case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
#ifdef ACC_MPU6050_ALIGN
accAlign = ACC_MPU6050_ALIGN;
#endif
accHardware = ACC_MPU6050;
if (accHardwareToUse == ACC_MPU6050)
break;
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_MMA8452
; // fallthrough
case ACC_MMA8452: // MMA8452
#ifdef USE_ACC_MMA8452
#ifdef NAZE
// Not supported with this frequency
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
@ -290,37 +326,34 @@ retry:
accAlign = ACC_MMA8452_ALIGN;
#endif
accHardware = ACC_MMA8452;
if (accHardwareToUse == ACC_MMA8452)
break;
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_BMA280
; // fallthrough
case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
if (bma280Detect(&acc)) {
#ifdef ACC_BMA280_ALIGN
accAlign = ACC_BMA280_ALIGN;
#endif
accHardware = ACC_BMA280;
if (accHardwareToUse == ACC_BMA280)
break;
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_SPI_MPU6000
; // fallthrough
case ACC_SPI_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_SPI_MPU6000_ALIGN
accAlign = ACC_SPI_MPU6000_ALIGN;
#endif
accHardware = ACC_SPI_MPU6000;
if (accHardwareToUse == ACC_SPI_MPU6000)
break;
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_SPI_MPU6500
; // fallthrough
case ACC_SPI_MPU6500:
#ifdef USE_ACC_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
#else
@ -330,32 +363,39 @@ retry:
accAlign = ACC_SPI_MPU6500_ALIGN;
#endif
accHardware = ACC_SPI_MPU6500;
if (accHardwareToUse == ACC_SPI_MPU6500)
break;
break;
}
; // fallthrough
#endif
; // prevent compiler error
; // fallthrough
case ACC_NONE: // disable ACC
accHardware = ACC_NONE;
break;
}
// Found anything? Check if error or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (accHardwareToUse > ACC_DEFAULT && accHardwareToUse < ACC_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
accHardwareToUse = ACC_DEFAULT;
goto retry;
} else {
// No ACC was detected
sensorsClear(SENSOR_ACC);
}
if (accHardwareToUse != ACC_DEFAULT && accHardware == ACC_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
accHardwareToUse = ACC_DEFAULT;
goto retry;
}
if (accHardware == ACC_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
}
static void detectBaro()
{
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
#ifdef BARO
baroSensor_e baroHardware = BARO_DEFAULT;
#ifdef USE_BARO_BMP085
const bmp085Config_t *bmp085Config = NULL;
@ -379,23 +419,43 @@ static void detectBaro()
#endif
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) {
return;
}
#endif
switch (baroHardware) {
case BARO_DEFAULT:
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) {
baroHardware = BARO_MS5611;
break;
}
#endif
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro)) {
if (bmp085Detect(bmp085Config, &baro)) {
baroHardware = BARO_BMP085;
break;
}
#endif
case BARO_NONE:
baroHardware = BARO_NONE;
break;
}
if (baroHardware == BARO_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
#endif
#endif
sensorsClear(SENSOR_BARO);
}
static void detectMag(uint8_t magHardwareToUse)
static void detectMag(magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
#ifdef USE_MAG_HMC5883
static hmc5883Config_t *hmc5883Config = 0;
@ -432,52 +492,50 @@ retry:
magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) {
case MAG_NONE: // disable MAG
sensorsClear(SENSOR_MAG);
break;
case MAG_DEFAULT: // autodetect
case MAG_DEFAULT:
; // fallthrough
#ifdef USE_MAG_HMC5883
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
if (magHardwareToUse == MAG_HMC5883)
break;
break;
}
; // fallthrough
#endif
; // fallthrough
#ifdef USE_MAG_AK8975
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975detect(&mag)) {
#ifdef MAG_AK8975_ALIGN
magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
if (magHardwareToUse == MAG_AK8975)
break;
break;
}
; // fallthrough
#endif
; // prevent compiler error.
; // fallthrough
case MAG_NONE:
magHardware = MAG_NONE;
break;
}
if (magHardware == MAG_DEFAULT) {
if (magHardwareToUse > MAG_DEFAULT && magHardwareToUse < MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
} else {
// No MAG was detected
sensorsClear(SENSOR_MAG);
}
if (magHardwareToUse != MAG_DEFAULT && magHardware == MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
}
if (magHardware == MAG_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
}
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
@ -496,13 +554,13 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig)
{
int16_t deg, min;
memset(&acc, sizeof(acc), 0);
memset(&gyro, sizeof(gyro), 0);
if (!detectGyro(gyroLpf)) {
return false;
}
sensorsSet(SENSOR_GYRO);
detectAcc(accHardwareToUse);
detectBaro();

10
src/main/sensors/sensors.h
View File

@ -17,6 +17,16 @@
#pragma once
typedef enum {
SENSOR_INDEX_GYRO = 0,
SENSOR_INDEX_ACC,
SENSOR_INDEX_BARO,
SENSOR_INDEX_MAG
} sensorIndex_e;
#define MAX_SENSORS_TO_DETECT (SENSOR_INDEX_MAG + 1)
extern uint8_t detectedSensors[MAX_SENSORS_TO_DETECT];
typedef struct int16_flightDynamicsTrims_s {
int16_t roll;

3
src/main/target/ALIENWIIF3/target.h
View File

@ -86,9 +86,6 @@
#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10
#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define BLACKBOX
#define SERIAL_RX
#define GPS

2
src/main/target/CC3D/target.h
View File

@ -98,8 +98,6 @@
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define SENSORS_SET (SENSOR_ACC)
#define GPS
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

3
src/main/target/CHEBUZZF3/target.h
View File

@ -90,9 +90,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP
#if 1

2
src/main/target/CJMCU/target.h
View File

@ -60,8 +60,6 @@
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
#define SERIAL_RX
#define SPEKTRUM_BIND

2
src/main/target/EUSTM32F103RC/target.h
View File

@ -116,8 +116,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

2
src/main/target/NAZE/target.h
View File

@ -135,8 +135,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP

2
src/main/target/NAZE32PRO/target.h
View File

@ -40,8 +40,6 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define SENSORS_SET (SENSOR_ACC)
#define GPS
#define BLACKBOX
#define TELEMETRY

3
src/main/target/OLIMEXINO/target.h
View File

@ -103,9 +103,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

2
src/main/target/PORT103R/target.h
View File

@ -126,8 +126,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define LED0
#define GPS
#define LED_STRIP

3
src/main/target/SPARKY/target.h
View File

@ -86,9 +86,6 @@
#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10
#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define BLACKBOX
#define SERIAL_RX
#define GPS

2
src/main/target/SPRACINGF3/target.h
View File

@ -120,8 +120,6 @@
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define BLACKBOX
#define TELEMETRY

2
src/main/target/STM32F3DISCOVERY/target.h
View File

@ -81,8 +81,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
#define BLACKBOX
#define GPS
#define LED_STRIP