From 9d56b4a00fca7e1f6a9a038e335a96487b4a3b1e Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 23 Apr 2014 00:35:39 +0100 Subject: [PATCH] Remove sensor_initialisation.c's dependency on mw.h/board.h. Extracted sensor alignment variables to sensorAlignmentConfig This commit marks the end of the sensor dependency cleanup. --- src/config.c | 14 ++++-- src/config_master.h | 6 +-- src/main.c | 4 +- src/sensors_common.h | 6 +++ src/sensors_initialisation.c | 82 ++++++++++++++++++++++++------------ src/serial_cli.c | 6 +-- 6 files changed, 80 insertions(+), 38 deletions(-) diff --git a/src/config.c b/src/config.c index 8618843f70..c5bb92d60c 100755 --- a/src/config.c +++ b/src/config.c @@ -235,6 +235,13 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_cf_alt = 0.965f; } +void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) +{ + sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT; + sensorAlignmentConfig->acc_align = ALIGN_DEFAULT; + sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; +} + // Default settings static void resetConf(void) { @@ -255,10 +262,11 @@ static void resetConf(void) masterConfig.gyro_cmpf_factor = 600; // default MWC masterConfig.gyro_cmpfm_factor = 250; // default MWC masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead + resetAccelerometerTrims(&masterConfig.accZero); - masterConfig.gyro_align = ALIGN_DEFAULT; - masterConfig.acc_align = ALIGN_DEFAULT; - masterConfig.mag_align = ALIGN_DEFAULT; + + resetSensorAlignment(&masterConfig.sensorAlignmentConfig); + masterConfig.boardAlignment.rollDegrees = 0; masterConfig.boardAlignment.pitchDegrees = 0; masterConfig.boardAlignment.yawDegrees = 0; diff --git a/src/config_master.h b/src/config_master.h index 0e488d5cf3..3c7e120a91 100644 --- a/src/config_master.h +++ b/src/config_master.h @@ -25,10 +25,10 @@ typedef struct master_t { uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) // global sensor-related stuff - sensor_align_e gyro_align; // gyro alignment - sensor_align_e acc_align; // acc alignment - sensor_align_e mag_align; // mag alignment + + sensorAlignmentConfig_t sensorAlignmentConfig; boardAlignment_t boardAlignment; + int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. diff --git a/src/main.c b/src/main.c index c8e28a4d9a..f5dd942a44 100755 --- a/src/main.c +++ b/src/main.c @@ -55,7 +55,7 @@ void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe); void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe); void buzzerInit(failsafe_t *initialFailsafe); void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); -void sensorsAutodetect(void); +void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig); void imuInit(void); void loop(void); @@ -160,7 +160,7 @@ int main(void) LED1_OFF; // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. - sensorsAutodetect(); + sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile.mag_declination); imuInit(); // Mag is initialized inside imuInit // Check battery type/voltage diff --git a/src/sensors_common.h b/src/sensors_common.h index 64287cf176..1ffac43864 100644 --- a/src/sensors_common.h +++ b/src/sensors_common.h @@ -26,4 +26,10 @@ typedef enum { CW270_DEG_FLIP = 8 } sensor_align_e; +typedef struct sensorAlignmentConfig_s { + sensor_align_e gyro_align; // gyro alignment + sensor_align_e acc_align; // acc alignment + sensor_align_e mag_align; // mag alignment +} sensorAlignmentConfig_t; + extern int16_t heading; diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c index e0e09c2529..efc01564f6 100755 --- a/src/sensors_initialisation.c +++ b/src/sensors_initialisation.c @@ -1,12 +1,39 @@ -#include "board.h" -#include "mw.h" +#include +#include +#include +#include "platform.h" +#include "common/axis.h" + +#include "drivers/accgyro_common.h" + +#ifdef FY90Q +#include "drivers/accgyro_fy90q.h" +#else +#include "drivers/accgyro_adxl345.h" +#include "drivers/accgyro_bma280.h" +#include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_mma845x.h" +#include "drivers/accgyro_mpu3050.h" +#include "drivers/accgyro_mpu6050.h" +#endif + +#include "drivers/barometer_common.h" +#include "drivers/barometer_bmp085.h" +#include "drivers/barometer_ms5611.h" +#include "drivers/compass_hmc5883l.h" +#include "drivers/sonar_hcsr04.h" +#include "drivers/system_common.h" + +#include "flight_common.h" +#include "runtime_config.h" + +#include "sensors_common.h" #include "sensors_acceleration.h" #include "sensors_barometer.h" #include "sensors_gyro.h" #include "sensors_compass.h" - -#include "sensors_common.h" +#include "sensors_sonar.h" extern uint16_t batteryWarningVoltage; extern uint8_t batteryCellCount; @@ -18,15 +45,14 @@ extern acc_t acc; #ifdef FY90Q // FY90Q analog gyro/acc -void sensorsAutodetect(void) +void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) { memset(&acc, sizeof(acc), 0); memset(&gyro, sizeof(gyro), 0); adcSensorInit(&acc, &gyro); } #else -// AfroFlight32 i2c sensors -void sensorsAutodetect(void) +void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) { int16_t deg, min; drv_adxl345_config_t acc_params; @@ -36,12 +62,12 @@ void sensorsAutodetect(void) memset(&gyro, sizeof(gyro), 0); // Autodetect gyro hardware. We have MPU3050 or MPU6050. - if (mpu6050Detect(&acc, &gyro, masterConfig.gyro_lpf)) { + if (mpu6050Detect(&acc, &gyro, gyroLpf)) { haveMpu6k = true; gyroAlign = CW0_DEG; // default NAZE alignment - } else if (l3g4200dDetect(&gyro, masterConfig.gyro_lpf)) { + } else if (l3g4200dDetect(&gyro, gyroLpf)) { gyroAlign = CW0_DEG; - } else if (mpu3050Detect(&gyro, masterConfig.gyro_lpf)) { + } else if (mpu3050Detect(&gyro, gyroLpf)) { gyroAlign = CW0_DEG; } else { // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. @@ -50,7 +76,7 @@ void sensorsAutodetect(void) // Accelerometer. Fuck it. Let user break shit. retry: - switch (masterConfig.acc_hardware) { + switch (accHardwareToUse) { case ACC_NONE: // disable ACC sensorsClear(SENSOR_ACC); break; @@ -62,15 +88,15 @@ retry: accHardware = ACC_ADXL345; accAlign = CW270_DEG; // default NAZE alignment } - if (masterConfig.acc_hardware == ACC_ADXL345) + if (accHardwareToUse == ACC_ADXL345) break; ; // fallthrough case ACC_MPU6050: // MPU6050 if (haveMpu6k) { - mpu6050Detect(&acc, &gyro, masterConfig.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct + mpu6050Detect(&acc, &gyro, gyroLpf); // yes, i'm rerunning it again. re-fill acc struct accHardware = ACC_MPU6050; accAlign = CW0_DEG; // default NAZE alignment - if (masterConfig.acc_hardware == ACC_MPU6050) + if (accHardwareToUse == ACC_MPU6050) break; } ; // fallthrough @@ -79,7 +105,7 @@ retry: if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; accAlign = CW90_DEG; // default NAZE alignment - if (masterConfig.acc_hardware == ACC_MMA8452) + if (accHardwareToUse == ACC_MMA8452) break; } ; // fallthrough @@ -87,7 +113,7 @@ retry: if (bma280Detect(&acc)) { accHardware = ACC_BMA280; accAlign = CW0_DEG; // - if (masterConfig.acc_hardware == ACC_BMA280) + if (accHardwareToUse == ACC_BMA280) break; } #endif @@ -95,9 +121,9 @@ retry: // Found anything? Check if user fucked up or ACC is really missing. if (accHardware == ACC_DEFAULT) { - if (masterConfig.acc_hardware > ACC_DEFAULT) { + if (accHardwareToUse > ACC_DEFAULT) { // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. - masterConfig.acc_hardware = ACC_DEFAULT; + accHardwareToUse = ACC_DEFAULT; goto retry; } else { // We're really screwed @@ -116,14 +142,15 @@ retry: } #endif - if (masterConfig.gyro_align != ALIGN_DEFAULT) { - gyroAlign = masterConfig.gyro_align; + + if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { + gyroAlign = sensorAlignmentConfig->gyro_align; } - if (masterConfig.acc_align != ALIGN_DEFAULT) { - accAlign = masterConfig.acc_align; + if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) { + accAlign = sensorAlignmentConfig->acc_align; } - if (masterConfig.mag_align != ALIGN_DEFAULT) { - magAlign = masterConfig.mag_align; + if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) { + magAlign = sensorAlignmentConfig->mag_align; } @@ -141,10 +168,11 @@ retry: } #endif + // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c if (sensors(SENSOR_MAG)) { - // calculate magnetic declination - deg = currentProfile.mag_declination / 100; - min = currentProfile.mag_declination % 100; + // calculate magnetic declination + deg = magDeclinationFromConfig / 100; + min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units } else { diff --git a/src/serial_cli.c b/src/serial_cli.c index c34d0d779c..811e8004b3 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -154,9 +154,9 @@ const clivalue_t valueTable[] = { { "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 }, { "vbatmincellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 }, { "power_adc_channel", VAR_UINT8, &masterConfig.power_adc_channel, 0, 9 }, - { "align_gyro", VAR_UINT8, &masterConfig.gyro_align, 0, 8 }, - { "align_acc", VAR_UINT8, &masterConfig.acc_align, 0, 8 }, - { "align_mag", VAR_UINT8, &masterConfig.mag_align, 0, 8 }, + { "align_gyro", VAR_UINT8, &masterConfig.sensorAlignmentConfig.gyro_align, 0, 8 }, + { "align_acc", VAR_UINT8, &masterConfig.sensorAlignmentConfig.acc_align, 0, 8 }, + { "align_mag", VAR_UINT8, &masterConfig.sensorAlignmentConfig.mag_align, 0, 8 }, { "align_board_roll", VAR_INT16, &masterConfig.boardAlignment.rollDegrees, -180, 360 }, { "align_board_pitch", VAR_INT16, &masterConfig.boardAlignment.pitchDegrees, -180, 360 }, { "align_board_yaw", VAR_INT16, &masterConfig.boardAlignment.yawDegrees, -180, 360 },