diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000000..9ef497f543 --- /dev/null +++ b/.travis.yml @@ -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 \ No newline at end of file diff --git a/Makefile b/Makefile index bb9e9d591e..1599e9303a 100644 --- a/Makefile +++ b/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) \ diff --git a/README.md b/README.md index 8696d23a1f..898acc7274 100644 --- a/README.md +++ b/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 + +[![Build Status](https://travis-ci.org/cleanflight/cleanflight.svg?branch=master)](https://travis-ci.org/cleanflight/cleanflight) + + diff --git a/docs/Gps.md b/docs/Gps.md index d8d40ceb7d..98dbb55b9b 100644 --- a/docs/Gps.md +++ b/docs/Gps.md @@ -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. diff --git a/docs/Rx.md b/docs/Rx.md index f61a6fa8c3..23ce0540b9 100644 --- a/docs/Rx.md +++ b/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. diff --git a/src/main/config/config.c b/src/main/config/config.c index f3f79f3eb7..04919bcbb0 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 2ab8158bea..9a486552d9 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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 diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 324ea03684..6dc55c70ab 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -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 diff --git a/src/main/drivers/accgyro_mpu9150.c b/src/main/drivers/accgyro_mpu9150.c deleted file mode 100644 index 3cbb2d086a..0000000000 --- a/src/main/drivers/accgyro_mpu9150.c +++ /dev/null @@ -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 . - */ - -// NOTE: this file is a copy of the mpu6050 driver with very minimal changes. -// some de-duplication needs to occur... - -#include -#include -#include - -#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�s 7-bit I2C address. - // The least significant bit of the MPU-60X0�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]); -} diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index ab08ae7e10..9bba2ffb60 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -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) { diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 2b63f5b23e..0812daaf19 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -20,6 +20,8 @@ #include +#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 } diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 758194d39b..ee1c78904c 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -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 diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index 9a72bfc597..9ca57d397f 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -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); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 4d2257a01b..d2bb2b146c 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -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 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ffdbeb234f..a9eb8b0de5 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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 { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index d8039f2b59..de5ac89f01 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 930fe4ebff..324bdf12fc 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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)) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index fa6613a81c..f198fd11dd 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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 { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ef31d80979..dd5c243409 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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; } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 309785b85f..27268a7795 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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(); diff --git a/src/main/main.c b/src/main/main.c index 9c3e8d82f9..e88b1b24ba 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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); diff --git a/src/main/mw.c b/src/main/mw.c index 4193f2cfb7..ba4dcd3618 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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(); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 4836bd2119..60ebc97263 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -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; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7d06f8e9d1..92ae2eb0f2 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -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) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c new file mode 100644 index 0000000000..9125151d77 --- /dev/null +++ b/src/main/rx/xbus.c @@ -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 . + */ + +#include +#include +#include + +#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; +} diff --git a/src/main/drivers/accgyro_mpu9150.h b/src/main/rx/xbus.h similarity index 67% rename from src/main/drivers/accgyro_mpu9150.h rename to src/main/rx/xbus.h index e511881703..bc06e5e6e4 100644 --- a/src/main/drivers/accgyro_mpu9150.h +++ b/src/main/rx/xbus.h @@ -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); diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 0b397cd130..b1cd83d758 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -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; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 1ec481f105..3eaf74d49f 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -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 diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 0f002e0d14..518cb56052 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -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 diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 45987e7543..a6a2a8b94d 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -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); diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index cacaa9b621..2b7cdd34f6 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -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 diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 6fb8d5999a..85d5f490c6 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -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 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index b45b145ff4..6eba579074 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -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