mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Merge branch 'master' of https://github.com/cleanflight/cleanflight
This commit is contained in:
commit
dfd8e5b2b0
33 changed files with 601 additions and 603 deletions
23
.travis.yml
Normal file
23
.travis.yml
Normal 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
|
5
Makefile
5
Makefile
|
@ -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) \
|
||||
|
|
33
README.md
33
README.md
|
@ -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
|
||||
|
||||
[](https://travis-ci.org/cleanflight/cleanflight)
|
||||
|
||||
|
||||
|
|
|
@ -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.
|
||||
|
||||
|
|
17
docs/Rx.md
17
docs/Rx.md
|
@ -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.
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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]);
|
||||
}
|
|
@ -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) {
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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(¤tProfile->accelerometerTrims, masterConfig.mixerConfiguration);
|
||||
computeIMU(¤tProfile->accelerometerTrims, masterConfig.mixerMode);
|
||||
|
||||
// Measure loop rate just after reading the sensors
|
||||
currentTime = micros();
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
207
src/main/rx/xbus.c
Normal 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;
|
||||
}
|
|
@ -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);
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue