Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Saving files before refreshing line endings

Browse source
This commit is contained in:
Dominic Clifton 2014-09-15 23:38:51 +01:00
parent 876cf6bdd7
commit 4237370b60
3 changed files with 724 additions and 724 deletions
Download patch file Download diff file Expand all files Collapse all files

426
src/main/common/printf.c
View file

@ -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 <stdbool.h>
#include <stdint.h>
#include <stdarg.h>
#include <stdlib.h>
#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 <stdbool.h>
#include <stdint.h>
#include <stdarg.h>
#include <stdlib.h>
#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;
}

878
src/main/sensors/initialisation.c
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#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;
}

144
src/main/target/NAZE/target.h
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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 <http://www.gnu.org/licenses/>.
*/
#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