From b08a9ac89c59ea022fc899b12f824e2b8e7e65e1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 12 Oct 2016 12:29:59 +0200 Subject: [PATCH] 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 154dca3df94..88843dd1404 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 10d3e67966d..03b894df062 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 e7647deebc8..00000000000 --- 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 e7dcdf590ff..0e0ee40f466 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 eca6531a073..59eb65015ce 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 f393208e562..57546e1748d 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 d38ec3a07c1..9c1fb89ed92 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 00000000000..80045614054 --- /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 f222b5b6b96..7a8142177c6 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