From 4237370b60c4721833fade0f5799e3574ac60de2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 15 Sep 2014 23:38:51 +0100 Subject: [PATCH] Saving files before refreshing line endings --- src/main/common/printf.c | 426 +++++++-------- src/main/sensors/initialisation.c | 878 +++++++++++++++--------------- src/main/target/NAZE/target.h | 144 ++--- 3 files changed, 724 insertions(+), 724 deletions(-) diff --git a/src/main/common/printf.c b/src/main/common/printf.c index 41daaca1c2c..e558ff86204 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -1,213 +1,213 @@ -/* - * Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or other - * materials provided with the distribution. - * - * Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its - * contributors may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. - * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, - * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT - * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, - * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY - * OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -#include "platform.h" - -#include "build_config.h" - -#include "drivers/serial.h" -#include "io/serial.h" - -#include "build_config.h" -#include "printf.h" - -#ifdef REQUIRE_PRINTF_LONG_SUPPORT -#include "typeconversion.h" -#endif - -static serialPort_t *printfSerialPort; - -#ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT - -typedef void (*putcf) (void *, char); -static putcf stdout_putf; -static void *stdout_putp; - -static void putchw(void *putp, putcf putf, int n, char z, char *bf) -{ - char fc = z ? '0' : ' '; - char ch; - char *p = bf; - while (*p++ && n > 0) - n--; - while (n-- > 0) - putf(putp, fc); - while ((ch = *bf++)) - putf(putp, ch); -} - -void tfp_format(void *putp, putcf putf, char *fmt, va_list va) -{ - char bf[12]; - - char ch; - - while ((ch = *(fmt++))) { - if (ch != '%') - putf(putp, ch); - else { - char lz = 0; -#ifdef REQUIRE_PRINTF_LONG_SUPPORT - char lng = 0; -#endif - int w = 0; - ch = *(fmt++); - if (ch == '0') { - ch = *(fmt++); - lz = 1; - } - if (ch >= '0' && ch <= '9') { - ch = a2i(ch, &fmt, 10, &w); - } -#ifdef REQUIRE_PRINTF_LONG_SUPPORT - if (ch == 'l') { - ch = *(fmt++); - lng = 1; - } -#endif - switch (ch) { - case 0: - goto abort; - case 'u':{ -#ifdef REQUIRE_PRINTF_LONG_SUPPORT - if (lng) - uli2a(va_arg(va, unsigned long int), 10, 0, bf); - else -#endif - ui2a(va_arg(va, unsigned int), 10, 0, bf); - putchw(putp, putf, w, lz, bf); - break; - } - case 'd':{ -#ifdef REQUIRE_PRINTF_LONG_SUPPORT - if (lng) - li2a(va_arg(va, unsigned long int), bf); - else -#endif - i2a(va_arg(va, int), bf); - putchw(putp, putf, w, lz, bf); - break; - } - case 'x': - case 'X': -#ifdef REQUIRE_PRINTF_LONG_SUPPORT - if (lng) - uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); - else -#endif - ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); - putchw(putp, putf, w, lz, bf); - break; - case 'c': - putf(putp, (char) (va_arg(va, int))); - break; - case 's': - putchw(putp, putf, w, 0, va_arg(va, char *)); - break; - case '%': - putf(putp, ch); - break; - default: - break; - } - } - } - abort:; -} - -void init_printf(void *putp, void (*putf) (void *, char)) -{ - stdout_putf = putf; - stdout_putp = putp; -} - -void tfp_printf(char *fmt, ...) -{ - va_list va; - va_start(va, fmt); - tfp_format(stdout_putp, stdout_putf, fmt, va); - va_end(va); - while (!isSerialTransmitBufferEmpty(printfSerialPort)); -} - -static void putcp(void *p, char c) -{ - *(*((char **) p))++ = c; -} - -void tfp_sprintf(char *s, char *fmt, ...) -{ - va_list va; - - va_start(va, fmt); - tfp_format(&s, putcp, fmt, va); - putcp(&s, 0); - va_end(va); -} - - -static void _putc(void *p, char c) -{ - UNUSED(p); - serialWrite(printfSerialPort, c); -} - -void initPrintfSupport(void) -{ - init_printf(NULL, _putc); -} - -#else - -// keil/armcc version -int fputc(int c, FILE *f) -{ - // let DMA catch up a bit when using set or dump, we're too fast. - while (!isSerialTransmitBufferEmpty(serialPorts.mainport)); - serialWrite(printfSerialPort, c); - return c; -} - -void initPrintfSupport(serialPort_t *serialPort) -{ - // Nothing to do -} -#endif - -void setPrintfSerialPort(serialPort_t *serialPort) -{ - printfSerialPort = serialPort; -} +/* + * Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its + * contributors may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include "platform.h" + +#include "build_config.h" + +#include "drivers/serial.h" +#include "io/serial.h" + +#include "build_config.h" +#include "printf.h" + +#ifdef REQUIRE_PRINTF_LONG_SUPPORT +#include "typeconversion.h" +#endif + +static serialPort_t *printfSerialPort; + +#ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT + +typedef void (*putcf) (void *, char); +static putcf stdout_putf; +static void *stdout_putp; + +static void putchw(void *putp, putcf putf, int n, char z, char *bf) +{ + char fc = z ? '0' : ' '; + char ch; + char *p = bf; + while (*p++ && n > 0) + n--; + while (n-- > 0) + putf(putp, fc); + while ((ch = *bf++)) + putf(putp, ch); +} + +void tfp_format(void *putp, putcf putf, char *fmt, va_list va) +{ + char bf[12]; + + char ch; + + while ((ch = *(fmt++))) { + if (ch != '%') + putf(putp, ch); + else { + char lz = 0; +#ifdef REQUIRE_PRINTF_LONG_SUPPORT + char lng = 0; +#endif + int w = 0; + ch = *(fmt++); + if (ch == '0') { + ch = *(fmt++); + lz = 1; + } + if (ch >= '0' && ch <= '9') { + ch = a2i(ch, &fmt, 10, &w); + } +#ifdef REQUIRE_PRINTF_LONG_SUPPORT + if (ch == 'l') { + ch = *(fmt++); + lng = 1; + } +#endif + switch (ch) { + case 0: + goto abort; + case 'u':{ +#ifdef REQUIRE_PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int), 10, 0, bf); + else +#endif + ui2a(va_arg(va, unsigned int), 10, 0, bf); + putchw(putp, putf, w, lz, bf); + break; + } + case 'd':{ +#ifdef REQUIRE_PRINTF_LONG_SUPPORT + if (lng) + li2a(va_arg(va, unsigned long int), bf); + else +#endif + i2a(va_arg(va, int), bf); + putchw(putp, putf, w, lz, bf); + break; + } + case 'x': + case 'X': +#ifdef REQUIRE_PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); + else +#endif + ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); + putchw(putp, putf, w, lz, bf); + break; + case 'c': + putf(putp, (char) (va_arg(va, int))); + break; + case 's': + putchw(putp, putf, w, 0, va_arg(va, char *)); + break; + case '%': + putf(putp, ch); + break; + default: + break; + } + } + } + abort:; +} + +void init_printf(void *putp, void (*putf) (void *, char)) +{ + stdout_putf = putf; + stdout_putp = putp; +} + +void tfp_printf(char *fmt, ...) +{ + va_list va; + va_start(va, fmt); + tfp_format(stdout_putp, stdout_putf, fmt, va); + va_end(va); + while (!isSerialTransmitBufferEmpty(printfSerialPort)); +} + +static void putcp(void *p, char c) +{ + *(*((char **) p))++ = c; +} + +void tfp_sprintf(char *s, char *fmt, ...) +{ + va_list va; + + va_start(va, fmt); + tfp_format(&s, putcp, fmt, va); + putcp(&s, 0); + va_end(va); +} + + +static void _putc(void *p, char c) +{ + UNUSED(p); + serialWrite(printfSerialPort, c); +} + +void initPrintfSupport(void) +{ + init_printf(NULL, _putc); +} + +#else + +// keil/armcc version +int fputc(int c, FILE *f) +{ + // let DMA catch up a bit when using set or dump, we're too fast. + while (!isSerialTransmitBufferEmpty(serialPorts.mainport)); + serialWrite(printfSerialPort, c); + return c; +} + +void initPrintfSupport(serialPort_t *serialPort) +{ + // Nothing to do +} +#endif + +void setPrintfSerialPort(serialPort_t *serialPort) +{ + printfSerialPort = serialPort; +} diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index ba5366e887e..c49057b0baa 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -1,439 +1,439 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ -#include -#include -#include - -#include "platform.h" - -#include "build_config.h" - -#include "common/axis.h" - -#include "drivers/accgyro.h" - -#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" -#ifdef STM32F3DISCOVERY -#include "drivers/accgyro_l3gd20.h" -#include "drivers/accgyro_lsm303dlhc.h" -#endif -#ifdef CC3D -#include "drivers/accgyro_spi_mpu6000.h" -#endif - -#include "drivers/barometer.h" -#include "drivers/barometer_bmp085.h" -#include "drivers/barometer_ms5611.h" -#include "drivers/compass_hmc5883l.h" -#include "drivers/sonar_hcsr04.h" -#include "drivers/system.h" - -#include "flight/flight.h" -#include "config/runtime_config.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/gyro.h" -#include "sensors/compass.h" -#include "sensors/sonar.h" - - -// Use these to help with porting to new boards -//#define USE_FAKE_GYRO -#define USE_GYRO_L3G4200D -#define USE_GYRO_L3GD20 -#define USE_GYRO_MPU6050 -#define USE_GYRO_SPI_MPU6000 -#define USE_GYRO_MPU3050 -//#define USE_FAKE_ACC -#define USE_ACC_ADXL345 -#define USE_ACC_BMA280 -#define USE_ACC_MMA8452 -#define USE_ACC_LSM303DLHC -#define USE_ACC_MPU6050 -#define USE_ACC_SPI_MPU6000 -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 - -#ifdef NAZE -#undef USE_ACC_LSM303DLHC -#undef USE_ACC_SPI_MPU6000 -#undef USE_GYRO_SPI_MPU6000 -#undef USE_GYRO_L3GD20 -#endif - -#ifdef NAZE32PRO -#define USE_FAKE_ACC -#define USE_FAKE_GYRO -#undef USE_ACC_LSM303DLHC -#undef USE_ACC_ADXL345 -#undef USE_ACC_BMA280 -#undef USE_ACC_MMA8452 -#undef USE_ACC_LSM303DLHC -#undef USE_ACC_MPU6050 -#undef USE_ACC_SPI_MPU6000 -#undef USE_GYRO_L3G4200D -#undef USE_GYRO_L3GD20 -#undef USE_GYRO_MPU3050 -#undef USE_GYRO_MPU6050 -#undef USE_GYRO_SPI_MPU6000 -#endif - -#if defined(OLIMEXINO) || defined(EUSTM32F103RC) -#undef USE_GYRO_L3GD20 -#undef USE_GYRO_L3G4200D -#undef USE_GYRO_MPU3050 -#undef USE_GYRO_SPI_MPU6000 -#undef USE_ACC_LSM303DLHC -#undef USE_ACC_BMA280 -#undef USE_ACC_MMA8452 -#undef USE_ACC_ADXL345 -#undef USE_ACC_SPI_MPU6000 -#undef USE_BARO_MS5611 -#endif - -#ifdef EUSTM32F103RC -#define USE_FAKE_GYRO -#define USE_FAKE_ACC -#define USE_GYRO_L3G4200D -#endif - -#ifdef STM32F3DISCOVERY -#undef USE_ACC_SPI_MPU6000 -#undef USE_GYRO_SPI_MPU6000 -#endif - -#ifdef CHEBUZZF3 -#undef USE_GYRO_L3G4200D -#undef USE_GYRO_MPU6050 -#undef USE_GYRO_MPU3050 -#undef USE_GYRO_SPI_MPU6000 -#undef USE_ACC_ADXL345 -#undef USE_ACC_BMA280 -#undef USE_ACC_MPU6050 -#undef USE_ACC_MMA8452 -#undef USE_ACC_SPI_MPU6000 -#endif - -#ifdef CC3D -#undef USE_GYRO_L3GD20 -#undef USE_GYRO_L3G4200D -#undef USE_GYRO_MPU6050 -#undef USE_GYRO_MPU3050 -#undef USE_ACC_LSM303DLHC -#undef USE_ACC_ADXL345 -#undef USE_ACC_BMA280 -#undef USE_ACC_MPU6050 -#undef USE_ACC_MMA8452 -#endif - -#ifdef CJMCU -#undef USE_GYRO_SPI_MPU6000 -#undef USE_GYRO_L3GD20 -#undef USE_GYRO_L3G4200D -#undef USE_GYRO_MPU3050 -#undef USE_ACC_LSM303DLHC -#undef USE_ACC_SPI_MPU6000 -#undef USE_ACC_ADXL345 -#undef USE_ACC_BMA280 -#undef USE_ACC_MMA8452 -#endif - -extern float magneticDeclination; - -extern gyro_t gyro; -extern baro_t baro; -extern acc_t acc; - -#ifdef USE_FAKE_GYRO -static void fakeGyroInit(void) {} -static void fakeGyroRead(int16_t *gyroData) { - UNUSED(gyroData); -} -static void fakeGyroReadTemp(int16_t *tempData) { - UNUSED(tempData); -} - -bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) -{ - UNUSED(lpf); - gyro->init = fakeGyroInit; - gyro->read = fakeGyroRead; - gyro->temperature = fakeGyroReadTemp; - return true; -} -#endif - -#ifdef USE_FAKE_ACC -static void fakeAccInit(void) {} -static void fakeAccRead(int16_t *accData) { - UNUSED(accData); -} - -bool fakeAccDetect(acc_t *acc) -{ - acc->init = fakeAccInit; - acc->read = fakeAccRead; - acc->revisionCode = 0; - return true; -} -#endif - -bool detectGyro(uint16_t gyroLpf) -{ - gyroAlign = ALIGN_DEFAULT; - -#ifdef USE_GYRO_MPU6050 - if (mpu6050GyroDetect(&gyro, gyroLpf)) { -#ifdef NAZE - gyroAlign = CW0_DEG; -#endif - return true; - } -#endif - -#ifdef USE_GYRO_L3G4200D - if (l3g4200dDetect(&gyro, gyroLpf)) { -#ifdef NAZE - gyroAlign = CW0_DEG; -#endif - return true; - } -#endif - -#ifdef USE_GYRO_MPU3050 - if (mpu3050Detect(&gyro, gyroLpf)) { -#ifdef NAZE - gyroAlign = CW0_DEG; -#endif - return true; - } -#endif - -#ifdef USE_GYRO_L3GD20 - if (l3gd20Detect(&gyro, gyroLpf)) { - return true; - } -#endif - -#ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) { -#ifdef CC3D - gyroAlign = CW270_DEG; -#endif - return true; - } -#endif - -#ifdef USE_FAKE_GYRO - if (fakeGyroDetect(&gyro, gyroLpf)) { - return true; - } -#endif - return false; -} - -static void detectAcc(uint8_t accHardwareToUse) -{ -#ifdef USE_ACC_ADXL345 - drv_adxl345_config_t acc_params; -#endif - -retry: - accAlign = ALIGN_DEFAULT; - - switch (accHardwareToUse) { -#ifdef USE_FAKE_ACC - default: - if (fakeAccDetect(&acc)) { - accHardware = ACC_FAKE; - if (accHardwareToUse == ACC_FAKE) - break; - } -#endif - case ACC_NONE: // disable ACC - sensorsClear(SENSOR_ACC); - break; - case ACC_DEFAULT: // autodetect -#ifdef USE_ACC_ADXL345 - case ACC_ADXL345: // ADXL345 - acc_params.useFifo = false; - acc_params.dataRate = 800; // unused currently - if (adxl345Detect(&acc_params, &acc)) { - accHardware = ACC_ADXL345; -#ifdef NAZE - accAlign = CW270_DEG; -#endif - } - if (accHardwareToUse == ACC_ADXL345) - break; - ; // fallthrough -#endif -#ifdef USE_ACC_MPU6050 - case ACC_MPU6050: // MPU6050 - if (mpu6050AccDetect(&acc)) { - accHardware = ACC_MPU6050; -#ifdef NAZE - accAlign = CW0_DEG; -#endif - if (accHardwareToUse == ACC_MPU6050) - break; - } - ; // fallthrough -#endif -#ifdef USE_ACC_MMA8452 - case ACC_MMA8452: // MMA8452 - if (mma8452Detect(&acc)) { - accHardware = ACC_MMA8452; -#ifdef NAZE - accAlign = CW90_DEG; -#endif - if (accHardwareToUse == ACC_MMA8452) - break; - } - ; // fallthrough -#endif -#ifdef USE_ACC_BMA280 - case ACC_BMA280: // BMA280 - if (bma280Detect(&acc)) { - accHardware = ACC_BMA280; -#ifdef NAZE - accAlign = CW0_DEG; -#endif - if (accHardwareToUse == ACC_BMA280) - break; - } - ; // fallthrough -#endif -#ifdef USE_ACC_LSM303DLHC - case ACC_LSM303DLHC: - if (lsm303dlhcAccDetect(&acc)) { - accHardware = ACC_LSM303DLHC; - if (accHardwareToUse == ACC_LSM303DLHC) - break; - } - ; // fallthrough -#endif -#ifdef USE_GYRO_SPI_MPU6000 - case ACC_SPI_MPU6000: - if (mpu6000SpiAccDetect(&acc)) { - accHardware = ACC_SPI_MPU6000; -#ifdef CC3D - accAlign = CW270_DEG; -#endif - if (accHardwareToUse == ACC_SPI_MPU6000) - break; - } - ; // fallthrough -#endif - ; // prevent compiler error - } - - // Found anything? Check if user fucked up or ACC is really missing. - if (accHardware == 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. - accHardwareToUse = ACC_DEFAULT; - goto retry; - } else { - // No ACC was detected - sensorsClear(SENSOR_ACC); - } - } -} - -static void detectBaro() -{ -#ifdef BARO -#ifdef USE_BARO_MS5611 - // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function - if (ms5611Detect(&baro)) { - return; - } -#endif - -#ifdef USE_BARO_BMP085 - // ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro - if (bmp085Detect(&baro)) { - return; - } -#endif - sensorsClear(SENSOR_BARO); -#endif -} - -void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) -{ - if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { - gyroAlign = sensorAlignmentConfig->gyro_align; - } - if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) { - accAlign = sensorAlignmentConfig->acc_align; - } - if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) { - magAlign = sensorAlignmentConfig->mag_align; - } -} - -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) -{ - int16_t deg, min; - memset(&acc, sizeof(acc), 0); - memset(&gyro, sizeof(gyro), 0); - - if (!detectGyro(gyroLpf)) { - return false; - } - detectAcc(accHardwareToUse); - detectBaro(); - - reconfigureAlignment(sensorAlignmentConfig); - - // Now time to init things, acc first - if (sensors(SENSOR_ACC)) - acc.init(); - // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyro.init(); - -#ifdef MAG - if (hmc5883lDetect()) { - magAlign = CW180_DEG; // default NAZE alignment - } else { - sensorsClear(SENSOR_MAG); - } -#endif - - // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c - if (sensors(SENSOR_MAG)) { - // 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 { - 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. - } - - return true; -} - +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ +#include +#include +#include + +#include "platform.h" + +#include "build_config.h" + +#include "common/axis.h" + +#include "drivers/accgyro.h" + +#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" +#ifdef STM32F3DISCOVERY +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" +#endif +#ifdef CC3D +#include "drivers/accgyro_spi_mpu6000.h" +#endif + +#include "drivers/barometer.h" +#include "drivers/barometer_bmp085.h" +#include "drivers/barometer_ms5611.h" +#include "drivers/compass_hmc5883l.h" +#include "drivers/sonar_hcsr04.h" +#include "drivers/system.h" + +#include "flight/flight.h" +#include "config/runtime_config.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/sonar.h" + + +// Use these to help with porting to new boards +//#define USE_FAKE_GYRO +#define USE_GYRO_L3G4200D +#define USE_GYRO_L3GD20 +#define USE_GYRO_MPU6050 +#define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_MPU3050 +//#define USE_FAKE_ACC +#define USE_ACC_ADXL345 +#define USE_ACC_BMA280 +#define USE_ACC_MMA8452 +#define USE_ACC_LSM303DLHC +#define USE_ACC_MPU6050 +#define USE_ACC_SPI_MPU6000 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 + +#ifdef NAZE +#undef USE_ACC_LSM303DLHC +#undef USE_ACC_SPI_MPU6000 +#undef USE_GYRO_SPI_MPU6000 +#undef USE_GYRO_L3GD20 +#endif + +#ifdef NAZE32PRO +#define USE_FAKE_ACC +#define USE_FAKE_GYRO +#undef USE_ACC_LSM303DLHC +#undef USE_ACC_ADXL345 +#undef USE_ACC_BMA280 +#undef USE_ACC_MMA8452 +#undef USE_ACC_LSM303DLHC +#undef USE_ACC_MPU6050 +#undef USE_ACC_SPI_MPU6000 +#undef USE_GYRO_L3G4200D +#undef USE_GYRO_L3GD20 +#undef USE_GYRO_MPU3050 +#undef USE_GYRO_MPU6050 +#undef USE_GYRO_SPI_MPU6000 +#endif + +#if defined(OLIMEXINO) || defined(EUSTM32F103RC) +#undef USE_GYRO_L3GD20 +#undef USE_GYRO_L3G4200D +#undef USE_GYRO_MPU3050 +#undef USE_GYRO_SPI_MPU6000 +#undef USE_ACC_LSM303DLHC +#undef USE_ACC_BMA280 +#undef USE_ACC_MMA8452 +#undef USE_ACC_ADXL345 +#undef USE_ACC_SPI_MPU6000 +#undef USE_BARO_MS5611 +#endif + +#ifdef EUSTM32F103RC +#define USE_FAKE_GYRO +#define USE_FAKE_ACC +#define USE_GYRO_L3G4200D +#endif + +#ifdef STM32F3DISCOVERY +#undef USE_ACC_SPI_MPU6000 +#undef USE_GYRO_SPI_MPU6000 +#endif + +#ifdef CHEBUZZF3 +#undef USE_GYRO_L3G4200D +#undef USE_GYRO_MPU6050 +#undef USE_GYRO_MPU3050 +#undef USE_GYRO_SPI_MPU6000 +#undef USE_ACC_ADXL345 +#undef USE_ACC_BMA280 +#undef USE_ACC_MPU6050 +#undef USE_ACC_MMA8452 +#undef USE_ACC_SPI_MPU6000 +#endif + +#ifdef CC3D +#undef USE_GYRO_L3GD20 +#undef USE_GYRO_L3G4200D +#undef USE_GYRO_MPU6050 +#undef USE_GYRO_MPU3050 +#undef USE_ACC_LSM303DLHC +#undef USE_ACC_ADXL345 +#undef USE_ACC_BMA280 +#undef USE_ACC_MPU6050 +#undef USE_ACC_MMA8452 +#endif + +#ifdef CJMCU +#undef USE_GYRO_SPI_MPU6000 +#undef USE_GYRO_L3GD20 +#undef USE_GYRO_L3G4200D +#undef USE_GYRO_MPU3050 +#undef USE_ACC_LSM303DLHC +#undef USE_ACC_SPI_MPU6000 +#undef USE_ACC_ADXL345 +#undef USE_ACC_BMA280 +#undef USE_ACC_MMA8452 +#endif + +extern float magneticDeclination; + +extern gyro_t gyro; +extern baro_t baro; +extern acc_t acc; + +#ifdef USE_FAKE_GYRO +static void fakeGyroInit(void) {} +static void fakeGyroRead(int16_t *gyroData) { + UNUSED(gyroData); +} +static void fakeGyroReadTemp(int16_t *tempData) { + UNUSED(tempData); +} + +bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) +{ + UNUSED(lpf); + gyro->init = fakeGyroInit; + gyro->read = fakeGyroRead; + gyro->temperature = fakeGyroReadTemp; + return true; +} +#endif + +#ifdef USE_FAKE_ACC +static void fakeAccInit(void) {} +static void fakeAccRead(int16_t *accData) { + UNUSED(accData); +} + +bool fakeAccDetect(acc_t *acc) +{ + acc->init = fakeAccInit; + acc->read = fakeAccRead; + acc->revisionCode = 0; + return true; +} +#endif + +bool detectGyro(uint16_t gyroLpf) +{ + gyroAlign = ALIGN_DEFAULT; + +#ifdef USE_GYRO_MPU6050 + if (mpu6050GyroDetect(&gyro, gyroLpf)) { +#ifdef NAZE + gyroAlign = CW0_DEG; +#endif + return true; + } +#endif + +#ifdef USE_GYRO_L3G4200D + if (l3g4200dDetect(&gyro, gyroLpf)) { +#ifdef NAZE + gyroAlign = CW0_DEG; +#endif + return true; + } +#endif + +#ifdef USE_GYRO_MPU3050 + if (mpu3050Detect(&gyro, gyroLpf)) { +#ifdef NAZE + gyroAlign = CW0_DEG; +#endif + return true; + } +#endif + +#ifdef USE_GYRO_L3GD20 + if (l3gd20Detect(&gyro, gyroLpf)) { + return true; + } +#endif + +#ifdef USE_GYRO_SPI_MPU6000 + if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) { +#ifdef CC3D + gyroAlign = CW270_DEG; +#endif + return true; + } +#endif + +#ifdef USE_FAKE_GYRO + if (fakeGyroDetect(&gyro, gyroLpf)) { + return true; + } +#endif + return false; +} + +static void detectAcc(uint8_t accHardwareToUse) +{ +#ifdef USE_ACC_ADXL345 + drv_adxl345_config_t acc_params; +#endif + +retry: + accAlign = ALIGN_DEFAULT; + + switch (accHardwareToUse) { +#ifdef USE_FAKE_ACC + default: + if (fakeAccDetect(&acc)) { + accHardware = ACC_FAKE; + if (accHardwareToUse == ACC_FAKE) + break; + } +#endif + case ACC_NONE: // disable ACC + sensorsClear(SENSOR_ACC); + break; + case ACC_DEFAULT: // autodetect +#ifdef USE_ACC_ADXL345 + case ACC_ADXL345: // ADXL345 + acc_params.useFifo = false; + acc_params.dataRate = 800; // unused currently + if (adxl345Detect(&acc_params, &acc)) { + accHardware = ACC_ADXL345; +#ifdef NAZE + accAlign = CW270_DEG; +#endif + } + if (accHardwareToUse == ACC_ADXL345) + break; + ; // fallthrough +#endif +#ifdef USE_ACC_MPU6050 + case ACC_MPU6050: // MPU6050 + if (mpu6050AccDetect(&acc)) { + accHardware = ACC_MPU6050; +#ifdef NAZE + accAlign = CW0_DEG; +#endif + if (accHardwareToUse == ACC_MPU6050) + break; + } + ; // fallthrough +#endif +#ifdef USE_ACC_MMA8452 + case ACC_MMA8452: // MMA8452 + if (mma8452Detect(&acc)) { + accHardware = ACC_MMA8452; +#ifdef NAZE + accAlign = CW90_DEG; +#endif + if (accHardwareToUse == ACC_MMA8452) + break; + } + ; // fallthrough +#endif +#ifdef USE_ACC_BMA280 + case ACC_BMA280: // BMA280 + if (bma280Detect(&acc)) { + accHardware = ACC_BMA280; +#ifdef NAZE + accAlign = CW0_DEG; +#endif + if (accHardwareToUse == ACC_BMA280) + break; + } + ; // fallthrough +#endif +#ifdef USE_ACC_LSM303DLHC + case ACC_LSM303DLHC: + if (lsm303dlhcAccDetect(&acc)) { + accHardware = ACC_LSM303DLHC; + if (accHardwareToUse == ACC_LSM303DLHC) + break; + } + ; // fallthrough +#endif +#ifdef USE_GYRO_SPI_MPU6000 + case ACC_SPI_MPU6000: + if (mpu6000SpiAccDetect(&acc)) { + accHardware = ACC_SPI_MPU6000; +#ifdef CC3D + accAlign = CW270_DEG; +#endif + if (accHardwareToUse == ACC_SPI_MPU6000) + break; + } + ; // fallthrough +#endif + ; // prevent compiler error + } + + // Found anything? Check if user fucked up or ACC is really missing. + if (accHardware == 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. + accHardwareToUse = ACC_DEFAULT; + goto retry; + } else { + // No ACC was detected + sensorsClear(SENSOR_ACC); + } + } +} + +static void detectBaro() +{ +#ifdef BARO +#ifdef USE_BARO_MS5611 + // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function + if (ms5611Detect(&baro)) { + return; + } +#endif + +#ifdef USE_BARO_BMP085 + // ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro + if (bmp085Detect(&baro)) { + return; + } +#endif + sensorsClear(SENSOR_BARO); +#endif +} + +void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) +{ + if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { + gyroAlign = sensorAlignmentConfig->gyro_align; + } + if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) { + accAlign = sensorAlignmentConfig->acc_align; + } + if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) { + magAlign = sensorAlignmentConfig->mag_align; + } +} + +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) +{ + int16_t deg, min; + memset(&acc, sizeof(acc), 0); + memset(&gyro, sizeof(gyro), 0); + + if (!detectGyro(gyroLpf)) { + return false; + } + detectAcc(accHardwareToUse); + detectBaro(); + + reconfigureAlignment(sensorAlignmentConfig); + + // Now time to init things, acc first + if (sensors(SENSOR_ACC)) + acc.init(); + // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. + gyro.init(); + +#ifdef MAG + if (hmc5883lDetect()) { + magAlign = CW180_DEG; // default NAZE alignment + } else { + sensorsClear(SENSOR_MAG); + } +#endif + + // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c + if (sensors(SENSOR_MAG)) { + // 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 { + 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. + } + + return true; +} + diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 4e78b53d9f4..97817a0e8e8 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -1,72 +1,72 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define LED0_GPIO GPIOB -#define LED0_PIN Pin_3 // PB3 (LED) -#define LED0_PERIPHERAL RCC_APB2Periph_GPIOB -#define LED1_GPIO GPIOB -#define LED1_PIN Pin_4 // PB4 (LED) -#define LED1_PERIPHERAL RCC_APB2Periph_GPIOB - - -#define BEEP_GPIO GPIOA -#define BEEP_PIN Pin_12 // PA12 (Beeper) -#define BEEP_PERIPHERAL RCC_APB2Periph_GPIOA - -#define BARO_GPIO GPIOC -#define BARO_PIN Pin_13 - -#define INVERTER_PIN Pin_2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_GPIO GPIOB -#define INVERTER_PERIPHERAL RCC_APB2Periph_GPIOB -#define INVERTER_USART USART2 - -#define GYRO -#define ACC -#define MAG -#define BARO -#define SONAR -#define BEEPER -#define LED0 -#define LED1 -#define INVERTER -#define DISPLAY - -#define USE_USART1 -#define USE_USART2 -#define USE_SOFT_SERIAL -#define SERIAL_PORT_COUNT 4 - - -#define I2C_DEVICE (I2CDEV_2) - -// #define SOFT_I2C // enable to test software i2c -// #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_BARO | SENSOR_MAG) - -#define GPS - -#define LED_STRIP -#define TELEMETRY -#define SOFT_SERIAL -#define SERIAL_RX -#define AUTOTUNE +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_3 // PB3 (LED) +#define LED0_PERIPHERAL RCC_APB2Periph_GPIOB +#define LED1_GPIO GPIOB +#define LED1_PIN Pin_4 // PB4 (LED) +#define LED1_PERIPHERAL RCC_APB2Periph_GPIOB + + +#define BEEP_GPIO GPIOA +#define BEEP_PIN Pin_12 // PA12 (Beeper) +#define BEEP_PERIPHERAL RCC_APB2Periph_GPIOA + +#define BARO_GPIO GPIOC +#define BARO_PIN Pin_13 + +#define INVERTER_PIN Pin_2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_GPIO GPIOB +#define INVERTER_PERIPHERAL RCC_APB2Periph_GPIOB +#define INVERTER_USART USART2 + +#define GYRO +#define ACC +#define MAG +#define BARO +#define SONAR +#define BEEPER +#define LED0 +#define LED1 +#define INVERTER +#define DISPLAY + +#define USE_USART1 +#define USE_USART2 +#define USE_SOFT_SERIAL +#define SERIAL_PORT_COUNT 4 + + +#define I2C_DEVICE (I2CDEV_2) + +// #define SOFT_I2C // enable to test software i2c +// #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_BARO | SENSOR_MAG) + +#define GPS + +#define LED_STRIP +#define TELEMETRY +#define SOFT_SERIAL +#define SERIAL_RX +#define AUTOTUNE