From 82626de33ad4ae2ad28669282bdffef889248953 Mon Sep 17 00:00:00 2001 From: androck Date: Tue, 16 Aug 2016 08:52:43 +0100 Subject: [PATCH 1/7] add support for flysky and turnigy ibus receiver ia6 --- src/main/config/config.c | 1 + src/main/io/serial_cli.c | 7 ++++++ src/main/rx/ibus.c | 51 ++++++++++++++++++++++++++++------------ src/main/rx/rx.h | 1 + 4 files changed, 45 insertions(+), 15 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 7a8e9a7a51..ea4534f6e2 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -523,6 +523,7 @@ void createDefaultConfig(master_t *config) #else config->rxConfig.serialrx_provider = 0; #endif + config->rxConfig.ibus_model = 0; config->rxConfig.sbus_inversion = 1; config->rxConfig.spektrum_sat_bind = 0; config->rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 92d8b22621..45355ef854 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -429,6 +429,10 @@ static const char * const lookupTableSerialRX[] = { }; #endif +static const char * const lookupTableIbusModel[] = { + "IA6B", "IA6" +}; + static const char * const lookupTableGyroLpf[] = { "OFF", "188HZ", @@ -546,6 +550,7 @@ typedef enum { #ifdef SERIAL_RX TABLE_SERIAL_RX, #endif + TABLE_IBUS_MODEL, TABLE_GYRO_LPF, TABLE_ACC_HARDWARE, #ifdef BARO @@ -585,6 +590,7 @@ static const lookupTableEntry_t lookupTables[] = { #ifdef SERIAL_RX { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, #endif + { lookupTableIbusModel, sizeof(lookupTableIbusModel) / sizeof(char *) }, { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, #ifdef BARO @@ -730,6 +736,7 @@ const clivalue_t valueTable[] = { #ifdef SERIAL_RX { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } }, #endif + { "ibus_model", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.ibus_model, .config.lookup = { TABLE_IBUS_MODEL } }, { "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.sbus_inversion, .config.lookup = { TABLE_OFF_ON } }, #ifdef SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} }, diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 507df1eff3..2064d40975 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -42,12 +42,20 @@ #include "rx/rx.h" #include "rx/ibus.h" -#define IBUS_MAX_CHANNEL 10 +#define IBUS_MAX_CHANNEL 14 #define IBUS_BUFFSIZE 32 -#define IBUS_SYNCBYTE 0x20 +#define IBUS_MODEL_IA6B 0 +#define IBUS_MODEL_IA6 1 #define IBUS_BAUDRATE 115200 +static uint8_t ibusModel = 0; +static uint8_t ibusSyncByte = 0x20; +static uint8_t ibusFrameSize = 32; +static uint8_t ibusChannelOffset = 2; +static uint32_t ibusInterframeGap = 3000; +static uint16_t ibusChecksum = 0xFFFF; + static bool ibusFrameDone = false; static uint32_t ibusChannelData[IBUS_MAX_CHANNEL]; @@ -56,7 +64,15 @@ static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { - UNUSED(rxConfig); + + if (rxConfig->ibus_model == IBUS_MODEL_IA6) { + ibusModel = 1; + ibusSyncByte = 0x55; + ibusFrameSize = 31; + ibusInterframeGap = 500; + ibusChecksum = 0x0000; + ibusChannelOffset = 1; + } if (callback) *callback = ibusReadRawRC; @@ -96,17 +112,17 @@ static void ibusDataReceive(uint16_t c) ibusTime = micros(); - if ((ibusTime - ibusTimeLast) > 3000) + if ((ibusTime - ibusTimeLast) > ibusInterframeGap) ibusFramePosition = 0; ibusTimeLast = ibusTime; - if (ibusFramePosition == 0 && c != IBUS_SYNCBYTE) + if (ibusFramePosition == 0 && c != ibusSyncByte) return; ibus[ibusFramePosition] = (uint8_t)c; - if (ibusFramePosition == IBUS_BUFFSIZE - 1) { + if (ibusFramePosition == ibusFrameSize - 1) { ibusFrameDone = true; } else { ibusFramePosition++; @@ -125,17 +141,22 @@ uint8_t ibusFrameStatus(void) ibusFrameDone = false; - chksum = 0xFFFF; - for (i = 0; i < 30; i++) - chksum -= ibus[i]; - - rxsum = ibus[30] + (ibus[31] << 8); + chksum = ibusChecksum; + rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8); + + if (ibusModel == IBUS_MODEL_IA6) { + for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) + chksum += ibus[offset] + (ibus[offset + 1] << 8); + } else { + for (i = 0; i < 30; i++) + chksum -= ibus[i]; + } if (chksum == rxsum) { - for (i = 0, offset = 2; i < IBUS_MAX_CHANNEL; i++, offset += 2) { - ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8); - } - frameStatus = SERIAL_RX_FRAME_COMPLETE; + for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) { + ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8); + } + frameStatus = SERIAL_RX_FRAME_COMPLETE; } return frameStatus; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index effd1cc526..153d145dfc 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -112,6 +112,7 @@ typedef struct rxChannelRangeConfiguration_s { typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. + uint8_t ibus_model; uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers. uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot From b51c9e0c038045c76226f2b8cc5836e76f7d3250 Mon Sep 17 00:00:00 2001 From: atomiclama Date: Mon, 3 Oct 2016 14:31:17 +0100 Subject: [PATCH 2/7] Cherry pick d2d40ea8cf13d474b3fb64f946006b3b539c115e from atomiclama Squashed commit of the following: commit ba4ef96 Author: atomiclama Date: Mon Oct 3 10:45:24 2016 +0100 rebased commit b8780a6 Author: atomiclama Date: Mon Oct 3 10:19:07 2016 +0100 comment added commit 2a7144c Author: atomiclama Date: Fri Sep 30 16:21:07 2016 +0100 fixed interframe value of 500us for both protocols. Setup variables on first occurance of correct sync byte. commit 4c3a503 Author: atomiclama Date: Fri Sep 30 13:09:20 2016 +0100 First working point for autodetect commit d947e5d Author: atomiclama Date: Wed Sep 28 09:20:58 2016 +0100 Merge remote-tracking branch 'androck/development' into vtx-betaflight-dev Reworked so that it's based on serial provider instead of the extra config entry. This saves space but requires a change in the configurator. --- src/main/config/config.c | 1 - src/main/io/serial_cli.c | 7 ----- src/main/rx/ibus.c | 66 ++++++++++++++++++++++++---------------- src/main/rx/rx.h | 1 - 4 files changed, 39 insertions(+), 36 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ea4534f6e2..7a8e9a7a51 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -523,7 +523,6 @@ void createDefaultConfig(master_t *config) #else config->rxConfig.serialrx_provider = 0; #endif - config->rxConfig.ibus_model = 0; config->rxConfig.sbus_inversion = 1; config->rxConfig.spektrum_sat_bind = 0; config->rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 45355ef854..92d8b22621 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -429,10 +429,6 @@ static const char * const lookupTableSerialRX[] = { }; #endif -static const char * const lookupTableIbusModel[] = { - "IA6B", "IA6" -}; - static const char * const lookupTableGyroLpf[] = { "OFF", "188HZ", @@ -550,7 +546,6 @@ typedef enum { #ifdef SERIAL_RX TABLE_SERIAL_RX, #endif - TABLE_IBUS_MODEL, TABLE_GYRO_LPF, TABLE_ACC_HARDWARE, #ifdef BARO @@ -590,7 +585,6 @@ static const lookupTableEntry_t lookupTables[] = { #ifdef SERIAL_RX { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, #endif - { lookupTableIbusModel, sizeof(lookupTableIbusModel) / sizeof(char *) }, { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, #ifdef BARO @@ -736,7 +730,6 @@ const clivalue_t valueTable[] = { #ifdef SERIAL_RX { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } }, #endif - { "ibus_model", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.ibus_model, .config.lookup = { TABLE_IBUS_MODEL } }, { "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.sbus_inversion, .config.lookup = { TABLE_OFF_ON } }, #ifdef SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} }, diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 2064d40975..53a63c1511 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -46,15 +46,15 @@ #define IBUS_BUFFSIZE 32 #define IBUS_MODEL_IA6B 0 #define IBUS_MODEL_IA6 1 +#define IBUS_FRAME_GAP 500 #define IBUS_BAUDRATE 115200 -static uint8_t ibusModel = 0; -static uint8_t ibusSyncByte = 0x20; -static uint8_t ibusFrameSize = 32; -static uint8_t ibusChannelOffset = 2; -static uint32_t ibusInterframeGap = 3000; -static uint16_t ibusChecksum = 0xFFFF; +static uint8_t ibusModel; +static uint8_t ibusSyncByte; +static uint8_t ibusFrameSize; +static uint8_t ibusChannelOffset; +static uint16_t ibusChecksum; static bool ibusFrameDone = false; static uint32_t ibusChannelData[IBUS_MAX_CHANNEL]; @@ -64,19 +64,13 @@ static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { - - if (rxConfig->ibus_model == IBUS_MODEL_IA6) { - ibusModel = 1; - ibusSyncByte = 0x55; - ibusFrameSize = 31; - ibusInterframeGap = 500; - ibusChecksum = 0x0000; - ibusChannelOffset = 1; - } + UNUSED(rxConfig); if (callback) *callback = ibusReadRawRC; + ibusSyncByte = 0; + rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL; serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); @@ -112,13 +106,32 @@ static void ibusDataReceive(uint16_t c) ibusTime = micros(); - if ((ibusTime - ibusTimeLast) > ibusInterframeGap) + if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP) ibusFramePosition = 0; ibusTimeLast = ibusTime; - if (ibusFramePosition == 0 && c != ibusSyncByte) - return; + if (ibusFramePosition == 0) { + if (ibusSyncByte == 0) { + // detect the frame type based on the STX byte. + if (c == 0x55) { + ibusModel = IBUS_MODEL_IA6; + ibusSyncByte = 0x55; + ibusFrameSize = 31; + ibusChecksum = 0x0000; + ibusChannelOffset = 1; + } else if (c == 0x20) { + ibusModel = IBUS_MODEL_IA6B; + ibusSyncByte = 0x20; + ibusFrameSize = 32; + ibusChannelOffset = 2; + ibusChecksum = 0xFFFF; + } else + return; + } else if (ibusSyncByte != c) { + return; + } + } ibus[ibusFramePosition] = (uint8_t)c; @@ -143,20 +156,19 @@ uint8_t ibusFrameStatus(void) chksum = ibusChecksum; rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8); - if (ibusModel == IBUS_MODEL_IA6) { - for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) - chksum += ibus[offset] + (ibus[offset + 1] << 8); - } else { - for (i = 0; i < 30; i++) - chksum -= ibus[i]; + for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) + chksum += ibus[offset] + (ibus[offset + 1] << 8); + } else { + for (i = 0; i < 30; i++) + chksum -= ibus[i]; } if (chksum == rxsum) { for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) { - ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8); - } - frameStatus = SERIAL_RX_FRAME_COMPLETE; + ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8); + } + frameStatus = SERIAL_RX_FRAME_COMPLETE; } return frameStatus; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 153d145dfc..effd1cc526 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -112,7 +112,6 @@ typedef struct rxChannelRangeConfiguration_s { typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. - uint8_t ibus_model; uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers. uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot From 850b0ba4b5268089ea1932990bbd322bc9ade6c0 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 10 Oct 2016 12:58:36 +1300 Subject: [PATCH 3/7] Moved KISS default serial RX to USART3 on @basdelfos' request. --- src/main/target/KISSFC/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 05346145d6..6f1f112794 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -71,7 +71,7 @@ #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART2 +#define SERIALRX_UART SERIAL_PORT_USART3 #define AVOID_UART2_FOR_PWM_PPM From b08a9ac89c59ea022fc899b12f824e2b8e7e65e1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 12 Oct 2016 12:29:59 +0200 Subject: [PATCH 4/7] Initial BeeBrain Support --- .travis.yml | 1 + fake_travis_build.sh | 1 + out.asm | 3 - src/main/config/config.c | 1 + src/main/flight/pid.c | 4 +- src/main/flight/pid.h | 1 + src/main/io/serial_cli.c | 2 + src/main/target/NAZE/config.c | 115 ++++++++++++++++++++++++++++++++++ src/main/target/NAZE/target.h | 13 +++- 9 files changed, 133 insertions(+), 8 deletions(-) delete mode 100644 out.asm create mode 100755 src/main/target/NAZE/config.c diff --git a/.travis.yml b/.travis.yml index 154dca3df9..88843dd140 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,6 +3,7 @@ env: # - PUBLISHMETA=True # - PUBLISHDOCS=True # - TARGET=AFROMINI +# - TARGET=BEEBRAIN # - TARGET=AIORACERF3 # - TARGET=AIR32 # - TARGET=ALIENFLIGHTF1 diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 10d3e67966..03b894df06 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -11,6 +11,7 @@ targets=("PUBLISHMETA=True" \ "TARGET=OMNIBUS" \ "TARGET=NAZE" \ "TARGET=AFROMINI" \ + "TARGET=BEEBRAIN" \ "TARGET=RMDO" \ "TARGET=SPARKY" \ "TARGET=MOTOLAB" \ diff --git a/out.asm b/out.asm deleted file mode 100644 index e7647deebc..0000000000 --- a/out.asm +++ /dev/null @@ -1,3 +0,0 @@ - -obj/main/betaflight_NAZE.elf: file format elf32-little - diff --git a/src/main/config/config.c b/src/main/config/config.c index e7dcdf590f..0e0ee40f46 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -253,6 +253,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; pidProfile->itermThrottleGain = 0; + pidProfile->levelSensitivity = 2.0f; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index eca6531a07..59eb65015c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -217,10 +217,10 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { // calculate error angle and limit the angle to the max inclination #ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f393208e56..57546e1748 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -96,6 +96,7 @@ typedef struct pidProfile_s { uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms + float levelSensitivity; #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d38ec3a07c..9c1fb89ed9 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -890,6 +890,8 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, + { "level_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 0.1, 3.0 } }, + #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } }, diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c new file mode 100755 index 0000000000..8004561405 --- /dev/null +++ b/src/main/target/NAZE/config.c @@ -0,0 +1,115 @@ +/* + * 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 "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +#ifdef BEEBRAIN +// alternative defaults settings for Beebrain target +void targetConfiguration(master_t *config) +{ + config->motor_pwm_rate = 4000; + config->failsafeConfig.failsafe_delay = 2; + config->failsafeConfig.failsafe_off_delay = 0; + + config->escAndServoConfig.minthrottle = 1049; + + config->gyro_lpf = 1; + config->gyro_soft_lpf_hz = 100; + config->gyro_soft_notch_hz_1 = 0; + + for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { + config->rxConfig.channelRanges[channel].min = 1180; + config->rxConfig.channelRanges[channel].max = 1860; + } + + for (int profileId = 0; profileId < 2; profileId++) { + config->profile[profileId].pidProfile.P8[ROLL] = 55; + config->profile[profileId].pidProfile.I8[ROLL] = 40; + config->profile[profileId].pidProfile.D8[ROLL] = 20; + config->profile[profileId].pidProfile.P8[PITCH] = 55; + config->profile[profileId].pidProfile.I8[PITCH] = 40; + config->profile[profileId].pidProfile.D8[PITCH] = 20; + config->profile[profileId].pidProfile.P8[YAW] = 180; + config->profile[profileId].pidProfile.I8[YAW] = 45; + config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50; + config->profile[profileId].pidProfile.D8[PIDLEVEL] = 50; + config->profile[profileId].pidProfile.levelSensitivity = 1.0f; + + for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { + config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 120; + config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 120; + config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 99; + config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 99; + config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 99; + + config->profile[profileId].pidProfile.setpointRelaxRatio = 100; + } + } +} +#endif diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index f222b5b6b9..7a8142177c 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -17,7 +17,6 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "AFNA" // AFroNAze - NAZE might be considered misleading on Naze clones like the flip32. #define USE_HARDWARE_REVISION_DETECTION #define BOARD_HAS_VOLTAGE_DIVIDER @@ -25,9 +24,17 @@ #define LED0 PB3 #define LED1 PB4 -#define BEEPER PA12 -#ifdef AFROMINI +#define BEEPER PA1 + +#if defined(AFROMINI) #define BEEPER_INVERTED +#define TARGET_BOARD_IDENTIFIER "AFMN" +#elif defined(BEEBRAIN) +#define BRUSHED_MOTORS +#define TARGET_BOARD_IDENTIFIER "BEBR" +#define TARGET_CONFIG +#else +#define TARGET_BOARD_IDENTIFIER "AFNA" #endif #define BARO_XCLR_PIN PC13 From 37c10e8a10030b67147b0e8bde94219e701a2a2b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 12 Oct 2016 13:07:37 +0200 Subject: [PATCH 5/7] Fix wrong Beeper definition NAZE --- src/main/target/NAZE/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 7a8142177c..3bc6f0549c 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -24,7 +24,7 @@ #define LED0 PB3 #define LED1 PB4 -#define BEEPER PA1 +#define BEEPER PA12 #if defined(AFROMINI) #define BEEPER_INVERTED From 6547741cfec36189f41cf41be908126a0a0e9cc5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 13 Oct 2016 01:41:04 +0200 Subject: [PATCH 6/7] New Beebrain defaults // refactoring --- src/main/common/filter.c | 4 ++-- src/main/common/filter.h | 10 +++++----- src/main/flight/pid.c | 10 +++++----- src/main/io/serial_cli.c | 2 +- src/main/sensors/gyro.c | 6 +++--- src/main/target/NAZE/config.c | 20 ++++++++++---------- src/main/target/NAZE/target.h | 2 ++ 7 files changed, 28 insertions(+), 26 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 36786199ba..87b0aaadde 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -115,12 +115,12 @@ float biquadFilterApply(biquadFilter_t *filter, float input) return result; } -void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { +void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_DENOISE_WINDOW_SIZE); } /* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */ -float denoisingFilterUpdate(denoisingState_t *filter, float input) { +float firFilterUpdate(firFilterState_t *filter, float input) { filter->state[filter->index] = input; filter->movingSum += filter->state[filter->index++]; if (filter->index == filter->targetCount) diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 4696edb325..03f91d21fc 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -29,18 +29,18 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; -typedef struct dennoisingState_s { +typedef struct firFilterState_s { int filledCount; int targetCount; int index; float movingSum; float state[MAX_DENOISE_WINDOW_SIZE]; -} denoisingState_t; +} firFilterState_t; typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, - FILTER_DENOISE + FILTER_FIR } filterType_e; typedef enum { @@ -56,6 +56,6 @@ float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); -void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); -float denoisingFilterUpdate(denoisingState_t *filter, float input); +void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); +float firFilterUpdate(firFilterState_t *filter, float input); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 59eb65015c..02f7f4e0ae 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -108,7 +108,7 @@ static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; static biquadFilter_t dtermFilterLpf[3]; static biquadFilter_t dtermFilterNotch[3]; -static denoisingState_t dtermDenoisingState[3]; +static firFilterState_t dtermDenoisingState[3]; static bool dtermNotchInitialised; void initFilters(const pidProfile_t *pidProfile) { @@ -126,8 +126,8 @@ void initFilters(const pidProfile_t *pidProfile) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } - if (pidProfile->dterm_filter_type == FILTER_DENOISE) { - for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + if (pidProfile->dterm_filter_type == FILTER_FIR) { + for (axis = 0; axis < 3; axis++) initFirFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } lowpassFilterType = pidProfile->dterm_filter_type; } @@ -282,7 +282,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc else if (pidProfile->dterm_filter_type == FILTER_PT1) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); else - delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta); + delta = firFilterUpdate(&dtermDenoisingState[axis], delta); } DTerm = Kd[axis] * delta * tpaFactor; @@ -422,7 +422,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina else if (pidProfile->dterm_filter_type == FILTER_PT1) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); else - delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta); + delta = firFilterUpdate(&dtermDenoisingState[axis], delta); delta = lrintf(deltaf); } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9c1fb89ed9..5d43234e1e 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -515,7 +515,7 @@ static const char * const lookupTableRcInterpolation[] = { }; static const char * const lookupTableLowpassType[] = { - "NORMAL", "HIGH", "DENOISE" + "PT1", "BIQUAD", "FIR" }; static const char * const lookupTableFailsafe[] = { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 3dd8f59c2c..a7a8a542a1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -48,7 +48,7 @@ static const gyroConfig_t *gyroConfig; static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; -static denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT]; +static firFilterState_t gyroDenoiseState[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; static float gyroSoftNotchQ1, gyroSoftNotchQ2; @@ -82,7 +82,7 @@ void gyroInit(void) else if (gyroSoftLpfType == FILTER_PT1) gyroDt = (float) gyro.targetLooptime * 0.000001f; else - initDenoisingFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); + initFirFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); } } @@ -197,7 +197,7 @@ void gyroUpdate(void) else if (gyroSoftLpfType == FILTER_PT1) gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); else - gyroADCf[axis] = denoisingFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); + gyroADCf[axis] = firFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 8004561405..7596d8ec65 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -90,11 +90,11 @@ void targetConfiguration(master_t *config) for (int profileId = 0; profileId < 2; profileId++) { config->profile[profileId].pidProfile.P8[ROLL] = 55; - config->profile[profileId].pidProfile.I8[ROLL] = 40; - config->profile[profileId].pidProfile.D8[ROLL] = 20; - config->profile[profileId].pidProfile.P8[PITCH] = 55; - config->profile[profileId].pidProfile.I8[PITCH] = 40; - config->profile[profileId].pidProfile.D8[PITCH] = 20; + config->profile[profileId].pidProfile.I8[ROLL] = 50; + config->profile[profileId].pidProfile.D8[ROLL] = 25; + config->profile[profileId].pidProfile.P8[PITCH] = 65; + config->profile[profileId].pidProfile.I8[PITCH] = 60; + config->profile[profileId].pidProfile.D8[PITCH] = 28; config->profile[profileId].pidProfile.P8[YAW] = 180; config->profile[profileId].pidProfile.I8[YAW] = 45; config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50; @@ -102,11 +102,11 @@ void targetConfiguration(master_t *config) config->profile[profileId].pidProfile.levelSensitivity = 1.0f; for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { - config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 120; - config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 120; - config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 99; - config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 99; - config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 99; + config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80; + config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80; + config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80; config->profile[profileId].pidProfile.setpointRelaxRatio = 100; } diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 3bc6f0549c..c82d66ea33 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -164,6 +164,8 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM + // IO - assuming all IOs on 48pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff From ab3dcb630237bd7ef47260877c1c7e5702349965 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 13 Oct 2016 01:53:37 +0200 Subject: [PATCH 7/7] Disable rx range from defaults for Beebrain --- src/main/target/NAZE/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 7596d8ec65..0036b6ead7 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -83,10 +83,10 @@ void targetConfiguration(master_t *config) config->gyro_soft_lpf_hz = 100; config->gyro_soft_notch_hz_1 = 0; - for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { + /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { config->rxConfig.channelRanges[channel].min = 1180; config->rxConfig.channelRanges[channel].max = 1860; - } + }*/ for (int profileId = 0; profileId < 2; profileId++) { config->profile[profileId].pidProfile.P8[ROLL] = 55;