From 4bb6820c42cca5d60977365a6d7d23ebf668db0d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 5 Dec 2016 15:26:14 +0000 Subject: [PATCH 01/13] Moved sensor detection into respective sensor modules --- src/main/drivers/barometer_bmp085.h | 2 + src/main/sensors/acceleration.c | 175 ++++++++++ src/main/sensors/acceleration.h | 1 + src/main/sensors/barometer.c | 81 ++++- src/main/sensors/barometer.h | 3 +- src/main/sensors/compass.c | 107 +++++- src/main/sensors/compass.h | 3 +- src/main/sensors/gyro.c | 160 +++++++++ src/main/sensors/gyro.h | 1 + src/main/sensors/initialisation.c | 510 +--------------------------- 10 files changed, 535 insertions(+), 508 deletions(-) diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index f6c6dece76..02fc0d8eb3 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -17,6 +17,8 @@ #pragma once +#include "io_types.h" + typedef struct bmp085Config_s { ioTag_t xclrIO; ioTag_t eocIO; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 13484e40c7..36b8c47d16 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -26,9 +26,27 @@ #include "common/axis.h" #include "common/filter.h" +#include "drivers/accgyro.h" +#include "drivers/accgyro_adxl345.h" +#include "drivers/accgyro_bma280.h" +#include "drivers/accgyro_fake.h" +#include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_mma845x.h" +#include "drivers/accgyro_mpu.h" +#include "drivers/accgyro_mpu3050.h" +#include "drivers/accgyro_mpu6050.h" +#include "drivers/accgyro_mpu6500.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" +#include "drivers/bus_spi.h" +#include "drivers/accgyro_spi_icm20689.h" +#include "drivers/accgyro_spi_mpu6000.h" +#include "drivers/accgyro_spi_mpu6500.h" +#include "drivers/accgyro_spi_mpu9250.h" #include "drivers/system.h" #include "fc/config.h" +#include "fc/runtime_config.h" #include "io/beeper.h" @@ -38,6 +56,10 @@ #include "config/feature.h" +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif + acc_t acc; // acc access functions @@ -53,8 +75,161 @@ static flightDynamicsTrims_t *accelerationTrims; static uint16_t accLpfCutHz = 0; static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; +bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) +{ + accelerationSensor_e accHardware; + +#ifdef USE_ACC_ADXL345 + drv_adxl345_config_t acc_params; +#endif + +retry: + acc.dev.accAlign = ALIGN_DEFAULT; + + switch (accHardwareToUse) { + case ACC_DEFAULT: + ; // fallthrough + case ACC_ADXL345: // ADXL345 +#ifdef USE_ACC_ADXL345 + acc_params.useFifo = false; + acc_params.dataRate = 800; // unused currently +#ifdef NAZE + if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, dev)) { +#else + if (adxl345Detect(&acc_params, dev)) { +#endif +#ifdef ACC_ADXL345_ALIGN + acc.dev.accAlign = ACC_ADXL345_ALIGN; +#endif + accHardware = ACC_ADXL345; + break; + } +#endif + ; // fallthrough + case ACC_LSM303DLHC: +#ifdef USE_ACC_LSM303DLHC + if (lsm303dlhcAccDetect(dev)) { +#ifdef ACC_LSM303DLHC_ALIGN + acc.dev.accAlign = ACC_LSM303DLHC_ALIGN; +#endif + accHardware = ACC_LSM303DLHC; + break; + } +#endif + ; // fallthrough + case ACC_MPU6050: // MPU6050 +#ifdef USE_ACC_MPU6050 + if (mpu6050AccDetect(dev)) { +#ifdef ACC_MPU6050_ALIGN + acc.dev.accAlign = ACC_MPU6050_ALIGN; +#endif + accHardware = ACC_MPU6050; + break; + } +#endif + ; // fallthrough + case ACC_MMA8452: // MMA8452 +#ifdef USE_ACC_MMA8452 +#ifdef NAZE + // Not supported with this frequency + if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) { +#else + if (mma8452Detect(dev)) { +#endif +#ifdef ACC_MMA8452_ALIGN + acc.dev.accAlign = ACC_MMA8452_ALIGN; +#endif + accHardware = ACC_MMA8452; + break; + } +#endif + ; // fallthrough + case ACC_BMA280: // BMA280 +#ifdef USE_ACC_BMA280 + if (bma280Detect(dev)) { +#ifdef ACC_BMA280_ALIGN + acc.dev.accAlign = ACC_BMA280_ALIGN; +#endif + accHardware = ACC_BMA280; + break; + } +#endif + ; // fallthrough + case ACC_MPU6000: +#ifdef USE_ACC_SPI_MPU6000 + if (mpu6000SpiAccDetect(dev)) { +#ifdef ACC_MPU6000_ALIGN + acc.dev.accAlign = ACC_MPU6000_ALIGN; +#endif + accHardware = ACC_MPU6000; + break; + } +#endif + ; // fallthrough + case ACC_MPU6500: +#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) +#ifdef USE_ACC_SPI_MPU6500 + if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) +#else + if (mpu6500AccDetect(dev)) +#endif + { +#ifdef ACC_MPU6500_ALIGN + acc.dev.accAlign = ACC_MPU6500_ALIGN; +#endif + accHardware = ACC_MPU6500; + break; + } +#endif + ; // fallthrough + case ACC_ICM20689: +#ifdef USE_ACC_SPI_ICM20689 + + if (icm20689SpiAccDetect(dev)) + { +#ifdef ACC_ICM20689_ALIGN + acc.dev.accAlign = ACC_ICM20689_ALIGN; +#endif + accHardware = ACC_ICM20689; + break; + } +#endif + ; // fallthrough + case ACC_FAKE: +#ifdef USE_FAKE_ACC + if (fakeAccDetect(dev)) { + accHardware = ACC_FAKE; + 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_NONE && 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 (accHardware == ACC_NONE) { + return false; + } + + detectedSensors[SENSOR_INDEX_ACC] = accHardware; + sensorsSet(SENSOR_ACC); + return true; +} + void accInit(uint32_t gyroSamplingInverval) { + acc.dev.acc_1G = 256; // set default + acc.dev.init(&acc.dev); // driver initialisation // set the acc sampling interval according to the gyro sampling interval switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future case 500: diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 88f391a5c8..1a4b05b3f3 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -61,6 +61,7 @@ typedef struct accelerometerConfig_s { flightDynamicsTrims_t accZero; } accelerometerConfig_t; +bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse); void accInit(uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 7534efaf85..bac86ad16c 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -24,9 +24,20 @@ #include "common/maths.h" #include "drivers/barometer.h" +#include "drivers/barometer_bmp085.h" +#include "drivers/barometer_bmp280.h" +#include "drivers/barometer_fake.h" +#include "drivers/barometer_ms5611.h" #include "drivers/system.h" +#include "fc/runtime_config.h" + #include "sensors/barometer.h" +#include "sensors/sensors.h" + +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif baro_t baro; // barometer access functions @@ -40,9 +51,75 @@ static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; -static barometerConfig_t *barometerConfig; +static const barometerConfig_t *barometerConfig; -void useBarometerConfig(barometerConfig_t *barometerConfigToUse) +bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) +{ + // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function + + baroSensor_e baroHardware = baroHardwareToUse; + +#ifdef USE_BARO_BMP085 + const bmp085Config_t *bmp085Config = NULL; + +#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) + static const bmp085Config_t defaultBMP085Config = { + .xclrIO = IO_TAG(BARO_XCLR_PIN), + .eocIO = IO_TAG(BARO_EOC_PIN), + }; + bmp085Config = &defaultBMP085Config; +#endif + +#ifdef NAZE + if (hardwareRevision == NAZE32) { + bmp085Disable(bmp085Config); + } +#endif + +#endif + + switch (baroHardware) { + case BARO_DEFAULT: + ; // fallthough + case BARO_BMP085: +#ifdef USE_BARO_BMP085 + if (bmp085Detect(bmp085Config, dev)) { + baroHardware = BARO_BMP085; + break; + } +#endif + ; // fallthough + case BARO_MS5611: +#ifdef USE_BARO_MS5611 + if (ms5611Detect(dev)) { + baroHardware = BARO_MS5611; + break; + } +#endif + ; // fallthough + case BARO_BMP280: +#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) + if (bmp280Detect(dev)) { + baroHardware = BARO_BMP280; + break; + } +#endif + ; // fallthough + case BARO_NONE: + baroHardware = BARO_NONE; + break; + } + + if (baroHardware == BARO_NONE) { + return false; + } + + detectedSensors[SENSOR_INDEX_BARO] = baroHardware; + sensorsSet(SENSOR_BARO); + return true; +} + +void useBarometerConfig(const barometerConfig_t *barometerConfigToUse) { barometerConfig = barometerConfigToUse; } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 53025fc4cd..536e6c453a 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -45,7 +45,8 @@ typedef struct baro_s { extern baro_t baro; -void useBarometerConfig(barometerConfig_t *barometerConfigToUse); +bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse); +void useBarometerConfig(const barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); uint32_t baroUpdate(void); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 050baf7b0b..b0ee1a098f 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -22,6 +22,12 @@ #include "common/axis.h" +#include "drivers/compass.h" +#include "drivers/compass_ak8975.h" +#include "drivers/compass_ak8963.h" +#include "drivers/compass_fake.h" +#include "drivers/compass_hmc5883l.h" +#include "drivers/io.h" #include "drivers/light_led.h" #include "fc/config.h" @@ -42,9 +48,108 @@ mag_t mag; // mag access functions static int16_t magADCRaw[XYZ_AXIS_COUNT]; static uint8_t magInit = 0; -void compassInit(void) +bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) +{ + magSensor_e magHardware; + +#ifdef USE_MAG_HMC5883 + const hmc5883Config_t *hmc5883Config = 0; + +#ifdef NAZE // TODO remove this target specific define + static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { + .intTag = IO_TAG(PB12) /* perhaps disabled? */ + }; + static const hmc5883Config_t nazeHmc5883Config_v5 = { + .intTag = IO_TAG(MAG_INT_EXTI) + }; + if (hardwareRevision < NAZE32_REV5) { + hmc5883Config = &nazeHmc5883Config_v1_v4; + } else { + hmc5883Config = &nazeHmc5883Config_v5; + } +#endif + +#ifdef MAG_INT_EXTI + static const hmc5883Config_t extiHmc5883Config = { + .intTag = IO_TAG(MAG_INT_EXTI) + }; + + hmc5883Config = &extiHmc5883Config; +#endif + +#endif + +retry: + + mag.dev.magAlign = ALIGN_DEFAULT; + + switch(magHardwareToUse) { + case MAG_DEFAULT: + ; // fallthrough + + case MAG_HMC5883: +#ifdef USE_MAG_HMC5883 + if (hmc5883lDetect(dev, hmc5883Config)) { +#ifdef MAG_HMC5883_ALIGN + mag.dev.magAlign = MAG_HMC5883_ALIGN; +#endif + magHardware = MAG_HMC5883; + break; + } +#endif + ; // fallthrough + + case MAG_AK8975: +#ifdef USE_MAG_AK8975 + if (ak8975Detect(dev)) { +#ifdef MAG_AK8975_ALIGN + mag.dev.magAlign = MAG_AK8975_ALIGN; +#endif + magHardware = MAG_AK8975; + break; + } +#endif + ; // fallthrough + + case MAG_AK8963: +#ifdef USE_MAG_AK8963 + if (ak8963Detect(dev)) { +#ifdef MAG_AK8963_ALIGN + mag.dev.magAlign = MAG_AK8963_ALIGN; +#endif + magHardware = MAG_AK8963; + break; + } +#endif + ; // fallthrough + + case MAG_NONE: + magHardware = MAG_NONE; + break; + } + + if (magHardware == MAG_NONE && 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 (magHardware == MAG_NONE) { + return false; + } + + detectedSensors[SENSOR_INDEX_MAG] = magHardware; + sensorsSet(SENSOR_MAG); + return true; +} + +void compassInit(const compassConfig_t *compassConfig) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) + // calculate magnetic declination + const int16_t deg = compassConfig->mag_declination / 100; + const int16_t min = compassConfig->mag_declination % 100; + mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units LED1_ON; mag.dev.init(); LED1_OFF; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 08ce80b300..4e534fea08 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -47,7 +47,8 @@ typedef struct compassConfig_s { flightDynamicsTrims_t magZero; } compassConfig_t; -void compassInit(void); +bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); +void compassInit(const compassConfig_t *compassConfig); union flightDynamicsTrims_u; void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e6a0a4d7d3..f99ca3cf5f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -27,8 +27,28 @@ #include "common/maths.h" #include "common/filter.h" +#include "drivers/accgyro.h" +#include "drivers/accgyro_adxl345.h" +#include "drivers/accgyro_bma280.h" +#include "drivers/accgyro_fake.h" +#include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_mma845x.h" +#include "drivers/accgyro_mpu.h" +#include "drivers/accgyro_mpu3050.h" +#include "drivers/accgyro_mpu6050.h" +#include "drivers/accgyro_mpu6500.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" +#include "drivers/bus_spi.h" +#include "drivers/accgyro_spi_icm20689.h" +#include "drivers/accgyro_spi_mpu6000.h" +#include "drivers/accgyro_spi_mpu6500.h" +#include "drivers/accgyro_spi_mpu9250.h" +#include "drivers/gyro_sync.h" #include "drivers/system.h" +#include "fc/runtime_config.h" + #include "io/beeper.h" #include "io/statusindicator.h" @@ -51,6 +71,143 @@ static void *notchFilter1[3]; static filterApplyFnPtr notchFilter2ApplyFn; static void *notchFilter2[3]; +bool gyroDetect(gyroDev_t *dev) +{ + gyroSensor_e gyroHardware = GYRO_DEFAULT; + + gyro.dev.gyroAlign = ALIGN_DEFAULT; + + switch(gyroHardware) { + case GYRO_DEFAULT: + ; // fallthrough + case GYRO_MPU6050: +#ifdef USE_GYRO_MPU6050 + if (mpu6050GyroDetect(dev)) { + gyroHardware = GYRO_MPU6050; +#ifdef GYRO_MPU6050_ALIGN + gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + case GYRO_L3G4200D: +#ifdef USE_GYRO_L3G4200D + if (l3g4200dDetect(dev)) { + gyroHardware = GYRO_L3G4200D; +#ifdef GYRO_L3G4200D_ALIGN + gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_MPU3050: +#ifdef USE_GYRO_MPU3050 + if (mpu3050Detect(dev)) { + gyroHardware = GYRO_MPU3050; +#ifdef GYRO_MPU3050_ALIGN + gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_L3GD20: +#ifdef USE_GYRO_L3GD20 + if (l3gd20Detect(dev)) { + gyroHardware = GYRO_L3GD20; +#ifdef GYRO_L3GD20_ALIGN + gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_MPU6000: +#ifdef USE_GYRO_SPI_MPU6000 + if (mpu6000SpiGyroDetect(dev)) { + gyroHardware = GYRO_MPU6000; +#ifdef GYRO_MPU6000_ALIGN + gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_MPU6500: +#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) +#ifdef USE_GYRO_SPI_MPU6500 + if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) +#else + if (mpu6500GyroDetect(dev)) +#endif + { + gyroHardware = GYRO_MPU6500; +#ifdef GYRO_MPU6500_ALIGN + gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough + +case GYRO_MPU9250: +#ifdef USE_GYRO_SPI_MPU9250 + + if (mpu9250SpiGyroDetect(dev)) + { + gyroHardware = GYRO_MPU9250; +#ifdef GYRO_MPU9250_ALIGN + gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough + + case GYRO_ICM20689: +#ifdef USE_GYRO_SPI_ICM20689 + if (icm20689SpiGyroDetect(dev)) + { + gyroHardware = GYRO_ICM20689; +#ifdef GYRO_ICM20689_ALIGN + gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough + + case GYRO_FAKE: +#ifdef USE_FAKE_GYRO + if (fakeGyroDetect(dev)) { + gyroHardware = GYRO_FAKE; + break; + } +#endif + ; // fallthrough + case GYRO_NONE: + gyroHardware = GYRO_NONE; + } + + if (gyroHardware == GYRO_NONE) { + return false; + } + + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; + sensorsSet(SENSOR_GYRO); + + return true; +} + void gyroInit(const gyroConfig_t *gyroConfigToUse) { static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; @@ -59,6 +216,9 @@ void gyroInit(const gyroConfig_t *gyroConfigToUse) static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; + gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation + gyro.dev.lpf = gyroConfig->gyro_lpf; + gyro.dev.init(&gyro.dev); gyroConfig = gyroConfigToUse; softLpfFilterApplyFn = nullFilterApply; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index a532a016a7..5d45d917c6 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -55,6 +55,7 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_cutoff_2; } gyroConfig_t; +bool gyroDetect(gyroDev_t *dev); void gyroSetCalibrationCycles(void); void gyroInit(const gyroConfig_t *gyroConfigToUse); void gyroUpdate(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 9116c18374..4e43b50ba1 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -26,44 +26,14 @@ #include "config/feature.h" +#include "drivers/accgyro_mpu.h" #include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" - #include "drivers/sensor.h" - #include "drivers/accgyro.h" -#include "drivers/accgyro_adxl345.h" -#include "drivers/accgyro_bma280.h" -#include "drivers/accgyro_fake.h" -#include "drivers/accgyro_l3g4200d.h" -#include "drivers/accgyro_mma845x.h" -#include "drivers/accgyro_mpu.h" -#include "drivers/accgyro_mpu3050.h" -#include "drivers/accgyro_mpu6050.h" -#include "drivers/accgyro_mpu6500.h" -#include "drivers/accgyro_l3gd20.h" -#include "drivers/accgyro_lsm303dlhc.h" - -#include "drivers/bus_spi.h" -#include "drivers/accgyro_spi_icm20689.h" -#include "drivers/accgyro_spi_mpu6000.h" -#include "drivers/accgyro_spi_mpu6500.h" -#include "drivers/accgyro_spi_mpu9250.h" -#include "drivers/gyro_sync.h" - #include "drivers/barometer.h" -#include "drivers/barometer_bmp085.h" -#include "drivers/barometer_bmp280.h" -#include "drivers/barometer_fake.h" -#include "drivers/barometer_ms5611.h" - #include "drivers/compass.h" -#include "drivers/compass_ak8975.h" -#include "drivers/compass_ak8963.h" -#include "drivers/compass_fake.h" -#include "drivers/compass_hmc5883l.h" - #include "drivers/sonar_hcsr04.h" #include "fc/config.h" @@ -97,459 +67,6 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #endif } -bool gyroDetect(gyroDev_t *dev) -{ - gyroSensor_e gyroHardware = GYRO_DEFAULT; - - gyro.dev.gyroAlign = ALIGN_DEFAULT; - - switch(gyroHardware) { - case GYRO_DEFAULT: - ; // fallthrough - case GYRO_MPU6050: -#ifdef USE_GYRO_MPU6050 - if (mpu6050GyroDetect(dev)) { - gyroHardware = GYRO_MPU6050; -#ifdef GYRO_MPU6050_ALIGN - gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - case GYRO_L3G4200D: -#ifdef USE_GYRO_L3G4200D - if (l3g4200dDetect(dev)) { - gyroHardware = GYRO_L3G4200D; -#ifdef GYRO_L3G4200D_ALIGN - gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_MPU3050: -#ifdef USE_GYRO_MPU3050 - if (mpu3050Detect(dev)) { - gyroHardware = GYRO_MPU3050; -#ifdef GYRO_MPU3050_ALIGN - gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_L3GD20: -#ifdef USE_GYRO_L3GD20 - if (l3gd20Detect(dev)) { - gyroHardware = GYRO_L3GD20; -#ifdef GYRO_L3GD20_ALIGN - gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_MPU6000: -#ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiGyroDetect(dev)) { - gyroHardware = GYRO_MPU6000; -#ifdef GYRO_MPU6000_ALIGN - gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_MPU6500: -#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) -#ifdef USE_GYRO_SPI_MPU6500 - if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) -#else - if (mpu6500GyroDetect(dev)) -#endif - { - gyroHardware = GYRO_MPU6500; -#ifdef GYRO_MPU6500_ALIGN - gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN; -#endif - - break; - } -#endif - ; // fallthrough - - case GYRO_MPU9250: -#ifdef USE_GYRO_SPI_MPU9250 - - if (mpu9250SpiGyroDetect(dev)) - { - gyroHardware = GYRO_MPU9250; -#ifdef GYRO_MPU9250_ALIGN - gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN; -#endif - - break; - } -#endif - ; // fallthrough - - case GYRO_ICM20689: -#ifdef USE_GYRO_SPI_ICM20689 - if (icm20689SpiGyroDetect(dev)) - { - gyroHardware = GYRO_ICM20689; -#ifdef GYRO_ICM20689_ALIGN - gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN; -#endif - - break; - } -#endif - ; // fallthrough - - case GYRO_FAKE: -#ifdef USE_FAKE_GYRO - if (fakeGyroDetect(dev)) { - gyroHardware = GYRO_FAKE; - break; - } -#endif - ; // fallthrough - case GYRO_NONE: - gyroHardware = GYRO_NONE; - } - - if (gyroHardware == GYRO_NONE) { - return false; - } - - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; - sensorsSet(SENSOR_GYRO); - - return true; -} - -static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) -{ - accelerationSensor_e accHardware; - -#ifdef USE_ACC_ADXL345 - drv_adxl345_config_t acc_params; -#endif - -retry: - acc.dev.accAlign = ALIGN_DEFAULT; - - switch (accHardwareToUse) { - case ACC_DEFAULT: - ; // fallthrough - case ACC_ADXL345: // ADXL345 -#ifdef USE_ACC_ADXL345 - acc_params.useFifo = false; - acc_params.dataRate = 800; // unused currently -#ifdef NAZE - if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, dev)) { -#else - if (adxl345Detect(&acc_params, dev)) { -#endif -#ifdef ACC_ADXL345_ALIGN - acc.dev.accAlign = ACC_ADXL345_ALIGN; -#endif - accHardware = ACC_ADXL345; - break; - } -#endif - ; // fallthrough - case ACC_LSM303DLHC: -#ifdef USE_ACC_LSM303DLHC - if (lsm303dlhcAccDetect(dev)) { -#ifdef ACC_LSM303DLHC_ALIGN - acc.dev.accAlign = ACC_LSM303DLHC_ALIGN; -#endif - accHardware = ACC_LSM303DLHC; - break; - } -#endif - ; // fallthrough - case ACC_MPU6050: // MPU6050 -#ifdef USE_ACC_MPU6050 - if (mpu6050AccDetect(dev)) { -#ifdef ACC_MPU6050_ALIGN - acc.dev.accAlign = ACC_MPU6050_ALIGN; -#endif - accHardware = ACC_MPU6050; - break; - } -#endif - ; // fallthrough - case ACC_MMA8452: // MMA8452 -#ifdef USE_ACC_MMA8452 -#ifdef NAZE - // Not supported with this frequency - if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) { -#else - if (mma8452Detect(dev)) { -#endif -#ifdef ACC_MMA8452_ALIGN - acc.dev.accAlign = ACC_MMA8452_ALIGN; -#endif - accHardware = ACC_MMA8452; - break; - } -#endif - ; // fallthrough - case ACC_BMA280: // BMA280 -#ifdef USE_ACC_BMA280 - if (bma280Detect(dev)) { -#ifdef ACC_BMA280_ALIGN - acc.dev.accAlign = ACC_BMA280_ALIGN; -#endif - accHardware = ACC_BMA280; - break; - } -#endif - ; // fallthrough - case ACC_MPU6000: -#ifdef USE_ACC_SPI_MPU6000 - if (mpu6000SpiAccDetect(dev)) { -#ifdef ACC_MPU6000_ALIGN - acc.dev.accAlign = ACC_MPU6000_ALIGN; -#endif - accHardware = ACC_MPU6000; - break; - } -#endif - ; // fallthrough - case ACC_MPU6500: -#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) -#ifdef USE_ACC_SPI_MPU6500 - if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) -#else - if (mpu6500AccDetect(dev)) -#endif - { -#ifdef ACC_MPU6500_ALIGN - acc.dev.accAlign = ACC_MPU6500_ALIGN; -#endif - accHardware = ACC_MPU6500; - break; - } -#endif - ; // fallthrough - case ACC_ICM20689: -#ifdef USE_ACC_SPI_ICM20689 - - if (icm20689SpiAccDetect(dev)) - { -#ifdef ACC_ICM20689_ALIGN - acc.dev.accAlign = ACC_ICM20689_ALIGN; -#endif - accHardware = ACC_ICM20689; - break; - } -#endif - ; // fallthrough - case ACC_FAKE: -#ifdef USE_FAKE_ACC - if (fakeAccDetect(dev)) { - accHardware = ACC_FAKE; - 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_NONE && 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 (accHardware == ACC_NONE) { - return false; - } - - detectedSensors[SENSOR_INDEX_ACC] = accHardware; - sensorsSet(SENSOR_ACC); - return true; -} - -#ifdef BARO -static bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) -{ - // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function - - baroSensor_e baroHardware = baroHardwareToUse; - -#ifdef USE_BARO_BMP085 - const bmp085Config_t *bmp085Config = NULL; - -#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) - static const bmp085Config_t defaultBMP085Config = { - .xclrIO = IO_TAG(BARO_XCLR_PIN), - .eocIO = IO_TAG(BARO_EOC_PIN), - }; - bmp085Config = &defaultBMP085Config; -#endif - -#ifdef NAZE - if (hardwareRevision == NAZE32) { - bmp085Disable(bmp085Config); - } -#endif - -#endif - - switch (baroHardware) { - case BARO_DEFAULT: - ; // fallthough - case BARO_BMP085: -#ifdef USE_BARO_BMP085 - if (bmp085Detect(bmp085Config, dev)) { - baroHardware = BARO_BMP085; - break; - } -#endif - ; // fallthough - case BARO_MS5611: -#ifdef USE_BARO_MS5611 - if (ms5611Detect(dev)) { - baroHardware = BARO_MS5611; - break; - } -#endif - ; // fallthough - case BARO_BMP280: -#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) - if (bmp280Detect(dev)) { - baroHardware = BARO_BMP280; - break; - } -#endif - ; // fallthough - case BARO_NONE: - baroHardware = BARO_NONE; - break; - } - - if (baroHardware == BARO_NONE) { - return false; - } - - detectedSensors[SENSOR_INDEX_BARO] = baroHardware; - sensorsSet(SENSOR_BARO); - return true; -} -#endif - -#ifdef MAG -static bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) -{ - magSensor_e magHardware; - -#ifdef USE_MAG_HMC5883 - const hmc5883Config_t *hmc5883Config = 0; - -#ifdef NAZE // TODO remove this target specific define - static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { - .intTag = IO_TAG(PB12) /* perhaps disabled? */ - }; - static const hmc5883Config_t nazeHmc5883Config_v5 = { - .intTag = IO_TAG(MAG_INT_EXTI) - }; - if (hardwareRevision < NAZE32_REV5) { - hmc5883Config = &nazeHmc5883Config_v1_v4; - } else { - hmc5883Config = &nazeHmc5883Config_v5; - } -#endif - -#ifdef MAG_INT_EXTI - static const hmc5883Config_t extiHmc5883Config = { - .intTag = IO_TAG(MAG_INT_EXTI) - }; - - hmc5883Config = &extiHmc5883Config; -#endif - -#endif - -retry: - - mag.dev.magAlign = ALIGN_DEFAULT; - - switch(magHardwareToUse) { - case MAG_DEFAULT: - ; // fallthrough - - case MAG_HMC5883: -#ifdef USE_MAG_HMC5883 - if (hmc5883lDetect(dev, hmc5883Config)) { -#ifdef MAG_HMC5883_ALIGN - mag.dev.magAlign = MAG_HMC5883_ALIGN; -#endif - magHardware = MAG_HMC5883; - break; - } -#endif - ; // fallthrough - - case MAG_AK8975: -#ifdef USE_MAG_AK8975 - if (ak8975Detect(dev)) { -#ifdef MAG_AK8975_ALIGN - mag.dev.magAlign = MAG_AK8975_ALIGN; -#endif - magHardware = MAG_AK8975; - break; - } -#endif - ; // fallthrough - - case MAG_AK8963: -#ifdef USE_MAG_AK8963 - if (ak8963Detect(dev)) { -#ifdef MAG_AK8963_ALIGN - mag.dev.magAlign = MAG_AK8963_ALIGN; -#endif - magHardware = MAG_AK8963; - break; - } -#endif - ; // fallthrough - - case MAG_NONE: - magHardware = MAG_NONE; - break; - } - - if (magHardware == MAG_NONE && 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 (magHardware == MAG_NONE) { - return false; - } - - detectedSensors[SENSOR_INDEX_MAG] = magHardware; - sensorsSet(SENSOR_MAG); - return true; -} -#endif - #ifdef SONAR static bool sonarDetect(void) { @@ -569,8 +86,6 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, const barometerConfig_t *barometerConfig, const sonarConfig_t *sonarConfig) { - memset(&acc, 0, sizeof(acc)); - memset(&gyro, 0, sizeof(gyro)); #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) @@ -580,33 +95,22 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, UNUSED(mpuDetectionResult); #endif + memset(&gyro, 0, sizeof(gyro)); if (!gyroDetect(&gyro.dev)) { return false; } + // gyro must be initialised before accelerometer + gyroInit(gyroConfig); - // Now time to init things - // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation - gyro.dev.lpf = gyroConfig->gyro_lpf; - gyro.dev.init(&gyro.dev); // driver initialisation - gyroInit(gyroConfig); // sensor initialisation - + memset(&acc, 0, sizeof(acc)); if (accDetect(&acc.dev, accelerometerConfig->acc_hardware)) { - acc.dev.acc_1G = 256; // set default - acc.dev.init(&acc.dev); // driver initialisation - accInit(gyro.targetLooptime); // sensor initialisation + accInit(gyro.targetLooptime); } - mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG - // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c if (compassDetect(&mag.dev, compassConfig->mag_hardware)) { - // calculate magnetic declination - const int16_t deg = compassConfig->mag_declination / 100; - const int16_t min = compassConfig->mag_declination % 100; - mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units - compassInit(); + compassInit(compassConfig); } #else UNUSED(compassConfig); From 6ea054dcf3c8c6a4dd77472c0d730a166dd8ee45 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 2 Dec 2016 21:50:17 +1300 Subject: [PATCH 02/13] Optimised size for non realtime targets. --- Makefile | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index f2edc40d58..ec0f397392 100644 --- a/Makefile +++ b/Makefile @@ -557,6 +557,12 @@ COMMON_SRC = \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) +NON_RT_SRC = \ + io/serial_cli.c \ + io/serial_4way.c \ + io/serial_4way_avrootloader.c \ + io/serial_4way_stk500v2.c + HIGHEND_SRC = \ blackbox/blackbox.c \ blackbox/blackbox_io.c \ @@ -762,13 +768,14 @@ OPTIMIZE = -Os else OPTIMIZE = -Ofast endif -LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) +LTO_BASE = -flto -fuse-linker-plugin +LTO_FLAGS = $(LTO_BASE) $(OPTIMIZE) +NON_RT_LTO_FLAGS = $(LTO_BASE) -Os endif DEBUG_FLAGS = -ggdb3 -DDEBUG CFLAGS += $(ARCH_FLAGS) \ - $(LTO_FLAGS) \ $(addprefix -D,$(OPTIONS)) \ $(addprefix -I,$(INCLUDE_DIRS)) \ $(DEBUG_FLAGS) \ @@ -848,8 +855,11 @@ $(TARGET_ELF): $(TARGET_OBJS) # Compile $(OBJECT_DIR)/$(TARGET)/%.o: %.c $(V1) mkdir -p $(dir $@) - $(V1) echo "%% $(notdir $<)" "$(STDOUT)" - $(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $< + $(V1) $(if $(findstring $(subst ./src/main/,,$<), $(NON_RT_SRC)), \ + echo "%% (unoptimised) $(notdir $<)" "$(STDOUT)" && \ + $(CROSS_CC) -c -o $@ $(CFLAGS) $(NON_RT_LTO_FLAGS) $<, \ + echo "%% $(notdir $<)" "$(STDOUT)" && \ + $(CROSS_CC) -c -o $@ $(CFLAGS) $(LTO_FLAGS) $<) # Assemble $(OBJECT_DIR)/$(TARGET)/%.o: %.s From 43b41d054e6c7d99f99d3e7db00a660b9a8ba8cc Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 5 Dec 2016 09:06:01 +1300 Subject: [PATCH 03/13] Added '-ffast-math' to space optimised targets, to eliminate linker warnings and further reduce size. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index ec0f397392..f12e502ec4 100644 --- a/Makefile +++ b/Makefile @@ -770,7 +770,7 @@ OPTIMIZE = -Ofast endif LTO_BASE = -flto -fuse-linker-plugin LTO_FLAGS = $(LTO_BASE) $(OPTIMIZE) -NON_RT_LTO_FLAGS = $(LTO_BASE) -Os +NON_RT_LTO_FLAGS = $(LTO_BASE) -Os -ffast-math endif DEBUG_FLAGS = -ggdb3 -DDEBUG From 9f7bae899f65ef38d82d66c57db29ac3d53d8245 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 5 Dec 2016 10:34:51 +1300 Subject: [PATCH 04/13] Added 'low priority' target list. --- Makefile | 74 ++++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 58 insertions(+), 16 deletions(-) diff --git a/Makefile b/Makefile index f12e502ec4..b5b854f0a3 100644 --- a/Makefile +++ b/Makefile @@ -557,12 +557,6 @@ COMMON_SRC = \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) -NON_RT_SRC = \ - io/serial_cli.c \ - io/serial_4way.c \ - io/serial_4way_avrootloader.c \ - io/serial_4way_stk500v2.c - HIGHEND_SRC = \ blackbox/blackbox.c \ blackbox/blackbox_io.c \ @@ -601,6 +595,44 @@ HIGHEND_SRC = \ telemetry/mavlink.c \ telemetry/esc_telemetry.c \ +NON_RT_SRC = \ + drivers/serial_escserial.c \ + io/serial_cli.c \ + io/serial_4way.c \ + io/serial_4way_avrootloader.c \ + io/serial_4way_stk500v2.c \ + msp/msp_serial.c \ + +LOW_PRIO_SRC = \ + msp/msp_serial.c \ + cms/cms.c \ + cms/cms_menu_blackbox.c \ + cms/cms_menu_builtin.c \ + cms/cms_menu_imu.c \ + cms/cms_menu_ledstrip.c \ + cms/cms_menu_misc.c \ + cms/cms_menu_osd.c \ + cms/cms_menu_vtx.c \ + flight/gtune.c \ + flight/gps_conversion.c \ + io/dashboard.c \ + io/displayport_max7456.c \ + io/displayport_msp.c \ + io/displayport_oled.c \ + io/gps.c \ + io/ledstrip.c \ + io/osd.c \ + sensors/sonar.c \ + sensors/barometer.c \ + telemetry/telemetry.c \ + telemetry/crsf.c \ + telemetry/frsky.c \ + telemetry/hott.c \ + telemetry/smartport.c \ + telemetry/ltm.c \ + telemetry/mavlink.c \ + telemetry/esc_telemetry.c \ + ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) VCP_SRC = \ vcpf4/stm32f4xx_it.c \ @@ -760,17 +792,24 @@ SIZE := $(ARM_SDK_PREFIX)size # ifeq ($(DEBUG),GDB) -OPTIMIZE = -O0 -LTO_FLAGS = $(OPTIMIZE) +OPTIMIZE = -O0 +CC_OPTIMISATION = $(OPTIMISE) +LOW_PRIO_CC_OPTIMISATION = $(OPTIMISE) +NON_RT_CC_OPTIMISATION = $(OPTIMISE) +LTO_FLAGS = $(OPTIMIZE) else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) -OPTIMIZE = -Os +OPTIMISE = -Os +OPTIMISE_LOW_PRIO = -Os else -OPTIMIZE = -Ofast +OPTIMISE = -Ofast +OPTIMISE_LOW_PRIO = -O2 endif -LTO_BASE = -flto -fuse-linker-plugin -LTO_FLAGS = $(LTO_BASE) $(OPTIMIZE) -NON_RT_LTO_FLAGS = $(LTO_BASE) -Os -ffast-math +OPTIMISATION_BASE = -flto -fuse-linker-plugin -ffast-math +CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE) +LOW_PRIO_CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_LOW_PRIO) +NON_RT_CC_OPTIMISATION = $(OPTIMISATION_BASE) -Os +LTO_FLAGS = $(OPTIMISATION_BASE) $(OPTIMIZE) endif DEBUG_FLAGS = -ggdb3 -DDEBUG @@ -856,10 +895,13 @@ $(TARGET_ELF): $(TARGET_OBJS) $(OBJECT_DIR)/$(TARGET)/%.o: %.c $(V1) mkdir -p $(dir $@) $(V1) $(if $(findstring $(subst ./src/main/,,$<), $(NON_RT_SRC)), \ - echo "%% (unoptimised) $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(NON_RT_LTO_FLAGS) $<, \ + echo "%% (non-realtime) $(notdir $<)" "$(STDOUT)" && \ + $(CROSS_CC) -c -o $@ $(CFLAGS) $(NON_RT_CC_OPTIMISATION) $<, \ + $(if $(findstring $(subst ./src/main/,,$<), $(LOW_PRIO_SRC)), \ + echo "%% (low priority) $(notdir $<)" "$(STDOUT)" && \ + $(CROSS_CC) -c -o $@ $(CFLAGS) $(LOW_PRIO_CC_OPTIMISATION) $<, \ echo "%% $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(LTO_FLAGS) $<) + $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_OPTIMISATION) $<)) # Assemble $(OBJECT_DIR)/$(TARGET)/%.o: %.s From 9caa8b6cbaa8c9eb72ed7f613eb3bcf905ad5c7d Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 5 Dec 2016 10:56:28 +1300 Subject: [PATCH 05/13] Disabled size optimisation for F4. --- Makefile | 22 ++++++++-------------- src/main/target/SPRACINGF3EVO/target.h | 4 ++-- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/Makefile b/Makefile index b5b854f0a3..35111f7b8e 100644 --- a/Makefile +++ b/Makefile @@ -615,23 +615,11 @@ LOW_PRIO_SRC = \ cms/cms_menu_vtx.c \ flight/gtune.c \ flight/gps_conversion.c \ - io/dashboard.c \ - io/displayport_max7456.c \ - io/displayport_msp.c \ - io/displayport_oled.c \ io/gps.c \ io/ledstrip.c \ io/osd.c \ sensors/sonar.c \ sensors/barometer.c \ - telemetry/telemetry.c \ - telemetry/crsf.c \ - telemetry/frsky.c \ - telemetry/hott.c \ - telemetry/smartport.c \ - telemetry/ltm.c \ - telemetry/mavlink.c \ - telemetry/esc_telemetry.c \ ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) VCP_SRC = \ @@ -801,14 +789,20 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) OPTIMISE = -Os OPTIMISE_LOW_PRIO = -Os -else +OPTIMISE_NON_RT = -Os +else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) OPTIMISE = -Ofast OPTIMISE_LOW_PRIO = -O2 +OPTIMISE_NON_RT = -Os +else +OPTIMISE = -Ofast +OPTIMISE_LOW_PRIO = -Ofast +OPTIMISE_NON_RT = -Ofast endif OPTIMISATION_BASE = -flto -fuse-linker-plugin -ffast-math CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE) LOW_PRIO_CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_LOW_PRIO) -NON_RT_CC_OPTIMISATION = $(OPTIMISATION_BASE) -Os +NON_RT_CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_NON_RT) LTO_FLAGS = $(OPTIMISATION_BASE) $(OPTIMIZE) endif diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index ce7e3b3685..88e7d09f42 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -49,8 +49,8 @@ #define BARO #define USE_BARO_BMP280 -//#define MAG -//#define USE_MAG_AK8963 +#define MAG +#define USE_MAG_AK8963 //#define USE_MAG_HMC5883 // External #define MAG_AK8963_ALIGN CW90_DEG_FLIP From 5b674c8e4f02260e45bed5ac6830e377f0d785fd Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 6 Dec 2016 08:04:02 +1300 Subject: [PATCH 06/13] Switched to 'SPEED_OPTIMISED_SRC', 'SIZE_OPTIMISED_SRC'. Also fixed spelling. --- Makefile | 154 +++++++++++++++++++++++++++++++++++++++---------------- 1 file changed, 110 insertions(+), 44 deletions(-) diff --git a/Makefile b/Makefile index 35111f7b8e..d4b7c11860 100644 --- a/Makefile +++ b/Makefile @@ -595,7 +595,91 @@ HIGHEND_SRC = \ telemetry/mavlink.c \ telemetry/esc_telemetry.c \ -NON_RT_SRC = \ +SPEED_OPTIMISED_SRC = \ + common/encoding.c \ + common/filter.c \ + common/maths.c \ + common/typeconversion.c \ + drivers/adc.c \ + drivers/buf_writer.c \ + drivers/bus_i2c_soft.c \ + drivers/bus_spi.c \ + drivers/bus_spi_soft.c \ + drivers/exti.c \ + drivers/gyro_sync.c \ + drivers/io.c \ + drivers/light_led.c \ + drivers/resource.c \ + drivers/rx_nrf24l01.c \ + drivers/rx_spi.c \ + drivers/rx_xn297.c \ + drivers/pwm_output.c \ + drivers/pwm_rx.c \ + drivers/rcc.c \ + drivers/serial.c \ + drivers/serial_uart.c \ + drivers/sound_beeper.c \ + drivers/stack_check.c \ + drivers/system.c \ + drivers/timer.c \ + fc/fc_tasks.c \ + fc/mw.c \ + fc/rc_controls.c \ + fc/rc_curves.c \ + fc/runtime_config.c \ + flight/altitudehold.c \ + flight/failsafe.c \ + flight/imu.c \ + flight/mixer.c \ + flight/pid.c \ + flight/servos.c \ + io/beeper.c \ + io/serial.c \ + io/statusindicator.c \ + rx/ibus.c \ + rx/jetiexbus.c \ + rx/msp.c \ + rx/nrf24_cx10.c \ + rx/nrf24_inav.c \ + rx/nrf24_h8_3d.c \ + rx/nrf24_syma.c \ + rx/nrf24_v202.c \ + rx/pwm.c \ + rx/rx.c \ + rx/rx_spi.c \ + rx/crsf.c \ + rx/sbus.c \ + rx/spektrum.c \ + rx/sumd.c \ + rx/sumh.c \ + rx/xbus.c \ + scheduler/scheduler.c \ + sensors/acceleration.c \ + sensors/boardalignment.c \ + sensors/gyro.c \ + $(CMSIS_SRC) \ + $(DEVICE_STDPERIPH_SRC) \ + blackbox/blackbox.c \ + blackbox/blackbox_io.c \ + drivers/display_ug2864hsweg01.c \ + drivers/light_ws2811strip.c \ + drivers/serial_softserial.c \ + io/dashboard.c \ + io/displayport_max7456.c \ + io/displayport_msp.c \ + io/displayport_oled.c \ + io/ledstrip.c \ + io/osd.c \ + telemetry/telemetry.c \ + telemetry/crsf.c \ + telemetry/frsky.c \ + telemetry/hott.c \ + telemetry/smartport.c \ + telemetry/ltm.c \ + telemetry/mavlink.c \ + telemetry/esc_telemetry.c \ + +SIZE_OPTIMISED_SRC = \ drivers/serial_escserial.c \ io/serial_cli.c \ io/serial_4way.c \ @@ -603,24 +687,6 @@ NON_RT_SRC = \ io/serial_4way_stk500v2.c \ msp/msp_serial.c \ -LOW_PRIO_SRC = \ - msp/msp_serial.c \ - cms/cms.c \ - cms/cms_menu_blackbox.c \ - cms/cms_menu_builtin.c \ - cms/cms_menu_imu.c \ - cms/cms_menu_ledstrip.c \ - cms/cms_menu_misc.c \ - cms/cms_menu_osd.c \ - cms/cms_menu_vtx.c \ - flight/gtune.c \ - flight/gps_conversion.c \ - io/gps.c \ - io/ledstrip.c \ - io/osd.c \ - sensors/sonar.c \ - sensors/barometer.c \ - ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) VCP_SRC = \ vcpf4/stm32f4xx_it.c \ @@ -780,30 +846,30 @@ SIZE := $(ARM_SDK_PREFIX)size # ifeq ($(DEBUG),GDB) -OPTIMIZE = -O0 -CC_OPTIMISATION = $(OPTIMISE) -LOW_PRIO_CC_OPTIMISATION = $(OPTIMISE) -NON_RT_CC_OPTIMISATION = $(OPTIMISE) -LTO_FLAGS = $(OPTIMIZE) +OPTIMISE = -O0 +CC_SPEED_OPTIMISATION = $(OPTIMISE) +CC_OPTIMISATION = $(OPTIMISE) +CC_SIZE_OPTIMISATION = $(OPTIMISE) +LTO_FLAGS = $(OPTIMISE) else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) -OPTIMISE = -Os -OPTIMISE_LOW_PRIO = -Os -OPTIMISE_NON_RT = -Os +OPTIMISE_SPEED = -Os +OPTIMISE = -Os +OPTIMISE_SIZE = -Os else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) -OPTIMISE = -Ofast -OPTIMISE_LOW_PRIO = -O2 -OPTIMISE_NON_RT = -Os +OPTIMISE_SPEED = -Ofast +OPTIMISE = -O2 +OPTIMISE_SIZE = -Os else -OPTIMISE = -Ofast -OPTIMISE_LOW_PRIO = -Ofast -OPTIMISE_NON_RT = -Ofast +OPTIMISE_SPEED = -Ofast +OPTIMISE = -Ofast +OPTIMISE_SIZE = -Ofast endif -OPTIMISATION_BASE = -flto -fuse-linker-plugin -ffast-math -CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE) -LOW_PRIO_CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_LOW_PRIO) -NON_RT_CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_NON_RT) -LTO_FLAGS = $(OPTIMISATION_BASE) $(OPTIMIZE) +OPTIMISATION_BASE = -flto -fuse-linker-plugin -ffast-math +CC_SPEED_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) +CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE) +CC_SIZE_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_SIZE) +LTO_FLAGS = $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) endif DEBUG_FLAGS = -ggdb3 -DDEBUG @@ -888,12 +954,12 @@ $(TARGET_ELF): $(TARGET_OBJS) # Compile $(OBJECT_DIR)/$(TARGET)/%.o: %.c $(V1) mkdir -p $(dir $@) - $(V1) $(if $(findstring $(subst ./src/main/,,$<), $(NON_RT_SRC)), \ - echo "%% (non-realtime) $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(NON_RT_CC_OPTIMISATION) $<, \ - $(if $(findstring $(subst ./src/main/,,$<), $(LOW_PRIO_SRC)), \ - echo "%% (low priority) $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(LOW_PRIO_CC_OPTIMISATION) $<, \ + $(V1) $(if $(findstring $(subst ./src/main/,,$<), $(SPEED_OPTIMISED_SRC)), \ + echo "%% (speed optimised) $(notdir $<)" "$(STDOUT)" && \ + $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_SPEED_OPTIMISATION) $<, \ + $(if $(findstring $(subst ./src/main/,,$<), $(SIZE_OPTIMISED_SRC)), \ + echo "%% (size optimised) $(notdir $<)" "$(STDOUT)" && \ + $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_SIZE_OPTIMISATION) $<, \ echo "%% $(notdir $<)" "$(STDOUT)" && \ $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_OPTIMISATION) $<)) From a47c073874c15ddf411df7ff4e47a308da70a681 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 7 Dec 2016 09:16:42 +0000 Subject: [PATCH 07/13] Used device pointer in detect functions --- src/main/drivers/compass_ak8963.c | 3 +++ src/main/drivers/compass_ak8963.h | 2 -- src/main/drivers/compass_ak8975.c | 3 +++ src/main/drivers/compass_ak8975.h | 2 -- src/main/drivers/compass_fake.c | 3 +-- src/main/drivers/compass_hmc5883l.c | 3 +++ src/main/drivers/compass_hmc5883l.h | 2 -- src/main/sensors/acceleration.c | 20 ++++++++++---------- src/main/sensors/compass.c | 19 ++++++++++--------- src/main/sensors/gyro.c | 18 +++++++++--------- 10 files changed, 39 insertions(+), 36 deletions(-) diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index b90abf1bd0..fb280f8ae3 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -42,6 +42,9 @@ #include "accgyro_spi_mpu9250.h" #include "compass_ak8963.h" +void ak8963Init(void); +bool ak8963Read(int16_t *magData); + // This sensor is available in MPU-9250. // AK8963, mag sensor address diff --git a/src/main/drivers/compass_ak8963.h b/src/main/drivers/compass_ak8963.h index 34a4922257..9a13b17943 100644 --- a/src/main/drivers/compass_ak8963.h +++ b/src/main/drivers/compass_ak8963.h @@ -18,5 +18,3 @@ #pragma once bool ak8963Detect(magDev_t *mag); -void ak8963Init(void); -bool ak8963Read(int16_t *magData); diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index b86218f8a9..579c7118cb 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -37,6 +37,9 @@ #include "compass_ak8975.h" +void ak8975Init(void); +bool ak8975Read(int16_t *magData); + // This sensor is available in MPU-9150. // AK8975, mag sensor address diff --git a/src/main/drivers/compass_ak8975.h b/src/main/drivers/compass_ak8975.h index 5a7d729bed..05f5367f7d 100644 --- a/src/main/drivers/compass_ak8975.h +++ b/src/main/drivers/compass_ak8975.h @@ -18,5 +18,3 @@ #pragma once bool ak8975Detect(magDev_t *mag); -void ak8975Init(void); -bool ak8975Read(int16_t *magData); diff --git a/src/main/drivers/compass_fake.c b/src/main/drivers/compass_fake.c index cb8098c77d..20b58b1a16 100644 --- a/src/main/drivers/compass_fake.c +++ b/src/main/drivers/compass_fake.c @@ -32,13 +32,12 @@ static int16_t fakeMagData[XYZ_AXIS_COUNT]; -static bool fakeMagInit(void) +static void fakeMagInit(void) { // initially point north fakeMagData[X] = 4096; fakeMagData[Y] = 0; fakeMagData[Z] = 0; - return true; } void fakeMagSet(int16_t x, int16_t y, int16_t z) diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 0514d3cad0..d4e802597b 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -41,6 +41,9 @@ #include "compass_hmc5883l.h" +void hmc5883lInit(void); +bool hmc5883lRead(int16_t *magData); + //#define DEBUG_MAG_DATA_READY_INTERRUPT // HMC5883L, default address 0x1E diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 215bc84afa..9028143afc 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -24,5 +24,3 @@ typedef struct hmc5883Config_s { } hmc5883Config_t; bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse); -void hmc5883lInit(void); -bool hmc5883lRead(int16_t *magData); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 36b8c47d16..6061341630 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -84,7 +84,7 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) #endif retry: - acc.dev.accAlign = ALIGN_DEFAULT; + dev->accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { case ACC_DEFAULT: @@ -99,7 +99,7 @@ retry: if (adxl345Detect(&acc_params, dev)) { #endif #ifdef ACC_ADXL345_ALIGN - acc.dev.accAlign = ACC_ADXL345_ALIGN; + dev->accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; break; @@ -110,7 +110,7 @@ retry: #ifdef USE_ACC_LSM303DLHC if (lsm303dlhcAccDetect(dev)) { #ifdef ACC_LSM303DLHC_ALIGN - acc.dev.accAlign = ACC_LSM303DLHC_ALIGN; + dev->accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; break; @@ -121,7 +121,7 @@ retry: #ifdef USE_ACC_MPU6050 if (mpu6050AccDetect(dev)) { #ifdef ACC_MPU6050_ALIGN - acc.dev.accAlign = ACC_MPU6050_ALIGN; + dev->accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; break; @@ -137,7 +137,7 @@ retry: if (mma8452Detect(dev)) { #endif #ifdef ACC_MMA8452_ALIGN - acc.dev.accAlign = ACC_MMA8452_ALIGN; + dev->accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; break; @@ -148,7 +148,7 @@ retry: #ifdef USE_ACC_BMA280 if (bma280Detect(dev)) { #ifdef ACC_BMA280_ALIGN - acc.dev.accAlign = ACC_BMA280_ALIGN; + dev->accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; break; @@ -159,7 +159,7 @@ retry: #ifdef USE_ACC_SPI_MPU6000 if (mpu6000SpiAccDetect(dev)) { #ifdef ACC_MPU6000_ALIGN - acc.dev.accAlign = ACC_MPU6000_ALIGN; + dev->accAlign = ACC_MPU6000_ALIGN; #endif accHardware = ACC_MPU6000; break; @@ -175,7 +175,7 @@ retry: #endif { #ifdef ACC_MPU6500_ALIGN - acc.dev.accAlign = ACC_MPU6500_ALIGN; + dev->accAlign = ACC_MPU6500_ALIGN; #endif accHardware = ACC_MPU6500; break; @@ -188,7 +188,7 @@ retry: if (icm20689SpiAccDetect(dev)) { #ifdef ACC_ICM20689_ALIGN - acc.dev.accAlign = ACC_ICM20689_ALIGN; + dev->accAlign = ACC_ICM20689_ALIGN; #endif accHardware = ACC_ICM20689; break; @@ -380,7 +380,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis]; + DEBUG_SET(DEBUG_ACCELEROMETER, axis, accADCRaw[axis]); acc.accSmooth[axis] = accADCRaw[axis]; } diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index b0ee1a098f..9cc1e29fc3 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -81,7 +81,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) retry: - mag.dev.magAlign = ALIGN_DEFAULT; + dev->magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { case MAG_DEFAULT: @@ -91,7 +91,7 @@ retry: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(dev, hmc5883Config)) { #ifdef MAG_HMC5883_ALIGN - mag.dev.magAlign = MAG_HMC5883_ALIGN; + dev->magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; @@ -103,7 +103,7 @@ retry: #ifdef USE_MAG_AK8975 if (ak8975Detect(dev)) { #ifdef MAG_AK8975_ALIGN - mag.dev.magAlign = MAG_AK8975_ALIGN; + dev->magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; @@ -115,7 +115,7 @@ retry: #ifdef USE_MAG_AK8963 if (ak8963Detect(dev)) { #ifdef MAG_AK8963_ALIGN - mag.dev.magAlign = MAG_AK8963_ALIGN; + dev->magAlign = MAG_AK8963_ALIGN; #endif magHardware = MAG_AK8963; break; @@ -161,15 +161,16 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) static uint32_t tCal = 0; static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; - uint32_t axis; mag.dev.read(magADCRaw); - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) mag.magADC[axis] = magADCRaw[axis]; // int32_t copy to work with + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + mag.magADC[axis] = magADCRaw[axis]; + } alignSensors(mag.magADC, mag.dev.magAlign); if (STATE(CALIBRATE_MAG)) { tCal = currentTime; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { magZero->raw[axis] = 0; magZeroTempMin.raw[axis] = mag.magADC[axis]; magZeroTempMax.raw[axis] = mag.magADC[axis]; @@ -186,7 +187,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) if (tCal != 0) { if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions LED0_TOGGLE; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { if (mag.magADC[axis] < magZeroTempMin.raw[axis]) magZeroTempMin.raw[axis] = mag.magADC[axis]; if (mag.magADC[axis] > magZeroTempMax.raw[axis]) @@ -194,7 +195,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) } } else { tCal = 0; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f99ca3cf5f..98c78bd7eb 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -75,7 +75,7 @@ bool gyroDetect(gyroDev_t *dev) { gyroSensor_e gyroHardware = GYRO_DEFAULT; - gyro.dev.gyroAlign = ALIGN_DEFAULT; + dev->gyroAlign = ALIGN_DEFAULT; switch(gyroHardware) { case GYRO_DEFAULT: @@ -85,7 +85,7 @@ bool gyroDetect(gyroDev_t *dev) if (mpu6050GyroDetect(dev)) { gyroHardware = GYRO_MPU6050; #ifdef GYRO_MPU6050_ALIGN - gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN; + dev->gyroAlign = GYRO_MPU6050_ALIGN; #endif break; } @@ -96,7 +96,7 @@ bool gyroDetect(gyroDev_t *dev) if (l3g4200dDetect(dev)) { gyroHardware = GYRO_L3G4200D; #ifdef GYRO_L3G4200D_ALIGN - gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN; + dev->gyroAlign = GYRO_L3G4200D_ALIGN; #endif break; } @@ -108,7 +108,7 @@ bool gyroDetect(gyroDev_t *dev) if (mpu3050Detect(dev)) { gyroHardware = GYRO_MPU3050; #ifdef GYRO_MPU3050_ALIGN - gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN; + dev->gyroAlign = GYRO_MPU3050_ALIGN; #endif break; } @@ -120,7 +120,7 @@ bool gyroDetect(gyroDev_t *dev) if (l3gd20Detect(dev)) { gyroHardware = GYRO_L3GD20; #ifdef GYRO_L3GD20_ALIGN - gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN; + dev->gyroAlign = GYRO_L3GD20_ALIGN; #endif break; } @@ -132,7 +132,7 @@ bool gyroDetect(gyroDev_t *dev) if (mpu6000SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU6000; #ifdef GYRO_MPU6000_ALIGN - gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN; + dev->gyroAlign = GYRO_MPU6000_ALIGN; #endif break; } @@ -149,7 +149,7 @@ bool gyroDetect(gyroDev_t *dev) { gyroHardware = GYRO_MPU6500; #ifdef GYRO_MPU6500_ALIGN - gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN; + dev->gyroAlign = GYRO_MPU6500_ALIGN; #endif break; @@ -164,7 +164,7 @@ case GYRO_MPU9250: { gyroHardware = GYRO_MPU9250; #ifdef GYRO_MPU9250_ALIGN - gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN; + dev->gyroAlign = GYRO_MPU9250_ALIGN; #endif break; @@ -178,7 +178,7 @@ case GYRO_MPU9250: { gyroHardware = GYRO_ICM20689; #ifdef GYRO_ICM20689_ALIGN - gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN; + dev->gyroAlign = GYRO_ICM20689_ALIGN; #endif break; From b4bcf6fd964f541facc28feb127a6f0674c48f0e Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Wed, 7 Dec 2016 16:34:56 +0200 Subject: [PATCH 08/13] Fix F7 link output --- src/main/drivers/system_stm32f7xx.c | 2 +- src/main/target/link/stm32_flash_f745.ld | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index fed1c61f9f..93a6c9e8dd 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -155,7 +155,7 @@ void systemInit(void) //SystemClock_Config(); // Configure NVIC preempt/priority groups - //NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); + HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITY_GROUPING); // cache RCC->CSR value to use it in isMPUSoftreset() and others cachedRccCsrValue = RCC->CSR; diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld index 6c60e5c2c8..c8da9a3aa2 100644 --- a/src/main/target/link/stm32_flash_f745.ld +++ b/src/main/target/link/stm32_flash_f745.ld @@ -15,12 +15,14 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1008K - FLASH_CONFIG (r) : ORIGIN = 0x080FC000, LENGTH = 16K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 768K + FLASH_CONFIG (r) : ORIGIN = 0x080C0000, LENGTH = 256K - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 320K + TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } /* note CCM could be used for stack */ +REGION_ALIAS("STACKRAM", TCM) INCLUDE "stm32_flash.ld" From bc43c1047379289f8ecb3056e8fc442d6b68022b Mon Sep 17 00:00:00 2001 From: blckmn Date: Thu, 8 Dec 2016 16:20:59 +1100 Subject: [PATCH 09/13] Removed specifics, and allowed ANY assignment as specified by the user --- src/main/drivers/pwm_rx.c | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 2adaa8af6c..ed52ecaec6 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -103,7 +103,6 @@ typedef struct ppmDevice_s { ppmDevice_t ppmDev; - #define PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds #define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds #define PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds @@ -111,7 +110,6 @@ ppmDevice_t ppmDev; #define PPM_IN_MIN_NUM_CHANNELS 4 #define PPM_IN_MAX_NUM_CHANNELS PWM_PORTS_OR_PPM_CAPTURE_COUNT - bool isPPMDataBeingReceived(void) { return (ppmFrameCount != lastPPMFrameCount); @@ -364,17 +362,15 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) TIM_ICInit(tim, &TIM_ICInitStructure); } - #endif - void pwmRxInit(const pwmConfig_t *pwmConfig) { for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) { pwmInputPort_t *port = &pwmInputPorts[channel]; - const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_PWM); + const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_ANY); if (!timer) { /* TODO: maybe fail here if not enough channels? */ @@ -444,7 +440,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT]; - const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_PPM); + const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_ANY); if (!timer) { /* TODO: fail here? */ return; From cfc44df4ad783486ff22bf5fc5c7b491c5c6e0b8 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 8 Dec 2016 22:31:27 +0100 Subject: [PATCH 10/13] Cli flash_erase print buffer flush and progress indicator added. --- src/main/io/serial_cli.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 159df98af5..2ae2b2ff83 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2252,18 +2252,28 @@ static void cliFlashInfo(char *cmdline) layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset()); } + static void cliFlashErase(char *cmdline) { UNUSED(cmdline); + uint32_t i; - cliPrintf("Erasing...\r\n"); + cliPrintf("Erasing, please wait ... \r\n"); + bufWriterFlush(cliWriter); flashfsEraseCompletely(); while (!flashfsIsReady()) { + cliPrintf("."); + if (i++ > 120) { + i=0; + cliPrintf("\r\n"); + } + + bufWriterFlush(cliWriter); delay(100); } - cliPrintf("Done.\r\n"); + cliPrintf("\r\nDone.\r\n"); } #ifdef USE_FLASH_TOOLS From 5e1e2397e300971ecacc699c795413a82e76f755 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 8 Dec 2016 23:58:04 +0100 Subject: [PATCH 11/13] Cli flash_erase rogress indicator conditional on CLI_MINIMAL_VERBOSITY. --- src/main/io/serial_cli.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2ae2b2ff83..2f6585a462 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2256,13 +2256,19 @@ static void cliFlashInfo(char *cmdline) static void cliFlashErase(char *cmdline) { UNUSED(cmdline); - uint32_t i; +#ifndef CLI_MINIMAL_VERBOSITY + uint32_t i; cliPrintf("Erasing, please wait ... \r\n"); +#else + cliPrintf("Erasing,\r\n"); +#endif + bufWriterFlush(cliWriter); flashfsEraseCompletely(); while (!flashfsIsReady()) { +#ifndef CLI_MINIMAL_VERBOSITY cliPrintf("."); if (i++ > 120) { i=0; @@ -2270,6 +2276,7 @@ static void cliFlashErase(char *cmdline) } bufWriterFlush(cliWriter); +#endif delay(100); } From b90185ef8c3fe0abf8d173c1aab56f7f620e248a Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 9 Dec 2016 01:05:08 +0100 Subject: [PATCH 12/13] Build date and rev was not updated on incremental builds. Fixed. --- Makefile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Makefile b/Makefile index f2edc40d58..f66541d3ea 100644 --- a/Makefile +++ b/Makefile @@ -831,6 +831,9 @@ CLEAN_ARTIFACTS := $(TARGET_BIN) CLEAN_ARTIFACTS += $(TARGET_HEX) CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) +# Make sure build date and revision is updated on every incremental build +$(OBJECT_DIR)/$(TARGET)/build/version.o : $(TARGET_SRC) + # List of buildable ELF files and their object dependencies. # It would be nice to compute these lists, but that seems to be just beyond make. From c7835f781dfd2264b6a89d7ea0a0f55cd1023b89 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 10 Dec 2016 10:03:18 +0000 Subject: [PATCH 13/13] Fixes issue 1789 --- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 98c78bd7eb..691597a5b6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -216,10 +216,10 @@ void gyroInit(const gyroConfig_t *gyroConfigToUse) static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; + gyroConfig = gyroConfigToUse; gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation gyro.dev.lpf = gyroConfig->gyro_lpf; gyro.dev.init(&gyro.dev); - gyroConfig = gyroConfigToUse; softLpfFilterApplyFn = nullFilterApply; notchFilter1ApplyFn = nullFilterApply; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 5d45d917c6..656ec47f53 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -46,9 +46,9 @@ typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyro_sync_denom; // Gyro sample divider + uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_type; uint8_t gyro_soft_lpf_hz; - uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint16_t gyro_soft_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2;