1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
This commit is contained in:
Nicholas Sherlock 2014-12-26 11:56:00 +13:00
commit dfd8e5b2b0
33 changed files with 601 additions and 603 deletions

23
.travis.yml Normal file
View file

@ -0,0 +1,23 @@
env:
- TARGET=CC3D
- TARGET=CHEBUZZF3
- TARGET=CJMCU
- TARGET=EUSTM32F103RC
- TARGET=MASSIVEF3
- TARGET=NAZE
- TARGET=NAZE32PRO
- TARGET=OLIMEXINO
- TARGET=PORT103R
- TARGET=SPARKY
- TARGET=STM32F3DISCOVERY
language: c
compiler: arm-none-eabi-gcc
before_install: sudo add-apt-repository -y ppa:terry.guo/gcc-arm-embedded && sudo apt-get update
install: sudo apt-get install build-essential gcc-arm-none-eabi git
before_script: $CC --version
script: make -j2
notifications:
irc: "chat.freenode.net#cleanflight"
use_notice: true
skip_join: true

View file

@ -190,6 +190,7 @@ COMMON_SRC = build_config.c \
rx/sumd.c \
rx/sumh.c \
rx/spektrum.c \
rx/xbus.c \
sensors/acceleration.c \
sensors/battery.c \
sensors/boardalignment.c \
@ -414,6 +415,8 @@ STM32F3DISCOVERY_SRC = $(STM32F3DISCOVERY_COMMON_SRC) \
drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \
drivers/accgyro_l3g4200d.c \
drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \
$(HIGHEND_SRC) \
$(COMMON_SRC)
@ -427,7 +430,7 @@ MASSIVEF3_SRC = $(STM32F3DISCOVERY_SRC) \
SPARKY_SRC = $(STM32F30x_COMMON_SRC) \
drivers/display_ug2864hsweg01.c \
drivers/accgyro_mpu9150.c \
drivers/accgyro_mpu6050.c \
drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \
$(HIGHEND_SRC) \

View file

@ -47,7 +47,6 @@ http://www.multiwii.com/forum/viewtopic.php?f=23&t=5149
See: https://github.com/cleanflight/cleanflight/blob/master/docs/Installation.md
## Documentation
There is lots of documentation here: https://github.com/cleanflight/cleanflight/tree/master/docs
@ -62,8 +61,9 @@ irc://irc.freenode.net/#cleanflight
If you are using windows and don't have an IRC client installed then take a look at HydraIRC - here: http://hydrairc.com/
## Videos
Etiquette: Don't ask to ask and please wait around long enough for a reply - sometimes people are out flying, asleep or at work and can't answer immediately.
## Videos
There is a dedicated Cleanflight youtube channel which has progress update videos, flight demonstrations, instrutions and other related videos.
@ -83,7 +83,26 @@ https://github.com/hydra/cleanflight-configurator
## Contributing
Before making any contributions, take a note of the https://github.com/multiwii/baseflight/wiki/CodingStyle
Contributions are welcome and encouraged. You can contibute in many ways:
* Documentation updates and corrections.
* How-To guides - received help? help others!
* Bug fixes.
* New features.
* Telling us your ideas and suggestions.
The best place to start is the IRC channel on freenode (see above), drop in, say hi. Next place is the github issue tracker:
https://github.com/cleanflight/cleanflight/issues
https://github.com/cleanflight/cleanflight-configurator/issues
Before creating new issues please check to see if there is an existing one, search first otherwise you waste peoples time when they could be coding instead!
## Developers
There is a developers section in the `docs/development` folder.
Before making any code contributions, take a note of the https://github.com/multiwii/baseflight/wiki/CodingStyle
For this fork it is also advised to read about clean code, here are some useful links:
@ -92,3 +111,11 @@ For this fork it is also advised to read about clean code, here are some useful
* http://en.wikipedia.org/wiki/Code_smell
* http://en.wikipedia.org/wiki/Code_refactoring
* http://www.amazon.co.uk/Working-Effectively-Legacy-Robert-Martin/dp/0131177052
TravisCI is also used to run automatic builds
https://travis-ci.org/cleanflight/cleanflight
[![Build Status](https://travis-ci.org/cleanflight/cleanflight.svg?branch=master)](https://travis-ci.org/cleanflight/cleanflight)

View file

@ -83,7 +83,7 @@ This will immediatly "break" communication to the GPS. Since you haven't saved t
Click on `PRT` in the Configuration view again and inspect the packet console to make sure messages are being sent and acknowledged.
Next, to ensure the FC doesn't waste time processing messages it does not need you must disable all messages on except:
Next, to ensure the FC doesn't waste time processing unneeded messages, click on `MSG` and enable the following on UART1 alone with a rate of 1. When changing message target and rates remember to click `Send` after changing each message.:
NAV-POSLLH
NAV-DOP
@ -91,13 +91,11 @@ Next, to ensure the FC doesn't waste time processing messages it does not need y
NAV-VELNED
NAV-TIMEUTC
The above messages should each be enabled with a rate of `1`.
Enable the following on UART1 with a rate of 5, to reduce bandwidth and load on the FC.
NAV-SVINFO
The above messages should each be enabled with a rate of `5` to reduce bandwidth and load on the FC.
When changing message target and rates remember to click `Send` after changing each message.
All other message types should be disabled.
Next change the global update rate, click `Rate (Rates)` in the Configuration view.

View file

@ -22,6 +22,21 @@ Allows you to use MSP commands as the RC input. Only 8 channel support to maint
16 channels via serial currently supported.
## XBus
The firmware currently supports the MODE B version of the XBus protocol.
Make sure to set your TX to use "MODE B" for XBUS in the TX menus!
See here for info on JR's XBus protocol: http://www.jrpropo.com/english/propo/XBus/
Tested hardware: JR XG14 + RG731BX with NAZE32 (rev4)
With the current CLI configuration:
`set serialrx_provider=5`
`set serial_port_2_scenario=3`
`feature RX_SERIAL`
This will set the FW to use serial RX, with XBUS_MODE_B as provider and finally the scenario to be used for serial port 2.
Please note that your config may vary depending on hw used.
### OpenTX configuration
If using OpenTX set the transmitter module to D16 mode and select CH1-16 on the transmitter before binding to allow reception
@ -53,6 +68,8 @@ For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` cli setting as
| SBUS | 2 |
| SUMD | 3 |
| SUMH | 4 |
| XBUS_MODE_B | 5 |
#### PPM/PWM input filtering.

View file

@ -309,7 +309,7 @@ static void resetConf(void)
setControlRateProfile(0);
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.mixerConfiguration = MULTITYPE_QUADX;
masterConfig.mixerMode = MIXER_QUADX;
featureClearAll();
#if defined(CJMCU) || defined(SPARKY)
featureSet(FEATURE_RX_PPM);

View file

@ -23,7 +23,7 @@ typedef struct master_t {
uint16_t size;
uint8_t magic_be; // magic number, should be 0xBE
uint8_t mixerConfiguration;
uint8_t mixerMode;
uint32_t enabledFeatures;
uint16_t looptime; // imu loop time in us
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band

View file

@ -33,10 +33,10 @@
extern int16_t debug[4];
// Address
// Addresses (7 bit address format)
#define LSM303DLHC_ACCEL_ADDRESS 0x32
#define LSM303DLHC_MAG_ADDRESS 0x3C
#define LSM303DLHC_ACCEL_ADDRESS 0x19
#define LSM303DLHC_MAG_ADDRESS 0x1E
// Registers

View file

@ -1,354 +0,0 @@
/*
* 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/>.
*/
// NOTE: this file is a copy of the mpu6050 driver with very minimal changes.
// some de-duplication needs to occur...
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "common/maths.h"
#include "system.h"
#include "gpio.h"
#include "bus_i2c.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu9150.h"
#define MPU9150_ADDRESS 0xD0 // (204) 1101000 // See http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf, section 6.5.
#define DMP_MEM_START_ADDR 0x6E
#define DMP_MEM_R_W 0x6F
#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU_RA_XA_OFFS_L_TC 0x07
#define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU_RA_YA_OFFS_L_TC 0x09
#define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU_RA_ZA_OFFS_L_TC 0x0B
#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register
#define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU_RA_XG_OFFS_USRL 0x14
#define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU_RA_YG_OFFS_USRL 0x16
#define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU_RA_ZG_OFFS_USRL 0x18
#define MPU_RA_SMPLRT_DIV 0x19
#define MPU_RA_CONFIG 0x1A
#define MPU_RA_GYRO_CONFIG 0x1B
#define MPU_RA_ACCEL_CONFIG 0x1C
#define MPU_RA_FF_THR 0x1D
#define MPU_RA_FF_DUR 0x1E
#define MPU_RA_MOT_THR 0x1F
#define MPU_RA_MOT_DUR 0x20
#define MPU_RA_ZRMOT_THR 0x21
#define MPU_RA_ZRMOT_DUR 0x22
#define MPU_RA_FIFO_EN 0x23
#define MPU_RA_I2C_MST_CTRL 0x24
#define MPU_RA_I2C_SLV0_ADDR 0x25
#define MPU_RA_I2C_SLV0_REG 0x26
#define MPU_RA_I2C_SLV0_CTRL 0x27
#define MPU_RA_I2C_SLV1_ADDR 0x28
#define MPU_RA_I2C_SLV1_REG 0x29
#define MPU_RA_I2C_SLV1_CTRL 0x2A
#define MPU_RA_I2C_SLV2_ADDR 0x2B
#define MPU_RA_I2C_SLV2_REG 0x2C
#define MPU_RA_I2C_SLV2_CTRL 0x2D
#define MPU_RA_I2C_SLV3_ADDR 0x2E
#define MPU_RA_I2C_SLV3_REG 0x2F
#define MPU_RA_I2C_SLV3_CTRL 0x30
#define MPU_RA_I2C_SLV4_ADDR 0x31
#define MPU_RA_I2C_SLV4_REG 0x32
#define MPU_RA_I2C_SLV4_DO 0x33
#define MPU_RA_I2C_SLV4_CTRL 0x34
#define MPU_RA_I2C_SLV4_DI 0x35
#define MPU_RA_I2C_MST_STATUS 0x36
#define MPU_RA_INT_PIN_CFG 0x37
#define MPU_RA_INT_ENABLE 0x38
#define MPU_RA_DMP_INT_STATUS 0x39
#define MPU_RA_INT_STATUS 0x3A
#define MPU_RA_ACCEL_XOUT_H 0x3B
#define MPU_RA_ACCEL_XOUT_L 0x3C
#define MPU_RA_ACCEL_YOUT_H 0x3D
#define MPU_RA_ACCEL_YOUT_L 0x3E
#define MPU_RA_ACCEL_ZOUT_H 0x3F
#define MPU_RA_ACCEL_ZOUT_L 0x40
#define MPU_RA_TEMP_OUT_H 0x41
#define MPU_RA_TEMP_OUT_L 0x42
#define MPU_RA_GYRO_XOUT_H 0x43
#define MPU_RA_GYRO_XOUT_L 0x44
#define MPU_RA_GYRO_YOUT_H 0x45
#define MPU_RA_GYRO_YOUT_L 0x46
#define MPU_RA_GYRO_ZOUT_H 0x47
#define MPU_RA_GYRO_ZOUT_L 0x48
#define MPU_RA_EXT_SENS_DATA_00 0x49
#define MPU_RA_MOT_DETECT_STATUS 0x61
#define MPU_RA_I2C_SLV0_DO 0x63
#define MPU_RA_I2C_SLV1_DO 0x64
#define MPU_RA_I2C_SLV2_DO 0x65
#define MPU_RA_I2C_SLV3_DO 0x66
#define MPU_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU_RA_SIGNAL_PATH_RESET 0x68
#define MPU_RA_MOT_DETECT_CTRL 0x69
#define MPU_RA_USER_CTRL 0x6A
#define MPU_RA_PWR_MGMT_1 0x6B
#define MPU_RA_PWR_MGMT_2 0x6C
#define MPU_RA_BANK_SEL 0x6D
#define MPU_RA_MEM_START_ADDR 0x6E
#define MPU_RA_MEM_R_W 0x6F
#define MPU_RA_DMP_CFG_1 0x70
#define MPU_RA_DMP_CFG_2 0x71
#define MPU_RA_FIFO_COUNTH 0x72
#define MPU_RA_FIFO_COUNTL 0x73
#define MPU_RA_FIFO_R_W 0x74
#define MPU_RA_WHO_AM_I 0x75
#define MPU9150_SMPLRT_DIV 0 // 8000Hz
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
NUM_GYRO_FSR
};
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
NUM_ACCEL_FSR
};
static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu9150AccInit(void);
static void mpu9150AccRead(int16_t *accData);
static void mpu9150GyroInit(void);
static void mpu9150GyroRead(int16_t *gyroData);
typedef enum {
MPU_9150_HALF_RESOLUTION,
MPU_9150_FULL_RESOLUTION
} mpu9150Resolution_e;
static mpu9150Resolution_e mpuAccelTrim;
static const mpu9150Config_t *mpu9150Config = NULL;
void mpu9150GpioInit(void) {
gpio_config_t gpio;
if (mpu9150Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(mpu9150Config->gpioAPB2Peripherals, ENABLE);
}
gpio.pin = mpu9150Config->gpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(mpu9150Config->gpioPort, &gpio);
}
static bool mpu9150Detect(void)
{
bool ack;
uint8_t sig;
ack = i2cRead(MPU9150_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
if (!ack) {
return false;
}
// So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
// The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0<58>s 7-bit I2C address.
// The least significant bit of the MPU-60X0<58>s I2C address is determined by the value of the AD0 pin. (we know that already).
// But here's the best part: The value of the AD0 pin is not reflected in this register.
return true;
if (sig != (MPU9150_ADDRESS & 0x7e))
return false;
return true;
}
bool mpu9150AccDetect(const mpu9150Config_t *configToUse, acc_t *acc)
{
uint8_t readBuffer[6];
uint8_t revision;
uint8_t productId;
mpu9150Config = configToUse;
if (!mpu9150Detect()) {
return false;
}
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
i2cRead(MPU9150_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
if (revision == 1) {
mpuAccelTrim = MPU_9150_HALF_RESOLUTION;
} else if (revision == 2) {
mpuAccelTrim = MPU_9150_FULL_RESOLUTION;
} else {
failureMode(5);
}
} else {
i2cRead(MPU9150_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F;
if (!revision) {
failureMode(5);
} else if (revision == 4) {
mpuAccelTrim = MPU_9150_HALF_RESOLUTION;
} else {
mpuAccelTrim = MPU_9150_FULL_RESOLUTION;
}
}
acc->init = mpu9150AccInit;
acc->read = mpu9150AccRead;
acc->revisionCode = (mpuAccelTrim == MPU_9150_HALF_RESOLUTION ? 'o' : 'n');
return true;
}
bool mpu9150GyroDetect(const mpu9150Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
{
mpu9150Config = configToUse;
if (!mpu9150Detect()) {
return false;
}
gyro->init = mpu9150GyroInit;
gyro->read = mpu9150GyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = INV_FILTER_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else
mpuLowPassFilter = INV_FILTER_5HZ;
return true;
}
static void mpu9150AccInit(void)
{
if (mpu9150Config) {
mpu9150GpioInit();
mpu9150Config = NULL; // avoid re-initialisation of GPIO;
}
switch (mpuAccelTrim) {
case MPU_9150_HALF_RESOLUTION:
acc_1G = 256 * 8;
break;
case MPU_9150_FULL_RESOLUTION:
acc_1G = 512 * 8;
break;
}
}
static void mpu9150AccRead(int16_t *accData)
{
uint8_t buf[6];
if (!i2cRead(MPU9150_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf)) {
return;
}
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
}
static void mpu9150GyroInit(void)
{
if (mpu9150Config) {
mpu9150GpioInit();
mpu9150Config = NULL; // avoid re-initialisation of GPIO;
}
i2cWrite(MPU9150_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
i2cWrite(MPU9150_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
i2cWrite(MPU9150_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
i2cWrite(MPU9150_ADDRESS, MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
i2cWrite(MPU9150_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
i2cWrite(MPU9150_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
i2cWrite(MPU9150_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
}
static void mpu9150GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
if (!i2cRead(MPU9150_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf)) {
return;
}
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
}

View file

@ -101,7 +101,7 @@ void i2cInitPort(I2C_TypeDef *I2Cx)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin = I2C1_SCL_PIN;
@ -197,6 +197,8 @@ uint16_t i2cGetErrorCounter(void)
bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data)
{
addr_ <<= 1;
/* Test on BUSY Flag */
i2cTimeout = I2C_LONG_TIMEOUT;
while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) {
@ -258,6 +260,8 @@ bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data)
bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf)
{
addr_ <<= 1;
/* Test on BUSY Flag */
i2cTimeout = I2C_LONG_TIMEOUT;
while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) {

View file

@ -20,6 +20,8 @@
#include <math.h>
#include "build_config.h"
#include "platform.h"
#include "common/axis.h"
@ -40,26 +42,30 @@
// This sensor is available in MPU-9150.
// AK8975, mag sensor address
#define AK8975_MAG_I2C_ADDRESS 0x0C
#define AK8975_MAG_ID_ADDRESS 0x00
#define AK8975_MAG_DATA_ADDRESS 0x03
#define AK8975_MAG_CONTROL_ADDRESS 0x0A
#define AK8975_MAG_I2C_ADDRESS 0x0C
// Registers
#define AK8975_MAG_REG_WHO_AM_I 0x00
#define AK8975_MAG_REG_INFO 0x01
#define AK8975_MAG_REG_STATUS1 0x02
#define AK8975_MAG_REG_HXL 0x03
#define AK8975_MAG_REG_HXH 0x04
#define AK8975_MAG_REG_HYL 0x05
#define AK8975_MAG_REG_HYH 0x06
#define AK8975_MAG_REG_HZL 0x07
#define AK8975_MAG_REG_HZH 0x08
#define AK8975_MAG_REG_STATUS2 0x09
#define AK8975_MAG_REG_CNTL 0x0a
#define AK8975_MAG_REG_ASCT 0x0c // self test
bool ak8975detect(mag_t *mag)
{
bool ack = false;
uint8_t sig = 0;
uint8_t ackCount = 0;
for (uint8_t address = 0; address < 0xff; address++) {
ack = i2cRead(address, AK8975_MAG_ID_ADDRESS, 1, &sig);
if (ack) {
ackCount++;
}
}
// device ID is in register 0 and is equal to 'H'
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_ID_ADDRESS, 1, &sig);
if (!ack || sig != 'H')
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
if (!ack || sig != 'H') // 0x48 / 01001000 / 'H'
return false;
mag->init = ak8975Init;
@ -70,14 +76,54 @@ bool ak8975detect(mag_t *mag)
void ak8975Init()
{
i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading
bool ack;
UNUSED(ack);
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00);
delay(20);
uint8_t status;
// Clear status registers
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
// Trigger first measurement
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01);
}
#define BIT_STATUS1_REG_DATA_READY (1 << 0)
#define BIT_STATUS2_REG_DATA_ERROR (1 << 2)
#define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3)
void ak8975Read(int16_t *magData)
{
bool ack;
UNUSED(ack);
uint8_t status;
uint8_t buf[6];
i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_DATA_ADDRESS, 6, buf);
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
return;
}
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
if (!ack) {
return;
}
if (status & BIT_STATUS2_REG_DATA_ERROR) {
return;
}
if (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW) {
return;
}
// align sensors to match MPU6050:
// x -> y
// y -> x
@ -86,5 +132,6 @@ void ak8975Read(int16_t *magData)
magData[Y] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading again
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
}

View file

@ -36,35 +36,21 @@
*
*/
static uint32_t last_measurement;
static volatile int32_t *distance_ptr = 0;
extern int16_t debug[4];
static uint32_t lastMeasurementAt;
static volatile int32_t measurement = -1;
static sonarHardware_t const *sonarHardware;
void ECHO_EXTI_IRQHandler(void)
{
static uint32_t timing_start;
uint32_t timing_stop;
if (digitalIn(GPIOB, sonarHardware->echo_pin) != 0) {
timing_start = micros();
} else {
timing_stop = micros();
if (timing_stop > timing_start) {
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance traveled.
//
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
int32_t distance = (timing_stop - timing_start) / 59;
// this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll)
if (distance > 300)
distance = -1;
if (distance_ptr) {
*distance_ptr = distance;
}
measurement = timing_stop - timing_start;
}
}
@ -115,30 +101,41 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware)
NVIC_EnableIRQ(sonarHardware->exti_irqn);
last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance()
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
}
// distance calculation is done asynchronously, using interrupt
void hcsr04_get_distance(volatile int32_t *distance)
// measurement reading is done asynchronously, using interrupt
void hcsr04_start_reading(void)
{
uint32_t current_time = millis();
uint32_t now = millis();
if (current_time < (last_measurement + 60)) {
if (now < (lastMeasurementAt + 60)) {
// the repeat interval of trig signal should be greater than 60ms
// to avoid interference between connective measurements.
return;
}
last_measurement = current_time;
distance_ptr = distance;
#if 0
debug[0] = *distance;
#endif
lastMeasurementAt = now;
digitalHi(GPIOB, sonarHardware->trigger_pin);
// The width of trig signal must be greater than 10us
delayMicroseconds(11);
digitalLo(GPIOB, sonarHardware->trigger_pin);
}
int32_t hcsr04_get_distance(void)
{
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance traveled.
//
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
int32_t distance = measurement / 59;
// this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll)
if (distance > 300)
distance = -1;
return distance;
}
#endif

View file

@ -27,4 +27,5 @@ typedef struct sonarHardware_s {
void hcsr04_init(const sonarHardware_t *sonarHardware);
void hcsr04_get_distance(volatile int32_t *distance);
void hcsr04_start_reading(void);
int32_t hcsr04_get_distance(void);

View file

@ -46,6 +46,8 @@
#include "config/runtime_config.h"
extern int16_t debug[4];
int32_t setVelocity = 0;
uint8_t velocityControl = 0;
int32_t errorVelocityI = 0;
@ -258,6 +260,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
#ifdef SONAR
tiltAngle = calculateTiltAngle(&inclination);
sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
#endif

View file

@ -103,7 +103,7 @@ void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
}
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration)
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
{
static int16_t gyroYawSmooth = 0;
@ -120,7 +120,7 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfigurat
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
if (mixerConfiguration == MULTITYPE_TRI) {
if (mixerMode == MIXER_TRI) {
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
gyroYawSmooth = gyroData[FD_YAW];
} else {

View file

@ -33,7 +33,7 @@ typedef struct imuRuntimeConfig_s {
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
void calculateEstimatedAltitude(uint32_t currentTime);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);

View file

@ -42,6 +42,9 @@
#include "config/runtime_config.h"
#include "config/config.h"
#define GIMBAL_SERVO_PITCH 0
#define GIMBAL_SERVO_ROLL 1
#define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
uint8_t motorCount = 0;
@ -62,7 +65,7 @@ static rxConfig_t *rxConfig;
static gimbalConfig_t *gimbalConfig;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static MultiType currentMixerConfiguration;
static mixerMode_e currentMixerMode;
static const motorMixer_t mixerQuadX[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
@ -70,8 +73,7 @@ static const motorMixer_t mixerQuadX[] = {
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
};
#ifndef CJMCU
#ifndef USE_QUAD_MIXER_ONLY
static const motorMixer_t mixerTri[] = {
{ 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
{ 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT
@ -189,29 +191,29 @@ static const motorMixer_t mixerDualcopter[] = {
const mixer_t mixers[] = {
// Mo Se Mixtable
{ 0, 0, NULL }, // entry 0
{ 3, 1, mixerTri }, // MULTITYPE_TRI
{ 4, 0, mixerQuadP }, // MULTITYPE_QUADP
{ 4, 0, mixerQuadX }, // MULTITYPE_QUADX
{ 2, 1, mixerBi }, // MULTITYPE_BI
{ 0, 1, NULL }, // * MULTITYPE_GIMBAL
{ 6, 0, mixerY6 }, // MULTITYPE_Y6
{ 6, 0, mixerHex6P }, // MULTITYPE_HEX6
{ 1, 1, NULL }, // * MULTITYPE_FLYING_WING
{ 4, 0, mixerY4 }, // MULTITYPE_Y4
{ 6, 0, mixerHex6X }, // MULTITYPE_HEX6X
{ 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8
{ 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP
{ 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX
{ 1, 1, NULL }, // * MULTITYPE_AIRPLANE
{ 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM
{ 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG
{ 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4
{ 6, 0, mixerHex6H }, // MULTITYPE_HEX6H
{ 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO
{ 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER
{ 1, 1, NULL }, // MULTITYPE_SINGLECOPTER
{ 4, 0, mixerAtail4 }, // MULTITYPE_ATAIL4
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
{ 3, 1, mixerTri }, // MIXER_TRI
{ 4, 0, mixerQuadP }, // MIXER_QUADP
{ 4, 0, mixerQuadX }, // MIXER_QUADX
{ 2, 1, mixerBi }, // MIXER_BI
{ 0, 1, NULL }, // * MIXER_GIMBAL
{ 6, 0, mixerY6 }, // MIXER_Y6
{ 6, 0, mixerHex6P }, // MIXER_HEX6
{ 1, 1, NULL }, // * MIXER_FLYING_WING
{ 4, 0, mixerY4 }, // MIXER_Y4
{ 6, 0, mixerHex6X }, // MIXER_HEX6X
{ 8, 0, mixerOctoX8 }, // MIXER_OCTOX8
{ 8, 0, mixerOctoFlatP }, // MIXER_OCTOFLATP
{ 8, 0, mixerOctoFlatX }, // MIXER_OCTOFLATX
{ 1, 1, NULL }, // * MIXER_AIRPLANE
{ 0, 1, NULL }, // * MIXER_HELI_120_CCPM
{ 0, 1, NULL }, // * MIXER_HELI_90_DEG
{ 4, 0, mixerVtail4 }, // MIXER_VTAIL4
{ 6, 0, mixerHex6H }, // MIXER_HEX6H
{ 0, 1, NULL }, // * MIXER_PPM_TO_SERVO
{ 2, 1, mixerDualcopter }, // MIXER_DUALCOPTER
{ 1, 1, NULL }, // MIXER_SINGLECOPTER
{ 4, 0, mixerAtail4 }, // MIXER_ATAIL4
{ 0, 0, NULL }, // MIXER_CUSTOM
};
#endif
@ -258,15 +260,15 @@ int servoDirection(int nr, int lr)
static motorMixer_t *customMixers;
#ifndef CJMCU
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
#ifndef USE_QUAD_MIXER_ONLY
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
{
currentMixerConfiguration = mixerConfiguration;
currentMixerMode = mixerMode;
customMixers = initialCustomMixers;
// enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[currentMixerConfiguration].useServo;
useServo = mixers[currentMixerMode].useServo;
// if we want camstab/trig, that also enables servos, even if mixer doesn't
if (feature(FEATURE_SERVO_TILT))
useServo = 1;
@ -278,7 +280,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
servoCount = pwmOutputConfiguration->servoCount;
if (currentMixerConfiguration == MULTITYPE_CUSTOM) {
if (currentMixerMode == MIXER_CUSTOM) {
// load custom mixer into currentMixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
@ -288,11 +290,11 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
motorCount++;
}
} else {
motorCount = mixers[currentMixerConfiguration].motorCount;
motorCount = mixers[currentMixerMode].motorCount;
// copy motor-based mixers
if (mixers[currentMixerConfiguration].motor) {
if (mixers[currentMixerMode].motor) {
for (i = 0; i < motorCount; i++)
currentMixer[i] = mixers[currentMixerConfiguration].motor[i];
currentMixer[i] = mixers[currentMixerMode].motor[i];
}
}
@ -308,19 +310,37 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
}
// set flag that we're on something with wings
if (currentMixerConfiguration == MULTITYPE_FLYING_WING ||
currentMixerConfiguration == MULTITYPE_AIRPLANE)
if (currentMixerMode == MIXER_FLYING_WING ||
currentMixerMode == MIXER_AIRPLANE)
ENABLE_STATE(FIXED_WING);
else
DISABLE_STATE(FIXED_WING);
mixerResetMotors();
}
void mixerLoadMix(int index, motorMixer_t *customMixers)
{
int i;
// we're 1-based
index++;
// clear existing
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
customMixers[i].throttle = 0.0f;
// do we have anything here to begin with?
if (mixers[index].motor != NULL) {
for (i = 0; i < mixers[index].motorCount; i++)
customMixers[i] = mixers[index].motor[i];
}
}
#else
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
{
currentMixerConfiguration = mixerConfiguration;
currentMixerMode = mixerMode;
customMixers = initialCustomMixers;
@ -342,7 +362,6 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
}
#endif
void mixerResetMotors(void)
{
int i;
@ -351,25 +370,6 @@ void mixerResetMotors(void)
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
}
#ifndef CJMCU
void mixerLoadMix(int index, motorMixer_t *customMixers)
{
int i;
// we're 1-based
index++;
// clear existing
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
customMixers[i].throttle = 0.0f;
// do we have anything here to begin with?
if (mixers[index].motor != NULL) {
for (i = 0; i < mixers[index].motorCount; i++)
customMixers[i] = mixers[index].motor[i];
}
}
#endif
static void updateGimbalServos(void)
{
pwmWriteServo(0, servo[0]);
@ -381,13 +381,13 @@ void writeServos(void)
if (!useServo)
return;
switch (currentMixerConfiguration) {
case MULTITYPE_BI:
switch (currentMixerMode) {
case MIXER_BI:
pwmWriteServo(0, servo[4]);
pwmWriteServo(1, servo[5]);
break;
case MULTITYPE_TRI:
case MIXER_TRI:
if (mixerConfig->tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(0, servo[5]);
@ -400,22 +400,22 @@ void writeServos(void)
}
break;
case MULTITYPE_FLYING_WING:
case MIXER_FLYING_WING:
pwmWriteServo(0, servo[3]);
pwmWriteServo(1, servo[4]);
break;
case MULTITYPE_GIMBAL:
case MIXER_GIMBAL:
updateGimbalServos();
break;
case MULTITYPE_DUALCOPTER:
case MIXER_DUALCOPTER:
pwmWriteServo(0, servo[4]);
pwmWriteServo(1, servo[5]);
break;
case MULTITYPE_AIRPLANE:
case MULTITYPE_SINGLECOPTER:
case MIXER_AIRPLANE:
case MIXER_SINGLECOPTER:
pwmWriteServo(0, servo[3]);
pwmWriteServo(1, servo[4]);
pwmWriteServo(2, servo[5]);
@ -454,6 +454,7 @@ void writeAllMotors(int16_t mc)
writeMotors();
}
#ifndef USE_QUAD_MIXER_ONLY
static void airplaneMixer(void)
{
int16_t flapperons[2] = { 0, 0 };
@ -499,6 +500,7 @@ static void airplaneMixer(void)
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}
}
#endif
void mixTable(void)
{
@ -513,29 +515,34 @@ void mixTable(void)
// motors for non-servo mixes
if (motorCount > 1)
for (i = 0; i < motorCount; i++)
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
#ifndef USE_QUAD_MIXER_ONLY
// airplane / servo mixes
switch (currentMixerConfiguration) {
case MULTITYPE_BI:
switch (currentMixerMode) {
case MIXER_BI:
servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4); // LEFT
servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5); // RIGHT
break;
case MULTITYPE_TRI:
case MIXER_TRI:
servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(5); // REAR
break;
case MULTITYPE_GIMBAL:
servo[0] = (((int32_t)servoConf[0].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
servo[1] = (((int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
case MIXER_GIMBAL:
servo[GIMBAL_SERVO_PITCH] = (((int32_t)servoConf[GIMBAL_SERVO_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(GIMBAL_SERVO_PITCH);
servo[GIMBAL_SERVO_ROLL] = (((int32_t)servoConf[GIMBAL_SERVO_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(GIMBAL_SERVO_ROLL);
break;
case MULTITYPE_AIRPLANE:
case MIXER_AIRPLANE:
airplaneMixer();
break;
case MULTITYPE_FLYING_WING:
case MIXER_FLYING_WING:
if (!ARMING_FLAG(ARMED))
servo[7] = escAndServoConfig->mincommand;
else
@ -554,14 +561,14 @@ void mixTable(void)
servo[4] += determineServoMiddleOrForwardFromChannel(4);
break;
case MULTITYPE_DUALCOPTER:
case MIXER_DUALCOPTER:
for (i = 4; i < 6; i++) {
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}
break;
case MULTITYPE_SINGLECOPTER:
case MIXER_SINGLECOPTER:
for (i = 3; i < 7; i++) {
servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
servo[i] += determineServoMiddleOrForwardFromChannel(i);
@ -572,6 +579,7 @@ void mixTable(void)
default:
break;
}
#endif
// do camstab
if (feature(FEATURE_SERVO_TILT)) {

View file

@ -20,34 +20,33 @@
#define MAX_SUPPORTED_MOTORS 12
#define MAX_SUPPORTED_SERVOS 8
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
typedef enum MultiType
// Note: this is called MultiType/MULTITYPE_* in baseflight.
typedef enum mixerMode
{
MULTITYPE_TRI = 1,
MULTITYPE_QUADP = 2,
MULTITYPE_QUADX = 3,
MULTITYPE_BI = 4,
MULTITYPE_GIMBAL = 5,
MULTITYPE_Y6 = 6,
MULTITYPE_HEX6 = 7,
MULTITYPE_FLYING_WING = 8,
MULTITYPE_Y4 = 9,
MULTITYPE_HEX6X = 10,
MULTITYPE_OCTOX8 = 11, // Java GUI is same for the next 3 configs
MULTITYPE_OCTOFLATP = 12, // MultiWinGui shows this differently
MULTITYPE_OCTOFLATX = 13, // MultiWinGui shows this differently
MULTITYPE_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported)
MULTITYPE_HELI_120_CCPM = 15,
MULTITYPE_HELI_90_DEG = 16,
MULTITYPE_VTAIL4 = 17,
MULTITYPE_HEX6H = 18,
MULTITYPE_PPM_TO_SERVO = 19, // PPM -> servo relay
MULTITYPE_DUALCOPTER = 20,
MULTITYPE_SINGLECOPTER = 21,
MULTITYPE_ATAIL4 = 22,
MULTITYPE_CUSTOM = 23,
MULTITYPE_LAST = 24
} MultiType;
MIXER_TRI = 1,
MIXER_QUADP = 2,
MIXER_QUADX = 3,
MIXER_BI = 4,
MIXER_GIMBAL = 5,
MIXER_Y6 = 6,
MIXER_HEX6 = 7,
MIXER_FLYING_WING = 8,
MIXER_Y4 = 9,
MIXER_HEX6X = 10,
MIXER_OCTOX8 = 11,
MIXER_OCTOFLATP = 12,
MIXER_OCTOFLATX = 13,
MIXER_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported)
MIXER_HELI_120_CCPM = 15,
MIXER_HELI_90_DEG = 16,
MIXER_VTAIL4 = 17,
MIXER_HEX6H = 18,
MIXER_PPM_TO_SERVO = 19, // PPM -> servo relay
MIXER_DUALCOPTER = 20,
MIXER_SINGLECOPTER = 21,
MIXER_ATAIL4 = 22,
MIXER_CUSTOM = 23
} mixerMode_e;
// Custom mixer data per motor
typedef struct motorMixer_t {

View file

@ -74,8 +74,10 @@
#include "serial_cli.h"
// we unset this on 'exit'
extern uint8_t cliMode;
extern uint16_t cycleTime; // FIXME dependency on mw.c
static serialPort_t *cliPort;
static void cliAux(char *cmdline);
static void cliAdjustmentRange(char *cmdline);
static void cliCMix(char *cmdline);
@ -83,18 +85,6 @@ static void cliDefaults(char *cmdline);
static void cliDump(char *cmdLine);
static void cliExit(char *cmdline);
static void cliFeature(char *cmdline);
#ifdef GPS
static void cliGpsPassthrough(char *cmdline);
#endif
static void cliHelp(char *cmdline);
static void cliMap(char *cmdline);
#ifdef LED_STRIP
static void cliLed(char *cmdline);
static void cliColor(char *cmdline);
#endif
#ifndef CJMCU
static void cliMixer(char *cmdline);
#endif
static void cliMotor(char *cmdline);
static void cliProfile(char *cmdline);
static void cliRateProfile(char *cmdline);
@ -105,9 +95,21 @@ static void cliGet(char *cmdline);
static void cliStatus(char *cmdline);
static void cliVersion(char *cmdline);
extern uint16_t cycleTime; // FIXME dependency on mw.c
#ifdef GPS
static void cliGpsPassthrough(char *cmdline);
#endif
static serialPort_t *cliPort;
static void cliHelp(char *cmdline);
static void cliMap(char *cmdline);
#ifdef LED_STRIP
static void cliLed(char *cmdline);
static void cliColor(char *cmdline);
#endif
#ifndef USE_QUAD_MIXER_ONLY
static void cliMixer(char *cmdline);
#endif
// signal that we're in cli mode
uint8_t cliMode = 0;
@ -116,7 +118,8 @@ uint8_t cliMode = 0;
static char cliBuffer[48];
static uint32_t bufferIndex = 0;
// sync this with mutiType_e
#ifndef USE_QUAD_MIXER_ONLY
// sync this with mixerMode_e
static const char * const mixerNames[] = {
"TRI", "QUADP", "QUADX", "BI",
"GIMBAL", "Y6", "HEX6",
@ -125,6 +128,7 @@ static const char * const mixerNames[] = {
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
"ATAIL4", "CUSTOM", NULL
};
#endif
// sync this with features_e
static const char * const featureNames[] = {
@ -141,7 +145,7 @@ static const char * const sensorNames[] = {
};
static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9150", "FAKE", "None", NULL
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL
};
typedef struct {
@ -171,7 +175,7 @@ const clicmd_t cmdTable[] = {
{ "led", "configure leds", cliLed },
#endif
{ "map", "mapping of rc channel order", cliMap },
#ifndef CJMCU
#ifndef USE_QUAD_MIXER_ONLY
{ "mixer", "mixer name or list", cliMixer },
#endif
{ "motor", "get/set motor output value", cliMotor },
@ -577,7 +581,7 @@ static void cliAdjustmentRange(char *cmdline)
static void cliCMix(char *cmdline)
{
#ifdef CJMCU
#ifdef USE_QUAD_MIXER_ONLY
UNUSED(cmdline);
#else
int i, check = 0;
@ -757,9 +761,12 @@ static void cliDump(char *cmdline)
{
unsigned int i;
char buf[16];
float thr, roll, pitch, yaw;
uint32_t mask;
#ifndef USE_QUAD_MIXER_ONLY
float thr, roll, pitch, yaw;
#endif
uint8_t dumpMask = DUMP_ALL;
if (strcasecmp(cmdline, "master") == 0) {
dumpMask = DUMP_MASTER; // only
@ -779,7 +786,8 @@ static void cliDump(char *cmdline)
printf("\r\n# dump master\r\n");
printf("\r\n# mixer\r\n");
printf("mixer %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
#ifndef USE_QUAD_MIXER_ONLY
printf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
if (masterConfig.customMixer[0].throttle != 0.0f) {
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@ -805,6 +813,7 @@ static void cliDump(char *cmdline)
}
printf("cmix %d 0 0 0 0\r\n", i + 1);
}
#endif
printf("\r\n\r\n# feature\r\n");
@ -1018,7 +1027,7 @@ static void cliMap(char *cmdline)
printf("%s\r\n", out);
}
#ifndef CJMCU
#ifndef USE_QUAD_MIXER_ONLY
static void cliMixer(char *cmdline)
{
int i;
@ -1027,7 +1036,7 @@ static void cliMixer(char *cmdline)
len = strlen(cmdline);
if (len == 0) {
printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
return;
} else if (strncasecmp(cmdline, "list", len) == 0) {
cliPrint("Available mixers: ");
@ -1046,7 +1055,7 @@ static void cliMixer(char *cmdline)
break;
}
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
masterConfig.mixerConfiguration = i + 1;
masterConfig.mixerMode = i + 1;
printf("Mixer set to %s\r\n", mixerNames[i]);
break;
}

View file

@ -225,7 +225,7 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
//
// DEPRECATED - See MSP_API_VERSION and MSP_MIXER
#define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable
#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
@ -616,7 +616,7 @@ void mspInit(serialConfig_t *serialConfig)
}
#endif
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE)
if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE)
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
@ -711,7 +711,7 @@ static bool processOutCommand(uint8_t cmdMSP)
case MSP_IDENT:
headSerialReply(7);
serialize8(MW_VERSION);
serialize8(masterConfig.mixerConfiguration); // type of multicopter
serialize8(masterConfig.mixerMode);
serialize8(MSP_PROTOCOL_VERSION);
serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability"
break;
@ -1027,7 +1027,7 @@ static bool processOutCommand(uint8_t cmdMSP)
case MSP_MIXER:
headSerialReply(1);
serialize8(masterConfig.mixerConfiguration);
serialize8(masterConfig.mixerMode);
break;
case MSP_RX_CONFIG:
@ -1052,7 +1052,7 @@ static bool processOutCommand(uint8_t cmdMSP)
case MSP_CONFIG:
headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
serialize8(masterConfig.mixerConfiguration);
serialize8(masterConfig.mixerMode);
serialize32(featureMask());
@ -1340,9 +1340,11 @@ static bool processInCommand(void)
masterConfig.batteryConfig.currentMeterOffset = read16();
break;
#ifndef USE_QUAD_MIXER_ONLY
case MSP_SET_MIXER:
masterConfig.mixerConfiguration = read8();
masterConfig.mixerMode = read8();
break;
#endif
case MSP_SET_RX_CONFIG:
masterConfig.rxConfig.serialrx_provider = read8();
@ -1364,10 +1366,10 @@ static bool processInCommand(void)
case MSP_SET_CONFIG:
#ifdef CJMCU
masterConfig.mixerConfiguration = read8(); // multitype
#ifdef USE_QUAD_MIXER_ONLY
read8(); // mixerMode ignored
#else
read8(); // multitype
masterConfig.mixerMode = read8(); // mixerMode
#endif
featureClearAll();

View file

@ -97,7 +97,7 @@ void initTelemetry(void);
void serialInit(serialConfig_t *initialSerialConfig);
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void beepcodeInit(failsafe_t *initialFailsafe);
@ -265,7 +265,7 @@ void init(void)
LED1_OFF;
imuInit();
mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
#ifdef MAG
if (sensors(SENSOR_MAG))
@ -276,7 +276,7 @@ void init(void)
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING)
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
@ -350,7 +350,7 @@ void init(void)
previousTime = micros();
if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) {
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);

View file

@ -594,7 +594,7 @@ void processRx(void)
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
}
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
}
@ -647,7 +647,7 @@ void loop(void)
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime;
computeIMU(&currentProfile->accelerometerTrims, masterConfig.mixerConfiguration);
computeIMU(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
// Measure loop rate just after reading the sensors
currentTime = micros();

View file

@ -42,6 +42,7 @@
#include "rx/sumd.h"
#include "rx/sumh.h"
#include "rx/msp.h"
#include "rx/xbus.h"
#include "rx/rx.h"
@ -100,6 +101,9 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo
case SERIALRX_SUMH:
sumhUpdateSerialRxFunctionConstraint(functionConstraintToUpdate);
break;
case SERIALRX_XBUS_MODE_B:
xBusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate);
break;
}
}
#endif
@ -109,6 +113,7 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo
void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe)
{
uint8_t i;
useRxConfig(rxConfig);
for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
@ -152,6 +157,9 @@ void serialRxInit(rxConfig_t *rxConfig)
case SERIALRX_SUMH:
enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_XBUS_MODE_B:
enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
}
if (!enabled) {
@ -162,6 +170,7 @@ void serialRxInit(rxConfig_t *rxConfig)
bool isSerialRxFrameComplete(rxConfig_t *rxConfig)
{
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
@ -172,6 +181,8 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig)
return sumdFrameComplete();
case SERIALRX_SUMH:
return sumhFrameComplete();
case SERIALRX_XBUS_MODE_B:
return xBusFrameComplete();
}
return false;
}

View file

@ -32,7 +32,8 @@ typedef enum {
SERIALRX_SBUS = 2,
SERIALRX_SUMD = 3,
SERIALRX_SUMH = 4,
SERIALRX_PROVIDER_MAX = SERIALRX_SUMD
SERIALRX_XBUS_MODE_B = 5,
SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B
} SerialRXType;
#define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1)

207
src/main/rx/xbus.c Normal file
View file

@ -0,0 +1,207 @@
/*
* 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 <stdlib.h>
#include "platform.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h"
#include "rx/rx.h"
#include "rx/xbus.h"
//
// Serial driver for JR's XBus (MODE B) receiver
//
#define XBUS_CHANNEL_COUNT 12
// Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27
#define XBUS_FRAME_SIZE 27
#define XBUS_CRC_BYTE_1 25
#define XBUS_CRC_BYTE_2 26
#define XBUS_CRC_AND_VALUE 0x8000
#define XBUS_CRC_POLY 0x1021
#define XBUS_BAUDRATE 115200
#define XBUS_MAX_FRAME_TIME 5000
// NOTE!
// This is actually based on ID+LENGTH (nibble each)
// 0xA - Multiplex ID (also used by JR, no idea why)
// 0x1 - 12 channels
// 0x2 - 16 channels
// However, the JR XG14 that is used for test at the moment
// does only use 0xA1 as its output. This is why the implementation
// is based on these numbers only. Maybe update this in the future?
#define XBUS_START_OF_FRAME_BYTE (0xA1)
// Pulse length convertion from [0...4095] to µs:
// 800µs -> 0x000
// 1500µs -> 0x800
// 2200µs -> 0xFFF
// Total range is: 2200 - 800 = 1400 <==> 4095
// Use formula: 800 + value * 1400 / 4096 (i.e. a shift by 12)
#define XBUS_CONVERT_TO_USEC(V) (800 + ((V * 1400) >> 12))
static bool xBusFrameReceived = false;
static bool xBusDataIncoming = false;
static uint8_t xBusFramePosition;
static volatile uint8_t xBusFrame[XBUS_FRAME_SIZE];
static uint16_t xBusChannelData[XBUS_CHANNEL_COUNT];
static void xBusDataReceive(uint16_t c);
static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static serialPort_t *xBusPort;
void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint)
{
functionConstraint->minBaudRate = XBUS_BAUDRATE;
functionConstraint->maxBaudRate = XBUS_BAUDRATE;
functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK;
}
bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
{
switch (rxConfig->serialrx_provider) {
case SERIALRX_XBUS_MODE_B:
rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT;
xBusFrameReceived = false;
xBusDataIncoming = false;
xBusFramePosition = 0;
break;
default:
return false;
break;
}
xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED);
if (callback) {
*callback = xBusReadRawRC;
}
return xBusPort != NULL;
}
// The xbus mode B CRC calculations
static uint16_t xBusCRC16(uint16_t crc, uint8_t value)
{
uint8_t i;
crc = crc ^ (int16_t)value << 8;
for (i = 0; i < 8; i++) {
if (crc & XBUS_CRC_AND_VALUE) {
crc = crc << 1 ^ XBUS_CRC_POLY;
} else {
crc = crc << 1;
}
}
return crc;
}
static void xBusUnpackFrame(void)
{
// Calculate the CRC of the incoming frame
uint16_t crc = 0;
uint16_t inCrc = 0;
uint8_t i = 0;
uint16_t value;
uint8_t frameAddr;
// Calculate on all bytes except the final two CRC bytes
for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) {
inCrc = xBusCRC16(inCrc, xBusFrame[i]);
}
// Get the received CRC
crc = ((uint16_t)xBusFrame[XBUS_CRC_BYTE_1]) << 8;
crc = crc + ((uint16_t)xBusFrame[XBUS_CRC_BYTE_2]);
if (crc == inCrc) {
// Unpack the data, we have a valid frame
for (i = 0; i < XBUS_CHANNEL_COUNT; i++) {
frameAddr = 1 + i * 2;
value = ((uint16_t)xBusFrame[frameAddr]) << 8;
value = value + ((uint16_t)xBusFrame[frameAddr + 1]);
// Convert to internal format
xBusChannelData[i] = XBUS_CONVERT_TO_USEC(value);
}
xBusFrameReceived = true;
}
}
// Receive ISR callback
static void xBusDataReceive(uint16_t c)
{
// Check if we shall start a frame?
if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) {
xBusDataIncoming = true;
}
// Only do this if we are receiving to a frame
if (xBusDataIncoming == true) {
// Store in frame copy
xBusFrame[xBusFramePosition] = (uint8_t)c;
xBusFramePosition++;
}
// Done?
if (xBusFramePosition == XBUS_FRAME_SIZE) {
xBusUnpackFrame();
xBusDataIncoming = false;
xBusFramePosition = 0;
}
}
// Indicate time to read a frame from the data...
bool xBusFrameComplete(void)
{
return xBusFrameReceived;
}
static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{
uint16_t data;
// Mark frame as read
if (xBusFrameReceived) {
xBusFrameReceived = false;
}
// Deliver the data wanted
if (chan >= rxRuntimeConfig->channelCount) {
return 0;
}
data = xBusChannelData[chan];
return data;
}

View file

@ -17,13 +17,8 @@
#pragma once
typedef struct mpu9150Config_s {
uint32_t gpioAPB2Peripherals;
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
} mpu9150Config_t;
#include "rx/rx.h"
bool mpu9150AccDetect(const mpu9150Config_t *config,acc_t *acc);
bool mpu9150GyroDetect(const mpu9150Config_t *config, gyro_t *gyro, uint16_t lpf);
void mpu9150DmpLoop(void);
void mpu9150DmpResetFifo(void);
bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
bool xBusFrameComplete(void);
void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint);

View file

@ -27,9 +27,8 @@ typedef enum AccelSensors {
ACC_LSM303DLHC = 5,
ACC_SPI_MPU6000 = 6,
ACC_SPI_MPU6500 = 7,
ACC_MPU9150 = 8,
ACC_FAKE = 9,
ACC_NONE = 10
ACC_FAKE = 8,
ACC_NONE = 9
} accelSensor_e;
extern uint8_t accHardware;

View file

@ -33,8 +33,6 @@
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_mpu9150.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
@ -146,12 +144,6 @@ bool detectGyro(uint16_t gyroLpf)
}
#endif
#ifdef USE_GYRO_MPU9150
if (mpu9150GyroDetect(NULL, &gyro, gyroLpf)) {
return true;
}
#endif
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef NAZE
@ -258,15 +250,6 @@ retry:
}
; // fallthrough
#endif
#ifdef USE_ACC_MPU9150
case ACC_MPU9150: // MPU9150
if (mpu9150AccDetect(NULL, &acc)) {
accHardware = ACC_MPU9150;
if (accHardwareToUse == ACC_MPU9150)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_MMA8452
case ACC_MMA8452: // MMA8452
#ifdef NAZE
@ -486,9 +469,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
sensorsSet(SENSOR_GYRO);
detectAcc(accHardwareToUse);
detectBaro();
detectMag(magHardwareToUse);
reconfigureAlignment(sensorAlignmentConfig);
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
@ -496,6 +477,10 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init();
detectMag(magHardwareToUse);
reconfigureAlignment(sensorAlignmentConfig);
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (sensors(SENSOR_MAG)) {
// calculate magnetic declination

View file

@ -33,7 +33,7 @@
#include "sensors/sensors.h"
#include "sensors/sonar.h"
int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range
int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range - inclination adjusted by imu
#ifdef SONAR
@ -79,7 +79,7 @@ void Sonar_init(void)
void Sonar_update(void)
{
hcsr04_get_distance(&sonarAlt);
hcsr04_start_reading();
}
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
@ -91,4 +91,8 @@ int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
return sonarAlt * (900.0f - tiltAngle) / 900.0f;
}
int32_t sonarRead(void) {
return hcsr04_get_distance();
}
#endif

View file

@ -22,4 +22,5 @@ extern int32_t sonarAlt;
void Sonar_init(void);
void Sonar_update(void);
int32_t sonarRead(void);
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);

View file

@ -35,10 +35,18 @@
#define GYRO
#define USE_GYRO_L3GD20
#define USE_GYRO_MPU6050
#define ACC
#define USE_ACC_MPU6050
#define USE_ACC_LSM303DLHC
#define BARO
#define USE_BARO_MS5611
#define MAG
#define USE_MAG_AK8975
#define BEEPER
#define LED0
#define LED1
@ -51,7 +59,7 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define SENSORS_SET (SENSOR_ACC)
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP

View file

@ -67,3 +67,6 @@
// USART2, PA3
#define BIND_PORT GPIOA
#define BIND_PIN Pin_3
// Since the CJMCU PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers.
#define USE_QUAD_MIXER_ONLY

View file

@ -26,15 +26,15 @@
#define LED1_PIN Pin_5 // Green LEDs - PB5
#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB
// MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor
// MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component.
#define GYRO
#define USE_GYRO_MPU9150
#define USE_GYRO_MPU6050
#define ACC
#define USE_ACC_MPU9150
#define USE_ACC_MPU6050
#define BARO
#define USE_BARO_MS5611
//#define BARO
//#define USE_BARO_MS5611
#define MAG
#define USE_MAG_AK8975