From 6c92ea8ee834be87f8f56c4f12953c5b4ecc5869 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 18 Feb 2015 22:55:05 +0100 Subject: [PATCH 1/7] Harakiri PID fix Change errorGyroI and errorAngleI from int32 to float --- src/main/flight/pid.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fccab9ab5..0abc12eff 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; + errorAngleIf[PITCH] = 0; } void resetErrorGyro(void) @@ -554,19 +558,19 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) 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; + 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); + 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); From 6548c90ca80e4c4e527fcab6bda9f5426445bc90 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 19 Feb 2015 08:30:41 +0100 Subject: [PATCH 2/7] Align Harakiri PID with Crashpilot1000 updates --- src/main/flight/pid.c | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0abc12eff..c63d85889 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -75,8 +75,8 @@ void resetErrorAngle(void) errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; - errorAngleIf[ROLL] = 0; - errorAngleIf[PITCH] = 0; + errorAngleIf[ROLL] = 0.0f; + errorAngleIf[PITCH] = 0.0f; } void resetErrorGyro(void) @@ -85,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 }; @@ -523,13 +523,14 @@ 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 PTerm = 0.0f, ITerm = 0.0f, DTerm = 0.0f, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO = 0.0f, error = 0.0f, prop = 0.0f; + static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f }; + float gyroDataQuant[2] = { 0.0f, 0.0f }; float tmp0flt; int32_t tmp0; uint8_t axis; - float ACCDeltaTimeINS = 0; - float FLOATcycleTime = 0; + float ACCDeltaTimeINS = 0.0f; + float FLOATcycleTime = 0.0f; // 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 @@ -542,6 +543,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } for (axis = 0; axis < 2; axis++) { + tmp0 = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea + gyroDataQuant[axis] = (float)tmp0 * 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 @@ -566,7 +569,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) if (ABS((int16_t)gyroData[axis]) > 2560) { errorGyroIf[axis] = 0.0f; } else { - error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis]; + error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant[axis]; errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); } @@ -584,10 +587,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = ITermACC; } - PTerm -= gyroData[axis] * dynP8[axis] * 0.003f; - delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS; + PTerm -= gyroDataQuant[axis] * dynP8[axis] * 0.003f; + delta = (gyroDataQuant[axis] - lastGyro[axis]) / ACCDeltaTimeINS; - lastGyro[axis] = gyroData[axis]; + lastGyro[axis] = gyroDataQuant[axis]; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; From cd94377651c8f298ae575ea825e5b3107d3e22e1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 19 Feb 2015 09:00:20 +0100 Subject: [PATCH 3/7] Latest Crashpilot1000 update --- src/main/flight/pid.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c63d85889..42cfe7f9c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -522,10 +522,9 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { UNUSED(rxConfig); - float delta, RCfactor, rcCommandAxis, MainDptCut; + float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant; float PTerm = 0.0f, ITerm = 0.0f, DTerm = 0.0f, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO = 0.0f, error = 0.0f, prop = 0.0f; static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f }; - float gyroDataQuant[2] = { 0.0f, 0.0f }; float tmp0flt; int32_t tmp0; uint8_t axis; @@ -544,7 +543,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) for (axis = 0; axis < 2; axis++) { tmp0 = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea - gyroDataQuant[axis] = (float)tmp0 * 3.2f; // but delivers more accuracy and also reduces jittery flight + gyroDataQuant = (float)tmp0 * 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 @@ -569,7 +568,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) if (ABS((int16_t)gyroData[axis]) > 2560) { errorGyroIf[axis] = 0.0f; } else { - error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant[axis]; + error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant; errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); } @@ -587,10 +586,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = ITermACC; } - PTerm -= gyroDataQuant[axis] * dynP8[axis] * 0.003f; - delta = (gyroDataQuant[axis] - lastGyro[axis]) / ACCDeltaTimeINS; + PTerm -= gyroDataQuant * dynP8[axis] * 0.003f; + delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS; - lastGyro[axis] = gyroDataQuant[axis]; + lastGyro[axis] = gyroDataQuant; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; From 77e5be5002da846c642c7159b137bf449a940f6e Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Thu, 19 Feb 2015 15:25:32 +1000 Subject: [PATCH 4/7] Fixed external barometer & magnetometer detection --- src/main/sensors/initialisation.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index f278893e7..ee6f3913c 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -381,12 +381,14 @@ static void detectBaro() #ifdef USE_BARO_MS5611 if (ms5611Detect(&baro)) { + sensorsSet(SENSOR_BARO); return; } #endif #ifdef USE_BARO_BMP085 if (bmp085Detect(bmp085Config, &baro)) { + sensorsSet(SENSOR_BARO); return; } #endif @@ -440,6 +442,7 @@ retry: #ifdef USE_MAG_HMC5883 case MAG_HMC5883: if (hmc5883lDetect(&mag, hmc5883Config)) { + sensorsSet(SENSOR_MAG); #ifdef MAG_HMC5883_ALIGN magAlign = MAG_HMC5883_ALIGN; #endif @@ -455,6 +458,7 @@ retry: case MAG_AK8975: if (ak8975detect(&mag)) { + sensorsSet(SENSOR_MAG); #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif From c45efac8122d9f668ff4e80c8594c14e61736bc3 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 19 Feb 2015 16:15:14 +0000 Subject: [PATCH 5/7] Cleanup sensor detection. Less code required and a similar pattern is used for each type of sensor. --- src/main/main.c | 3 - src/main/sensors/acceleration.c | 2 +- src/main/sensors/acceleration.h | 4 +- src/main/sensors/barometer.c | 1 + src/main/sensors/barometer.h | 9 +++ src/main/sensors/compass.c | 2 +- src/main/sensors/compass.h | 2 +- src/main/sensors/initialisation.c | 97 +++++++++++++---------- src/main/target/ALIENWIIF3/target.h | 3 - src/main/target/CC3D/target.h | 2 - src/main/target/CHEBUZZF3/target.h | 3 - src/main/target/CJMCU/target.h | 2 - src/main/target/EUSTM32F103RC/target.h | 2 - src/main/target/NAZE/target.h | 2 - src/main/target/NAZE32PRO/target.h | 2 - src/main/target/OLIMEXINO/target.h | 3 - src/main/target/PORT103R/target.h | 2 - src/main/target/SPARKY/target.h | 3 - src/main/target/SPRACINGF3/target.h | 2 - src/main/target/STM32F3DISCOVERY/target.h | 2 - 20 files changed, 70 insertions(+), 78 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index f83d5e23f..1cc6a1878 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -294,9 +294,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. diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index f02e0e992..b43711f0b 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -37,7 +37,7 @@ int16_t accADC[XYZ_AXIS_COUNT]; acc_t acc; // acc access functions -uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected +accelerationSensor_e 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. diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 159393aa1..709f4415a 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -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,7 +29,7 @@ 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; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index f019dd628..719c83eed 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -41,6 +41,7 @@ static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; barometerConfig_t *barometerConfig; +baroSensor_e baroHardware; void useBarometerConfig(barometerConfig_t *barometerConfigToUse) { diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 9edd7c6a5..fed38d0aa 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -17,6 +17,15 @@ #pragma once +typedef enum { + BARO_NONE = 0, + BARO_DEFAULT = 1, + BARO_BMP085 = 2, + BARO_MS5611 = 3 +} baroSensor_e; + +extern baroSensor_e baroHardware; + #define BARO_SAMPLE_COUNT_MAX 48 typedef struct barometerConfig_s { diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index e3c18036a..7c5f11f6a 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,7 +40,7 @@ #endif mag_t mag; // mag access functions -uint8_t magHardware = MAG_DEFAULT; +magSensor_e magHardware = MAG_DEFAULT; extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead. diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 374cac185..2effbabc9 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -18,7 +18,7 @@ #pragma once // Type of accelerometer used/detected -typedef enum MagSensors { +typedef enum { MAG_DEFAULT = 0, MAG_HMC5883 = 1, MAG_AK8975 = 2, diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index ee6f3913c..883c253f1 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -233,11 +233,10 @@ retry: } #endif case ACC_NONE: // disable ACC - sensorsClear(SENSOR_ACC); break; case ACC_DEFAULT: // autodetect -#ifdef USE_ACC_ADXL345 case ACC_ADXL345: // ADXL345 +#ifdef USE_ACC_ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently #ifdef NAZE @@ -252,10 +251,10 @@ retry: if (accHardwareToUse == ACC_ADXL345) 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; @@ -264,10 +263,10 @@ retry: if (accHardwareToUse == ACC_LSM303DLHC) 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; @@ -276,10 +275,10 @@ retry: if (accHardwareToUse == ACC_MPU6050) 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)) { @@ -293,10 +292,10 @@ retry: if (accHardwareToUse == ACC_MMA8452) 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; @@ -305,10 +304,10 @@ retry: if (accHardwareToUse == ACC_BMA280) 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; @@ -317,10 +316,10 @@ retry: if (accHardwareToUse == ACC_SPI_MPU6000) 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 @@ -333,9 +332,8 @@ retry: if (accHardwareToUse == ACC_SPI_MPU6500) break; } - ; // fallthrough #endif - ; // prevent compiler error + ; // fallthrough } // Found anything? Check if error or ACC is really missing. @@ -344,18 +342,21 @@ retry: // 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 (accHardware != ACC_NONE) { + 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 + baroHardware = BARO_DEFAULT; + #ifdef USE_BARO_BMP085 const bmp085Config_t *bmp085Config = NULL; @@ -379,24 +380,37 @@ static void detectBaro() #endif -#ifdef USE_BARO_MS5611 - if (ms5611Detect(&baro)) { - sensorsSet(SENSOR_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)) { - sensorsSet(SENSOR_BARO); - return; + if (bmp085Detect(bmp085Config, &baro)) { + baroHardware = BARO_BMP085; + break; + } +#endif + baroHardware = BARO_NONE; // nothing detected or enabled. + case BARO_NONE: + break; } -#endif -#endif - sensorsClear(SENSOR_BARO); + + if (baroHardware != BARO_NONE) { + sensorsSet(SENSOR_BARO); + } + #endif } -static void detectMag(uint8_t magHardwareToUse) +static void detectMag(magSensor_e magHardwareToUse) { #ifdef USE_MAG_HMC5883 static hmc5883Config_t *hmc5883Config = 0; @@ -435,12 +449,11 @@ retry: switch(magHardwareToUse) { case MAG_NONE: // disable MAG - sensorsClear(SENSOR_MAG); break; case MAG_DEFAULT: // autodetect -#ifdef USE_MAG_HMC5883 case MAG_HMC5883: +#ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag, hmc5883Config)) { sensorsSet(SENSOR_MAG); #ifdef MAG_HMC5883_ALIGN @@ -451,11 +464,11 @@ retry: break; } - ; // fallthrough #endif + ; // fallthrough -#ifdef USE_MAG_AK8975 case MAG_AK8975: +#ifdef USE_MAG_AK8975 if (ak8975detect(&mag)) { sensorsSet(SENSOR_MAG); @@ -466,9 +479,8 @@ retry: if (magHardwareToUse == MAG_AK8975) break; } - ; // fallthrough #endif - ; // prevent compiler error. + ; // fallthrough } if (magHardware == MAG_DEFAULT) { @@ -476,12 +488,13 @@ retry: // 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 (magHardware != MAG_NONE) { + sensorsSet(SENSOR_MAG); + } + } void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h index 009782a69..924a9306f 100644 --- a/src/main/target/ALIENWIIF3/target.h +++ b/src/main/target/ALIENWIIF3/target.h @@ -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 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index a06d3540a..b0e1a2f7a 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -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 diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index e27951b9f..d8a0f704a 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -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 diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 2bdabd78e..ac4088e08 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -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 diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index ccc9bebaa..48ac2fd27 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -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 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index c2109e5c2..7d53b0c4e 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -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 diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index 19f806d71..7435a8e99 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -40,8 +40,6 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) -#define SENSORS_SET (SENSOR_ACC) - #define GPS #define BLACKBOX #define TELEMETRY diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 87dd9aadd..6288c6b40 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -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 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 0f4aa3904..f3af089d0 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -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 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index d9bfe31e3..2c6101d7c 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -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 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index ae1feff44..4af92665a 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -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 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 29f5f39e8..a98f03d84 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -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 From 257c7e092e3a3554a59323e09c4b4453269c1900 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 19 Feb 2015 21:28:26 +0100 Subject: [PATCH 6/7] Harakiri PID controller variables cleanup Flight tested --- src/main/flight/pid.c | 43 ++++++++++++++++++++----------------------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 42cfe7f9c..d12325048 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -523,13 +523,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) UNUSED(rxConfig); float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant; - float PTerm = 0.0f, ITerm = 0.0f, DTerm = 0.0f, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO = 0.0f, error = 0.0f, prop = 0.0f; + 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 }; - float tmp0flt; - int32_t tmp0; uint8_t axis; - float ACCDeltaTimeINS = 0.0f; - float FLOATcycleTime = 0.0f; + 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 @@ -542,8 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } for (axis = 0; axis < 2; axis++) { - tmp0 = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea - gyroDataQuant = (float)tmp0 * 3.2f; // but delivers more accuracy and also reduces jittery flight + 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 @@ -558,8 +555,8 @@ 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); + 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; } @@ -602,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; From 1de72b11ce770f4437cee789cf0ac1b08f88d09a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 19 Feb 2015 21:04:06 +0000 Subject: [PATCH 7/7] Update the cli `status` command to show all detected sensors (Except on CJMCU). Further cleanup of sensor initialisation. --- src/main/io/serial_cli.c | 52 ++++--- src/main/sensors/acceleration.c | 1 - src/main/sensors/acceleration.h | 1 - src/main/sensors/barometer.c | 1 - src/main/sensors/barometer.h | 2 - src/main/sensors/compass.c | 1 - src/main/sensors/compass.h | 7 +- src/main/sensors/gyro.h | 12 ++ src/main/sensors/initialisation.c | 217 ++++++++++++++++++------------ src/main/sensors/sensors.h | 10 ++ 10 files changed, 190 insertions(+), 114 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 276c9310a..7098e0610 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -146,14 +146,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; @@ -1357,27 +1366,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 diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index b43711f0b..c265b7595 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -37,7 +37,6 @@ int16_t accADC[XYZ_AXIS_COUNT]; acc_t acc; // acc access functions -accelerationSensor_e 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. diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 709f4415a..5b925b01d 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -31,7 +31,6 @@ typedef enum { ACC_NONE = 9 } accelerationSensor_e; -extern uint8_t accHardware; extern sensor_align_e accAlign; extern acc_t acc; extern uint16_t acc_1G; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 719c83eed..f019dd628 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -41,7 +41,6 @@ static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; barometerConfig_t *barometerConfig; -baroSensor_e baroHardware; void useBarometerConfig(barometerConfig_t *barometerConfigToUse) { diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index fed38d0aa..685f6b843 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -24,8 +24,6 @@ typedef enum { BARO_MS5611 = 3 } baroSensor_e; -extern baroSensor_e baroHardware; - #define BARO_SAMPLE_COUNT_MAX 48 typedef struct barometerConfig_s { diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 7c5f11f6a..31d7688c6 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,7 +40,6 @@ #endif mag_t mag; // mag access functions -magSensor_e magHardware = MAG_DEFAULT; extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead. diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 2effbabc9..502c18728 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -20,9 +20,9 @@ // Type of accelerometer used/detected 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; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index caa34281d..f09e2af5e 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 883c253f1..cd90c28f3 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -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,17 +265,16 @@ 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 - break; - case ACC_DEFAULT: // autodetect + ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; @@ -248,8 +288,7 @@ retry: accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; - if (accHardwareToUse == ACC_ADXL345) - break; + break; } #endif ; // fallthrough @@ -260,8 +299,7 @@ retry: accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; - if (accHardwareToUse == ACC_LSM303DLHC) - break; + break; } #endif ; // fallthrough @@ -272,8 +310,7 @@ retry: accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; - if (accHardwareToUse == ACC_MPU6050) - break; + break; } #endif ; // fallthrough @@ -289,8 +326,7 @@ retry: accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; - if (accHardwareToUse == ACC_MMA8452) - break; + break; } #endif ; // fallthrough @@ -301,8 +337,7 @@ retry: accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; - if (accHardwareToUse == ACC_BMA280) - break; + break; } #endif ; // fallthrough @@ -313,8 +348,7 @@ retry: accAlign = ACC_SPI_MPU6000_ALIGN; #endif accHardware = ACC_SPI_MPU6000; - if (accHardwareToUse == ACC_SPI_MPU6000) - break; + break; } #endif ; // fallthrough @@ -329,25 +363,30 @@ retry: accAlign = ACC_SPI_MPU6500_ALIGN; #endif accHardware = ACC_SPI_MPU6500; - if (accHardwareToUse == ACC_SPI_MPU6500) - break; + break; } #endif ; // 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; - } + 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) { - sensorsSet(SENSOR_ACC); + + if (accHardware == ACC_NONE) { + return; } + + detectedSensors[SENSOR_INDEX_ACC] = accHardware; + sensorsSet(SENSOR_ACC); } static void detectBaro() @@ -355,7 +394,7 @@ static void detectBaro() #ifdef BARO // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function - baroHardware = BARO_DEFAULT; + baroSensor_e baroHardware = BARO_DEFAULT; #ifdef USE_BARO_BMP085 @@ -399,19 +438,24 @@ static void detectBaro() break; } #endif - baroHardware = BARO_NONE; // nothing detected or enabled. case BARO_NONE: + baroHardware = BARO_NONE; break; } - if (baroHardware != BARO_NONE) { - sensorsSet(SENSOR_BARO); + if (baroHardware == BARO_NONE) { + return; } - #endif + + detectedSensors[SENSOR_INDEX_BARO] = baroHardware; + sensorsSet(SENSOR_BARO); +#endif } static void detectMag(magSensor_e magHardwareToUse) { + magSensor_e magHardware; + #ifdef USE_MAG_HMC5883 static hmc5883Config_t *hmc5883Config = 0; @@ -448,21 +492,17 @@ retry: magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { - case MAG_NONE: // disable MAG - break; - case MAG_DEFAULT: // autodetect + case MAG_DEFAULT: + ; // fallthrough case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag, hmc5883Config)) { - sensorsSet(SENSOR_MAG); #ifdef MAG_HMC5883_ALIGN magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; - if (magHardwareToUse == MAG_HMC5883) - break; - + break; } #endif ; // fallthrough @@ -470,31 +510,32 @@ retry: case MAG_AK8975: #ifdef USE_MAG_AK8975 if (ak8975detect(&mag)) { - - sensorsSet(SENSOR_MAG); #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; - if (magHardwareToUse == MAG_AK8975) - break; + break; } #endif ; // 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; - } + 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) { - sensorsSet(SENSOR_MAG); + if (magHardware == MAG_NONE) { + return; } + detectedSensors[SENSOR_INDEX_MAG] = magHardware; + sensorsSet(SENSOR_MAG); } void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) @@ -513,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(); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 805043d5c..78c452f46 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -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;