From 0dcc2bbeaa0f8738fecb7115e73ecb1b0de14680 Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Sat, 6 Aug 2016 19:40:07 -0500 Subject: [PATCH 01/26] FuryF4 Target Changes/Cleanup --- src/main/target/FURYF4/config.c | 86 +++++++++++++++++++++++++++++++++ src/main/target/FURYF4/target.h | 23 +++------ 2 files changed, 94 insertions(+), 15 deletions(-) create mode 100644 src/main/target/FURYF4/config.c diff --git a/src/main/target/FURYF4/config.c b/src/main/target/FURYF4/config.c new file mode 100644 index 000000000..0a12258f2 --- /dev/null +++ b/src/main/target/FURYF4/config.c @@ -0,0 +1,86 @@ +/* + * 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/compass.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/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.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 "io/gps.h" +#include "io/osd.h" +#include "io/vtx.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" + +// alternative defaults settings for FURYF4 targets +void targetConfiguration(void) { + masterConfig.mag_hardware = MAG_NONE; // disabled by default + masterConfig.motor_pwm_rate = 32000; + masterConfig.failsafeConfig.failsafe_delay = 2; + masterConfig.failsafeConfig.failsafe_off_delay = 0; + masterConfig.gyro_sync_denom = 1; + masterConfig.pid_process_denom = 1; +} diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 8f6937859..4006d9d66 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -18,6 +18,8 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FYF4" +#define TARGET_CONFIG + #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define USBD_PRODUCT_STRING "FuryF4" @@ -36,6 +38,7 @@ #define MPU_INT_EXTI PC4 #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) #define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -43,15 +46,18 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 +#define GYRO #define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW180_DEG -#define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG @@ -68,17 +74,6 @@ #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB12 -/* -#define SDCARD_DETECT_PIN PD2 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD -#define SDCARD_DETECT_EXTI_IRQn EXTI2_IRQn - -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PB3 -*/ - // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz // Divide to under 25MHz for normal operation: @@ -95,7 +90,6 @@ #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_Channel_0 - #define USE_FLASHFS #define USE_FLASH_M25P16 #define M25P16_CS_PIN PB3 @@ -110,7 +104,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 From f6cc56514a4fd784c96f5e2089c45f2475934154 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 11:13:50 +0200 Subject: [PATCH 02/26] Corrected the usage of TARGET_FLAGS in main Makefile. --- Makefile | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 281b169c6..777c04026 100644 --- a/Makefile +++ b/Makefile @@ -179,7 +179,6 @@ LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion DEVICE_FLAGS = -DSTM32F303xC -DSTM32F303 -TARGET_FLAGS = -D$(TARGET) # End F3 targets # # Start F4 targets @@ -273,7 +272,6 @@ $(error Unknown MCU for F4 target) endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -TARGET_FLAGS = -D$(TARGET) # End F4 targets # # Start F1 targets @@ -313,7 +311,6 @@ endif LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f103_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m3 -TARGET_FLAGS := -D$(TARGET) -pedantic $(TARGET_FLAGS) ifeq ($(DEVICE_FLAGS),) DEVICE_FLAGS = -DSTM32F10X_MD @@ -581,8 +578,10 @@ CFLAGS += $(ARCH_FLAGS) \ -Wall -Wextra -Wunsafe-loop-optimizations -Wdouble-promotion \ -ffunction-sections \ -fdata-sections \ + -pedantic \ $(DEVICE_FLAGS) \ -DUSE_STDPERIPH_DRIVER \ + -D$(TARGET) \ $(TARGET_FLAGS) \ -D'__FORKNAME__="$(FORKNAME)"' \ -D'__TARGET__="$(TARGET)"' \ From cba6c508fadaa392ac9c7b6c4a263e3ac8d0e1a5 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 14:47:45 +0200 Subject: [PATCH 03/26] First try with Travis on a few targets. --- .travis.sh | 2 +- .travis.yml | 69 ++++++++++++++++++++++++++++++++++++----------------- 2 files changed, 48 insertions(+), 23 deletions(-) diff --git a/.travis.sh b/.travis.sh index f848242d3..ee310af88 100755 --- a/.travis.sh +++ b/.travis.sh @@ -4,7 +4,7 @@ REVISION=$(git rev-parse --short HEAD) BRANCH=$(git rev-parse --abbrev-ref HEAD) REVISION=$(git rev-parse --short HEAD) LAST_COMMIT_DATE=$(git log -1 --date=short --format="%cd") -TARGET_FILE=obj/cleanflight_${TARGET} +TARGET_FILE=obj/betaflight_${TARGET} TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/undefined} BUILDNAME=${BUILDNAME:=travis} TRAVIS_BUILD_NUMBER=${TRAVIS_BUILD_NUMBER:=undefined} diff --git a/.travis.yml b/.travis.yml index 9efb7f246..661522e0d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,27 +1,52 @@ env: - - RUNTESTS=True +# - RUNTESTS=True - PUBLISHMETA=True - - PUBLISHDOCS=True +# - PUBLISHDOCS=True +# - TARGET=AIORACERF3 + - TARGET=AFROMINI - TARGET=CC3D - TARGET=CC3D_OPBL - - TARGET=CC3D_BP6 - - TARGET=CC3D_OPBL_BP6 - - TARGET=COLIBRI_RACE - - TARGET=LUX_RACE - - TARGET=CHEBUZZF3 +# - TARGET=ALIENFLIGHTF1 +# - TARGET=ALIENFLIGHTF3 +# - TARGET=ALIENFLIGHTF4 +# - TARGET=AIR32 +# - TARGET=BLUEJAYF4 +# - TARGET=CHEBUZZF3 - TARGET=CJMCU - - TARGET=EUSTM32F103RC - - TARGET=SPRACINGF3 + - TARGET=COLIBRI + - TARGET=COLIBRI_RACE + - TARGET=DOGE +# - TARGET=EUSTM32F103RC +# - TARGET=F4BY +# - TARGET=FURYF3 +# - TARGET=FURYF4 +# - TARGET=IRCFUSIONF3 +# - TARGET=KISSFC +# - TARGET=LUX_RACE +# - TARGET=MICROSCISKY +# - TARGET=MOTOLAB - TARGET=NAZE - - TARGET=NAZE32PRO - - TARGET=OLIMEXINO - - TARGET=RMDO - - TARGET=PORT103R - - TARGET=SPARKY - - TARGET=STM32F3DISCOVERY - - TARGET=ALIENFLIGHTF1 - - TARGET=ALIENFLIGHTF3 - +# - TARGET=OLIMEXINO +# - TARGET=OMNIBUS +# - TARGET=OMNIBUSF4 +# - TARGET=PIKOBLX +# - TARGET=PORT103R + - TARGET=REVO +# - TARGET=REVONANO +# - TARGET=REVO_OPBL +# - TARGET=RMDO +# - TARGET=SINGULARITY +# - TARGET=SIRINFPV +# - TARGET=SPARKY +# - TARGET=SPARKY2 +# - TARGET=SPARKY_OPBL + - TARGET=SPRACINGF3 +# - TARGET=SPRACINGF3EVO +# - TARGET=SPRACINGF3MINI +# - TARGET=STM32F3DISCOVERY +# - TARGET=VRRACE +# - TARGET=X_RACERSPI +# - TARGET=ZCOREF3 # use new docker environment sudo: false @@ -52,7 +77,7 @@ script: ./.travis.sh cache: apt -notifications: - irc: "chat.freenode.net#cleanflight" - use_notice: true - skip_join: true +#notifications: +# irc: "chat.freenode.net#cleanflight" +# use_notice: true +# skip_join: true From 98d634231e476245def36e9f6f9370aa43b43403 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 15:19:47 +0200 Subject: [PATCH 04/26] Second TRavis trail with all 45 targets. --- .travis.yml | 72 ++++++++++++++++++++++++++--------------------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/.travis.yml b/.travis.yml index 661522e0d..ca73f8c14 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,52 +1,52 @@ env: # - RUNTESTS=True - - PUBLISHMETA=True +# - PUBLISHMETA=True # - PUBLISHDOCS=True -# - TARGET=AIORACERF3 - TARGET=AFROMINI + - TARGET=AIORACERF3 + - TARGET=AIR32 + - TARGET=ALIENFLIGHTF1 + - TARGET=ALIENFLIGHTF3 + - TARGET=ALIENFLIGHTF4 + - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL -# - TARGET=ALIENFLIGHTF1 -# - TARGET=ALIENFLIGHTF3 -# - TARGET=ALIENFLIGHTF4 -# - TARGET=AIR32 -# - TARGET=BLUEJAYF4 -# - TARGET=CHEBUZZF3 + - TARGET=CHEBUZZF3 - TARGET=CJMCU - TARGET=COLIBRI - TARGET=COLIBRI_RACE - TARGET=DOGE -# - TARGET=EUSTM32F103RC -# - TARGET=F4BY -# - TARGET=FURYF3 -# - TARGET=FURYF4 -# - TARGET=IRCFUSIONF3 -# - TARGET=KISSFC -# - TARGET=LUX_RACE -# - TARGET=MICROSCISKY -# - TARGET=MOTOLAB + - TARGET=EUSTM32F103RC + - TARGET=F4BY + - TARGET=FURYF3 + - TARGET=FURYF4 + - TARGET=IRCFUSIONF3 + - TARGET=KISSFC + - TARGET=LUX_RACE + - TARGET=MICROSCISKY + - TARGET=MOTOLAB - TARGET=NAZE -# - TARGET=OLIMEXINO -# - TARGET=OMNIBUS -# - TARGET=OMNIBUSF4 -# - TARGET=PIKOBLX -# - TARGET=PORT103R + - TARGET=OLIMEXINO + - TARGET=OMNIBUS + - TARGET=OMNIBUSF4 + - TARGET=PIKOBLX + - TARGET=PORT103R - TARGET=REVO -# - TARGET=REVONANO -# - TARGET=REVO_OPBL -# - TARGET=RMDO -# - TARGET=SINGULARITY -# - TARGET=SIRINFPV -# - TARGET=SPARKY -# - TARGET=SPARKY2 -# - TARGET=SPARKY_OPBL + - TARGET=REVONANO + - TARGET=REVO_OPBL + - TARGET=RMDO + - TARGET=SINGULARITY + - TARGET=SIRINFPV + - TARGET=SPARKY + - TARGET=SPARKY2 + - TARGET=SPARKY_OPBL - TARGET=SPRACINGF3 -# - TARGET=SPRACINGF3EVO -# - TARGET=SPRACINGF3MINI -# - TARGET=STM32F3DISCOVERY -# - TARGET=VRRACE -# - TARGET=X_RACERSPI -# - TARGET=ZCOREF3 + - TARGET=SPRACINGF3EVO + - TARGET=SPRACINGF3MINI + - TARGET=STM32F3DISCOVERY + - TARGET=VRRACE + - TARGET=X_RACERSPI + - TARGET=ZCOREF3 # use new docker environment sudo: false From c3dd940107accbbf53ad2f93572da6b45df85028 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 10:32:51 +0200 Subject: [PATCH 05/26] README.md merged from development. Needs to be here on master too. --- README.md | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index 0253334ef..9b88d1fbe 100644 --- a/README.md +++ b/README.md @@ -61,11 +61,9 @@ If what you need is not covered then refer to the baseflight documentation. If y ## IRC Support and Developers Channel -There's a dedicated IRC channel here: +There's a dedicated IRCgitter chat channel here: -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/ +https://gitter.im/betaflight/betaflight 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. @@ -79,13 +77,13 @@ Please subscribe and '+1' the videos if you find them useful. ## Configuration Tool -To configure Cleanflight you should use the Cleanflight-configurator GUI tool (Windows/OSX/Linux) that can be found here: +To configure Betaflight you should use the Betaflight-configurator GUI tool (Windows/OSX/Linux) that can be found here: -https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb +https://chrome.google.com/webstore/detail/betaflight-configurator/kdaghagfopacdngbohiknlhcocjccjao The source for it is here: -https://github.com/cleanflight/cleanflight-configurator +https://github.com/betaflight/betaflight-configurator ## Contributing @@ -97,10 +95,10 @@ Contributions are welcome and encouraged. You can contribute in many ways: * 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: +The best place to start is the IRC channel on gitter (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 +https://github.com/betaflight/betaflight/issues +https://github.com/betaflight/betaflight-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! @@ -110,11 +108,11 @@ Please refer to the development section in the `docs/development` folder. TravisCI is used to run automatic builds -https://travis-ci.org/cleanflight/cleanflight +https://travis-ci.org/betaflight/betaflight -[![Build Status](https://travis-ci.org/cleanflight/cleanflight.svg?branch=master)](https://travis-ci.org/cleanflight/cleanflight) +[![Build Status](https://travis-ci.org/betaflight/betaflight.svg?branch=master)](https://travis-ci.org/betaflight/betaflight) -## Cleanflight Releases -https://github.com/cleanflight/cleanflight/releases +## Betaflight Releases +https://github.com/betaflight/betaflight/releases From f9354e53b3b41c71e0982ef65b01b26474d7af41 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 10 Aug 2016 00:00:40 +1200 Subject: [PATCH 06/26] Graft of 'cli_diff_command' into 'master'. --- src/main/config/config.c | 306 +++++++------ src/main/config/config.h | 1 - src/main/config/config_master.h | 2 + src/main/io/ledstrip.c | 3 - src/main/io/osd.c | 34 +- src/main/io/osd.h | 2 +- src/main/io/serial_cli.c | 706 +++++++++++++++++++++--------- src/main/target/CC3D/target.h | 12 +- src/main/target/NAZE/target.h | 2 +- src/main/target/PORT103R/target.h | 10 +- 10 files changed, 696 insertions(+), 382 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index dcb0ce85c..b8b5cf11e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -273,7 +273,7 @@ void resetProfile(profile_t *profile) resetControlRateConfig(&profile->controlRateProfile[rI]); } - profile->activeRateProfile = 0; + profile->activeRateProfile = 0; } #ifdef GPS @@ -440,263 +440,280 @@ uint16_t getCurrentMinthrottle(void) return masterConfig.escAndServoConfig.minthrottle; } +static void intFeatureClearAll(master_t *config); +static void intFeatureSet(uint32_t mask, master_t *config); +static void intFeatureClear(uint32_t mask, master_t *config); + // Default settings -static void resetConf(void) +void createDefaultConfig(master_t *config) { // Clear all configuration - memset(&masterConfig, 0, sizeof(master_t)); - setProfile(0); + memset(config, 0, sizeof(master_t)); - featureClearAll(); - featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES); + intFeatureClearAll(config); + intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config); #ifdef DEFAULT_FEATURES - featureSet(DEFAULT_FEATURES); + intFeatureSet(DEFAULT_FEATURES, config); #endif #ifdef OSD - resetOsdConfig(); + intFeatureSet(FEATURE_OSD, config); + resetOsdConfig(&config->osdProfile); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. - featureSet(FEATURE_VBAT); + intFeatureSet(FEATURE_VBAT, config); #endif - - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.mixerMode = MIXER_QUADX; + config->version = EEPROM_CONF_VERSION; + config->mixerMode = MIXER_QUADX; // global settings - masterConfig.current_profile_index = 0; // default profile - masterConfig.dcm_kp = 2500; // 1.0 * 10000 - masterConfig.dcm_ki = 0; // 0.003 * 10000 - masterConfig.gyro_lpf = 0; // 256HZ default + config->current_profile_index = 0; // default profile + config->dcm_kp = 2500; // 1.0 * 10000 + config->dcm_ki = 0; // 0.003 * 10000 + config->gyro_lpf = 0; // 256HZ default #ifdef STM32F10X - masterConfig.gyro_sync_denom = 8; - masterConfig.pid_process_denom = 1; + config->gyro_sync_denom = 8; + config->pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) - masterConfig.gyro_sync_denom = 1; - masterConfig.pid_process_denom = 4; + config->gyro_sync_denom = 1; + config->pid_process_denom = 4; #else - masterConfig.gyro_sync_denom = 4; - masterConfig.pid_process_denom = 2; + config->gyro_sync_denom = 4; + config->pid_process_denom = 2; #endif - masterConfig.gyro_soft_type = FILTER_PT1; - masterConfig.gyro_soft_lpf_hz = 90; - masterConfig.gyro_soft_notch_hz = 0; - masterConfig.gyro_soft_notch_cutoff = 150; + config->gyro_soft_type = FILTER_PT1; + config->gyro_soft_lpf_hz = 90; + config->gyro_soft_notch_hz = 0; + config->gyro_soft_notch_cutoff = 150; - masterConfig.debug_mode = DEBUG_NONE; + config->debug_mode = DEBUG_NONE; - resetAccelerometerTrims(&masterConfig.accZero); + resetAccelerometerTrims(&config->accZero); - resetSensorAlignment(&masterConfig.sensorAlignmentConfig); + resetSensorAlignment(&config->sensorAlignmentConfig); - masterConfig.boardAlignment.rollDegrees = 0; - masterConfig.boardAlignment.pitchDegrees = 0; - masterConfig.boardAlignment.yawDegrees = 0; - masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect - masterConfig.max_angle_inclination = 700; // 70 degrees - masterConfig.yaw_control_direction = 1; - masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; + config->boardAlignment.rollDegrees = 0; + config->boardAlignment.pitchDegrees = 0; + config->boardAlignment.yawDegrees = 0; + config->acc_hardware = ACC_DEFAULT; // default/autodetect + config->max_angle_inclination = 700; // 70 degrees + config->yaw_control_direction = 1; + config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable - masterConfig.mag_hardware = 0; + config->mag_hardware = 0; - masterConfig.baro_hardware = 0; + config->baro_hardware = 0; - resetBatteryConfig(&masterConfig.batteryConfig); + resetBatteryConfig(&config->batteryConfig); #ifdef TELEMETRY - resetTelemetryConfig(&masterConfig.telemetryConfig); + resetTelemetryConfig(&config->telemetryConfig); #endif #ifdef SERIALRX_PROVIDER - masterConfig.rxConfig.serialrx_provider = SERIALRX_PROVIDER; + config->rxConfig.serialrx_provider = SERIALRX_PROVIDER; #else - masterConfig.rxConfig.serialrx_provider = 0; + config->rxConfig.serialrx_provider = 0; #endif - masterConfig.rxConfig.sbus_inversion = 1; - masterConfig.rxConfig.spektrum_sat_bind = 0; - masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; - masterConfig.rxConfig.midrc = 1500; - masterConfig.rxConfig.mincheck = 1100; - masterConfig.rxConfig.maxcheck = 1900; - masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection - masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection + config->rxConfig.sbus_inversion = 1; + config->rxConfig.spektrum_sat_bind = 0; + config->rxConfig.spektrum_sat_bind_autoreset = 1; + config->rxConfig.midrc = 1500; + config->rxConfig.mincheck = 1100; + config->rxConfig.maxcheck = 1900; + config->rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection + config->rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &config->rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; - channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); + channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc); } - masterConfig.rxConfig.rssi_channel = 0; - masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; - masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; - masterConfig.rxConfig.rcInterpolationInterval = 19; - masterConfig.rxConfig.fpvCamAngleDegrees = 0; - masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS; - masterConfig.rxConfig.airModeActivateThreshold = 1350; + config->rxConfig.rssi_channel = 0; + config->rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; + config->rxConfig.rssi_ppm_invert = 0; + config->rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; + config->rxConfig.rcInterpolationInterval = 19; + config->rxConfig.fpvCamAngleDegrees = 0; + config->rxConfig.max_aux_channel = MAX_AUX_CHANNELS; + config->rxConfig.airModeActivateThreshold = 1350; - resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); + resetAllRxChannelRangeConfigurations(config->rxConfig.channelRanges); - masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; + config->inputFilteringMode = INPUT_FILTERING_DISABLED; - masterConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support - masterConfig.disarm_kill_switch = 1; - masterConfig.auto_disarm_delay = 5; - masterConfig.small_angle = 25; + config->gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support + config->disarm_kill_switch = 1; + config->auto_disarm_delay = 5; + config->small_angle = 25; - resetMixerConfig(&masterConfig.mixerConfig); + resetMixerConfig(&config->mixerConfig); - masterConfig.airplaneConfig.fixedwing_althold_dir = 1; + config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo - resetEscAndServoConfig(&masterConfig.escAndServoConfig); - resetFlight3DConfig(&masterConfig.flight3DConfig); + resetEscAndServoConfig(&config->escAndServoConfig); + resetFlight3DConfig(&config->flight3DConfig); #ifdef BRUSHED_MOTORS - masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; - masterConfig.motor_pwm_protocol = PWM_TYPE_BRUSHED; - masterConfig.use_unsyncedPwm = true; + config->motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; + config->motor_pwm_protocol = PWM_TYPE_BRUSHED; + config->use_unsyncedPwm = true; #else - masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; - masterConfig.motor_pwm_protocol = PWM_TYPE_ONESHOT125; + config->motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; + config->motor_pwm_protocol = PWM_TYPE_ONESHOT125; #endif - masterConfig.servo_pwm_rate = 50; + + config->servo_pwm_rate = 50; #ifdef CC3D - masterConfig.use_buzzer_p6 = 0; + config->use_buzzer_p6 = 0; #endif #ifdef GPS // gps/nav stuff - masterConfig.gpsConfig.provider = GPS_NMEA; - masterConfig.gpsConfig.sbasMode = SBAS_AUTO; - masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; - masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; + config->gpsConfig.provider = GPS_NMEA; + config->gpsConfig.sbasMode = SBAS_AUTO; + config->gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; + config->gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif - resetSerialConfig(&masterConfig.serialConfig); + resetSerialConfig(&config->serialConfig); - masterConfig.emf_avoidance = 0; // TODO - needs removal + config->emf_avoidance = 0; // TODO - needs removal - resetProfile(currentProfile); + resetProfile(&config->profile[0]); - resetRollAndPitchTrims(&masterConfig.accelerometerTrims); + resetRollAndPitchTrims(&config->accelerometerTrims); - masterConfig.mag_declination = 0; - masterConfig.acc_lpf_hz = 10.0f; - masterConfig.accDeadband.xy = 40; - masterConfig.accDeadband.z = 40; - masterConfig.acc_unarmedcal = 1; + config->mag_declination = 0; + config->acc_lpf_hz = 10.0f; + config->accDeadband.xy = 40; + config->accDeadband.z = 40; + config->acc_unarmedcal = 1; #ifdef BARO - resetBarometerConfig(&masterConfig.barometerConfig); + resetBarometerConfig(&config->barometerConfig); #endif // Radio #ifdef RX_CHANNELS_TAER - parseRcChannels("TAER1234", &masterConfig.rxConfig); + parseRcChannels("TAER1234", &config->rxConfig); #else - parseRcChannels("AETR1234", &masterConfig.rxConfig); + parseRcChannels("AETR1234", &config->rxConfig); #endif - resetRcControlsConfig(&masterConfig.rcControlsConfig); + resetRcControlsConfig(&config->rcControlsConfig); - masterConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv - masterConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv + config->throttle_correction_value = 0; // could 10 with althold or 40 for fpv + config->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables - masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec - masterConfig.failsafeConfig.failsafe_off_delay = 10; // 1sec - masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. - masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss - masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition - masterConfig.failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing + config->failsafeConfig.failsafe_delay = 10; // 1sec + config->failsafeConfig.failsafe_off_delay = 10; // 1sec + config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. + config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss + config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition + config->failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - masterConfig.servoConf[i].min = DEFAULT_SERVO_MIN; - masterConfig.servoConf[i].max = DEFAULT_SERVO_MAX; - masterConfig.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; - masterConfig.servoConf[i].rate = 100; - masterConfig.servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; - masterConfig.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; - masterConfig.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; + config->servoConf[i].min = DEFAULT_SERVO_MIN; + config->servoConf[i].max = DEFAULT_SERVO_MAX; + config->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; + config->servoConf[i].rate = 100; + config->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; + config->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; + config->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal - masterConfig.gimbalConfig.mode = GIMBAL_MODE_NORMAL; + config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; #endif #ifdef GPS - resetGpsProfile(&masterConfig.gpsProfile); + resetGpsProfile(&config->gpsProfile); #endif // custom mixer. clear by defaults. for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - masterConfig.customMotorMixer[i].throttle = 0.0f; + config->customMotorMixer[i].throttle = 0.0f; } #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors); - applyDefaultLedStripConfig(masterConfig.ledConfigs); - applyDefaultModeColors(masterConfig.modeColors); - applyDefaultSpecialColors(&(masterConfig.specialColors)); - masterConfig.ledstrip_visual_beeper = 0; + applyDefaultColors(config->colors); + applyDefaultLedStripConfig(config->ledConfigs); + applyDefaultModeColors(config->modeColors); + applyDefaultSpecialColors(&(config->specialColors)); + config->ledstrip_visual_beeper = 0; #endif #ifdef VTX - masterConfig.vtx_band = 4; //Fatshark/Airwaves - masterConfig.vtx_channel = 1; //CH1 - masterConfig.vtx_mode = 0; //CH+BAND mode - masterConfig.vtx_mhz = 5740; //F0 + config->vtx_band = 4; //Fatshark/Airwaves + config->vtx_channel = 1; //CH1 + config->vtx_mode = 0; //CH+BAND mode + config->vtx_mhz = 5740; //F0 #endif #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware - memcpy(masterConfig.transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); + memcpy(config->transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); #endif #ifdef BLACKBOX - #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) - featureSet(FEATURE_BLACKBOX); - masterConfig.blackbox_device = BLACKBOX_DEVICE_FLASH; + intFeatureSet(FEATURE_BLACKBOX, config); + config->blackbox_device = BLACKBOX_DEVICE_FLASH; #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) - featureSet(FEATURE_BLACKBOX); - masterConfig.blackbox_device = BLACKBOX_DEVICE_SDCARD; + intFeatureSet(FEATURE_BLACKBOX, config); + config->blackbox_device = BLACKBOX_DEVICE_SDCARD; #else - masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; + config->blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif - masterConfig.blackbox_rate_num = 1; - masterConfig.blackbox_rate_denom = 1; - + config->blackbox_rate_num = 1; + config->blackbox_rate_denom = 1; #endif // BLACKBOX #ifdef SERIALRX_UART if (featureConfigured(FEATURE_RX_SERIAL)) { - masterConfig.serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; + config->serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; } #endif #if defined(TARGET_CONFIG) - targetConfiguration(); + // Don't look at target specific config settings when getting default + // values for a CLI diff. This is suboptimal, but it doesn't break the + // public API + if (config == &masterConfig) { + targetConfiguration(); + } #endif // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { - memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); + memcpy(&config->profile[i], &config->profile[0], sizeof(profile_t)); } +} +static void resetConf(void) +{ + createDefaultConfig(&masterConfig); + + setProfile(0); + +#ifdef LED_STRIP + reevaluateLedConfig(); +#endif } static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) @@ -1067,17 +1084,32 @@ bool feature(uint32_t mask) void featureSet(uint32_t mask) { - masterConfig.enabledFeatures |= mask; + intFeatureSet(mask, &masterConfig); +} + +static void intFeatureSet(uint32_t mask, master_t *config) +{ + config->enabledFeatures |= mask; } void featureClear(uint32_t mask) { - masterConfig.enabledFeatures &= ~(mask); + intFeatureClear(mask, &masterConfig); +} + +static void intFeatureClear(uint32_t mask, master_t *config) +{ + config->enabledFeatures &= ~(mask); } void featureClearAll() { - masterConfig.enabledFeatures = 0; + intFeatureClearAll(&masterConfig); +} + +static void intFeatureClearAll(master_t *config) +{ + config->enabledFeatures = 0; } uint32_t featureMask(void) diff --git a/src/main/config/config.h b/src/main/config/config.h index cc239da92..6570440c3 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -26,7 +26,6 @@ #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define MAX_NAME_LENGTH 16 - typedef enum { FEATURE_RX_PPM = 1 << 0, FEATURE_VBAT = 1 << 1, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 9e37154d1..0f79ee2f4 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -174,3 +174,5 @@ typedef struct master_t { extern master_t masterConfig; extern profile_t *currentProfile; extern controlRateConfig_t *currentControlRateProfile; + +void createDefaultConfig(master_t *config); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 573d56237..b707a2a01 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1223,9 +1223,6 @@ void pgResetFn_specialColors(specialColorIndexes_t *instance) void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); - memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); - - reevaluateLedConfig(); } void applyDefaultColors(hsvColor_t *colors) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index abd862dd1..e7e02eac9 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -831,24 +831,22 @@ void osdInit(void) max7456_draw_screen(); } -void resetOsdConfig(void) +void resetOsdConfig(osd_profile *osdProfile) { - featureSet(FEATURE_OSD); - masterConfig.osdProfile.video_system = AUTO; - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; - masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; - masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; - masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; - masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; - masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; - masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; - masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; - masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = -1; - masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; - masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW] = -23; - masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] = -18; - masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] = 1; + osdProfile->video_system = AUTO; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + osdProfile->item_pos[OSD_RSSI_VALUE] = -59; + osdProfile->item_pos[OSD_TIMER] = -39; + osdProfile->item_pos[OSD_THROTTLE_POS] = -9; + osdProfile->item_pos[OSD_CPU_LOAD] = 26; + osdProfile->item_pos[OSD_VTX_CHANNEL] = 1; + osdProfile->item_pos[OSD_VOLTAGE_WARNING] = -80; + osdProfile->item_pos[OSD_ARMED] = -107; + osdProfile->item_pos[OSD_DISARMED] = -109; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = -1; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = -1; + osdProfile->item_pos[OSD_CURRENT_DRAW] = -23; + osdProfile->item_pos[OSD_MAH_DRAWN] = -18; + osdProfile->item_pos[OSD_CRAFT_NAME] = 1; } - #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b5f4e953d..0685f9897 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -65,4 +65,4 @@ typedef struct { void updateOsd(void); void osdInit(void); -void resetOsdConfig(void); +void resetOsdConfig(osd_profile *osdProfile); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 463ffb72d..179b18779 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -114,8 +114,10 @@ static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); -void cliDumpProfile(uint8_t profileIndex); -void cliDumpRateProfile(uint8_t rateProfileIndex) ; +static void cliDiff(char *cmdLine); +static void printConfig(char *cmdLine, bool doDiff); +static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *defaultProfile); +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultProfile) ; static void cliExit(char *cmdline); static void cliFeature(char *cmdline); static void cliMotor(char *cmdline); @@ -189,6 +191,17 @@ static void cliBeeper(char *cmdline); static char cliBuffer[48]; static uint32_t bufferIndex = 0; +typedef enum { + DUMP_MASTER = (1 << 0), + DUMP_PROFILE = (1 << 1), + DUMP_RATES = (1 << 2), + DUMP_ALL = (1 << 3), + DO_DIFF = (1 << 4), + DIFF_COMMENTED = (1 << 5), +} dumpFlags_e; + +static const char* const sectionBreak = "\r\n"; + #ifndef USE_QUAD_MIXER_ONLY // sync this with mixerMode_e static const char * const mixerNames[] = { @@ -269,7 +282,10 @@ const clicmd_t cmdTable[] = { #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), - CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile]", cliDump), + CLI_COMMAND_DEF("diff", "list configuration changes from default", + "[master|profile|rates|all] {commented}", cliDiff), + CLI_COMMAND_DEF("dump", "dump configuration", + "[master|profile|rates|all]", cliDump), CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), CLI_COMMAND_DEF("feature", "configure features", "list\r\n" @@ -923,6 +939,11 @@ static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); +#define printSectionBreak() cliPrintf((char *)sectionBreak) + +#define COMPARE_CONFIG(value) (masterConfig.value == defaultConfig.value) +static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); + static void cliPrompt(void) { cliPrint("\r\n# "); @@ -968,6 +989,36 @@ static bool isEmpty(const char *string) return *string == '\0'; } +static void printRxFail(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out rxConfig failsafe settings + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration; + rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault; + bool equalsDefault; + bool requireValue; + char modeCharacter; + for (uint8_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { + channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[channel]; + channelFailsafeConfigurationDefault = &defaultConfig->rxConfig.failsafe_channel_configurations[channel]; + equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode + && channelFailsafeConfiguration->step == channelFailsafeConfigurationDefault->step; + requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; + modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; + if (requireValue) { + cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c %d\r\n", + channel, + modeCharacter, + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step) + ); + } else { + cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c\r\n", + channel, + modeCharacter + ); + } + } +} + static void cliRxFail(char *cmdline) { uint8_t channel; @@ -1030,10 +1081,9 @@ static void cliRxFail(char *cmdline) char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; - // triple use of cliPrintf below + // double use of cliPrintf below // 1. acknowledge interpretation on command, // 2. query current setting on single item, - // 3. recursive use for full list. if (requireValue) { cliPrintf("rxfail %u %c %d\r\n", @@ -1053,23 +1103,36 @@ static void cliRxFail(char *cmdline) } } +static void printAux(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out aux channel settings + modeActivationCondition_t *mac; + modeActivationCondition_t *macDefault; + bool equalsDefault; + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + mac = &masterConfig.modeActivationConditions[i]; + macDefault = &defaultConfig->modeActivationConditions[i]; + equalsDefault = mac->modeId == macDefault->modeId + && mac->auxChannelIndex == macDefault->auxChannelIndex + && mac->range.startStep == macDefault->range.startStep + && mac->range.endStep == macDefault->range.endStep; + cliDumpPrintf(dumpMask, equalsDefault, "aux %u %u %u %u %u\r\n", + i, + mac->modeId, + mac->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) + ); + } +} + static void cliAux(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out aux channel settings - for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; - cliPrintf("aux %u %u %u %u %u\r\n", - i, - mac->modeId, - mac->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) - ); - } + printAux(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -1103,28 +1166,44 @@ static void cliAux(char *cmdline) } } +static void printSerial(uint8_t dumpMask, master_t *defaultConfig) +{ + serialConfig_t *serialConfig; + serialConfig_t *serialConfigDefault; + bool equalsDefault; + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + serialConfig = &masterConfig.serialConfig; + if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { + continue; + }; + serialConfigDefault = &defaultConfig->serialConfig; + equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier + && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask + && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex + && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex + && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex + && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; + cliDumpPrintf(dumpMask, equalsDefault, "serial %d %d %ld %ld %ld %ld\r\n" , + serialConfig->portConfigs[i].identifier, + serialConfig->portConfigs[i].functionMask, + baudRates[serialConfig->portConfigs[i].msp_baudrateIndex], + baudRates[serialConfig->portConfigs[i].gps_baudrateIndex], + baudRates[serialConfig->portConfigs[i].telemetry_baudrateIndex], + baudRates[serialConfig->portConfigs[i].blackbox_baudrateIndex] + ); + } +} + static void cliSerial(char *cmdline) { int i, val; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < SERIAL_PORT_COUNT; i++) { - if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { - continue; - }; - cliPrintf("serial %d %d %ld %ld %ld %ld\r\n" , - masterConfig.serialConfig.portConfigs[i].identifier, - masterConfig.serialConfig.portConfigs[i].functionMask, - baudRates[masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex] - ); - } - return; - } + printSerial(DUMP_MASTER, &masterConfig); + return; + } serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); @@ -1270,25 +1349,40 @@ static void cliSerialPassthrough(char *cmdline) } #endif +static void printAdjustmentRange(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out adjustment ranges channel settings + adjustmentRange_t *ar; + adjustmentRange_t *arDefault; + bool equalsDefault; + for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + ar = &masterConfig.adjustmentRanges[i]; + arDefault = &defaultConfig->adjustmentRanges[i]; + equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex + && ar->range.startStep == arDefault->range.startStep + && ar->range.endStep == arDefault->range.endStep + && ar->adjustmentFunction == arDefault->adjustmentFunction + && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex + && ar->adjustmentIndex == arDefault->adjustmentIndex; + cliDumpPrintf(dumpMask, equalsDefault, "adjrange %u %u %u %u %u %u %u\r\n", + i, + ar->adjustmentIndex, + ar->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), + ar->adjustmentFunction, + ar->auxSwitchChannelIndex + ); + } +} + static void cliAdjustmentRange(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out adjustment ranges channel settings - for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *ar = &masterConfig.adjustmentRanges[i]; - cliPrintf("adjrange %u %u %u %u %u %u %u\r\n", - i, - ar->adjustmentIndex, - ar->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), - ar->adjustmentFunction, - ar->auxSwitchChannelIndex - ); - } + printAdjustmentRange(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -1423,16 +1517,27 @@ static void cliMotorMix(char *cmdline) #endif } +static void printRxRange(uint8_t dumpMask, master_t *defaultConfig) +{ + rxChannelRangeConfiguration_t *channelRangeConfiguration; + rxChannelRangeConfiguration_t *channelRangeConfigurationDefault; + bool equalsDefault; + for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { + channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; + channelRangeConfigurationDefault = &defaultConfig->rxConfig.channelRanges[i]; + equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min + && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; + cliDumpPrintf(dumpMask, equalsDefault, "rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); + } +} + static void cliRxRange(char *cmdline) { int i, validArgumentCount = 0; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - rxChannelRangeConfiguration_t *channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; - cliPrintf("rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); - } + printRxRange(DUMP_MASTER, &masterConfig); } else if (strcasecmp(cmdline, "reset") == 0) { resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); } else { @@ -1469,17 +1574,28 @@ static void cliRxRange(char *cmdline) } #ifdef LED_STRIP +static void printLed(uint8_t dumpMask, master_t *defaultConfig) +{ + bool equalsDefault; + ledConfig_t ledConfig; + ledConfig_t ledConfigDefault; + char ledConfigBuffer[20]; + for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + ledConfig = masterConfig.ledConfigs[i]; + ledConfigDefault = defaultConfig->ledConfigs[i]; + equalsDefault = ledConfig == ledConfigDefault; + generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); + cliDumpPrintf(dumpMask, equalsDefault, "led %u %s\r\n", i, ledConfigBuffer); + } +} + static void cliLed(char *cmdline) { int i; char *ptr; - char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); - cliPrintf("led %u %s\r\n", i, ledConfigBuffer); - } + printLed(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr); @@ -1494,20 +1610,33 @@ static void cliLed(char *cmdline) } } +static void printColor(uint8_t dumpMask, master_t *defaultConfig) +{ + hsvColor_t *color; + hsvColor_t *colorDefault; + bool equalsDefault; + for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + color = &masterConfig.colors[i]; + colorDefault = &defaultConfig->colors[i]; + equalsDefault = color->h == colorDefault->h + && color->s == colorDefault->s + && color->v == colorDefault->v; + cliDumpPrintf(dumpMask, equalsDefault, "color %u %d,%u,%u\r\n", + i, + color->h, + color->s, + color->v + ); + } +} + static void cliColor(char *cmdline) { int i; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - cliPrintf("color %u %d,%u,%u\r\n", - i, - masterConfig.colors[i].h, - masterConfig.colors[i].s, - masterConfig.colors[i].v - ); - } + printColor(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr); @@ -1522,20 +1651,27 @@ static void cliColor(char *cmdline) } } +static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) +{ + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = masterConfig.modeColors[i].color[j]; + int colorIndexDefault = defaultConfig->modeColors[i].color[j]; + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", i, j, colorIndex); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + int colorIndex = masterConfig.specialColors.color[j]; + int colorIndexDefault = defaultConfig->specialColors.color[j]; + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + } +} + static void cliModeColor(char *cmdline) { if (isEmpty(cmdline)) { - for (int i = 0; i < LED_MODE_COUNT; i++) { - for (int j = 0; j < LED_DIRECTION_COUNT; j++) { - int colorIndex = modeColors[i].color[j]; - cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); - } - } - - for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - int colorIndex = specialColors.color[j]; - cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); - } + printModeColor(DUMP_MASTER, &masterConfig); } else { enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; int args[ARGS_COUNT]; @@ -1565,6 +1701,52 @@ static void cliModeColor(char *cmdline) #endif #ifdef USE_SERVOS +static void printServo(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out servo settings + int i; + servoParam_t *servoConf; + servoParam_t *servoConfDefault; + bool equalsDefault; + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoConf = &masterConfig.servoConf[i]; + servoConfDefault = &defaultConfig->servoConf[i]; + equalsDefault = servoConf->min == servoConfDefault->min + && servoConf->max == servoConfDefault->max + && servoConf->middle == servoConfDefault->middle + && servoConf->angleAtMin == servoConfDefault->angleAtMax + && servoConf->rate == servoConfDefault->rate + && servoConf->forwardFromChannel == servoConfDefault->forwardFromChannel; + + cliDumpPrintf(dumpMask, equalsDefault, "servo %u %d %d %d %d %d %d %d\r\n", + i, + servoConf->min, + servoConf->max, + servoConf->middle, + servoConf->angleAtMin, + servoConf->angleAtMax, + servoConf->rate, + servoConf->forwardFromChannel + ); + } + + printSectionBreak(); + + // print servo directions + unsigned int channel; + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoConf = &masterConfig.servoConf[i]; + servoConfDefault = &defaultConfig->servoConf[i]; + equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; + for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + if (servoConf->reversedSources & (1 << channel)) { + equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); + cliDumpPrintf(dumpMask, equalsDefault, "smix reverse %d %d r\r\n", i , channel); + } + } + } +} + static void cliServo(char *cmdline) { enum { SERVO_ARGUMENT_COUNT = 8 }; @@ -1576,21 +1758,7 @@ static void cliServo(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - // print out servo settings - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo = &masterConfig.servoConf[i]; - - cliPrintf("servo %u %d %d %d %d %d %d %d\r\n", - i, - servo->min, - servo->max, - servo->middle, - servo->angleAtMin, - servo->angleAtMax, - servo->rate, - servo->forwardFromChannel - ); - } + printServo(DUMP_MASTER, &masterConfig); } else { int validArgumentCount = 0; @@ -1947,24 +2115,38 @@ static void cliFlashRead(char *cmdline) #endif #ifdef VTX +static void printVtx(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out vtx channel settings + vtxChannelActivationCondition_t *cac; + vtxChannelActivationCondition_t *cacDefault; + bool equalsDefault; + for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { + cac = &masterConfig.vtxChannelActivationConditions[i]; + cacDefault = &defaultConfig->vtxChannelActivationConditions[i]; + equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex + && cac->band == cacDefault->band + && cac->channel == cacDefault->channel + && cac->range.startStep == cacDefault->range.startStep + && cac->range.endStep == cacDefault->range.endStep; + cliDumpPrintf(dumpMask, equalsDefault, "vtx %u %u %u %u %u %u\r\n", + i, + cac->auxChannelIndex, + cac->band, + cac->channel, + MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) + ); + } +} + static void cliVtx(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out vtx channel settings - for (i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { - vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; - printf("vtx %u %u %u %u %u %u\r\n", - i, - cac->auxChannelIndex, - cac->band, - cac->channel, - MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) - ); - } + printVtx(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -2007,7 +2189,72 @@ static void cliVtx(char *cmdline) } #endif -static void dumpValues(uint16_t valueSection) +static void printName(uint8_t dumpMask) +{ + cliDumpPrintf(dumpMask, strlen(masterConfig.name) == 0, "name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); +} + +static void cliName(char *cmdline) +{ + uint32_t len = strlen(cmdline); + if (len > 0) { + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + } + printName(DUMP_MASTER); +} + +void *getValuePointer(const clivalue_t *value) +{ + void *ptr = value->ptr; + + if ((value->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); + } + + if ((value->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); + } + + return ptr; +} + +static bool valueEqualsDefault(const clivalue_t *value, master_t *defaultConfig) +{ + void *ptr = getValuePointer(value); + + void *ptrDefault = ((uint8_t *)ptr) - (uint32_t)&masterConfig + (uint32_t)defaultConfig; + + bool result = false; + switch (value->type & VALUE_TYPE_MASK) { + case VAR_UINT8: + result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; + break; + + case VAR_INT8: + result = *(int8_t *)ptr == *(int8_t *)ptrDefault; + break; + + case VAR_UINT16: + result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; + break; + + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; + + case VAR_UINT32: + result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; + break; + + case VAR_FLOAT: + result = *(float *)ptr == *(float *)ptrDefault; + break; + } + return result; +} + +static void dumpValues(uint16_t valueSection, uint8_t dumpMask, master_t *defaultConfig) { uint32_t i; const clivalue_t *value; @@ -2018,64 +2265,83 @@ static void dumpValues(uint16_t valueSection) continue; } - cliPrintf("set %s = ", valueTable[i].name); - cliPrintVar(value, 0); - cliPrint("\r\n"); + if (cliDumpPrintf(dumpMask, valueEqualsDefault(value, defaultConfig), "set %s = ", valueTable[i].name)) { + cliPrintVar(value, 0); + cliPrint("\r\n"); + } } } -typedef enum { - DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), -} dumpFlags_e; - -static const char* const sectionBreak = "\r\n"; - -#define printSectionBreak() cliPrintf((char *)sectionBreak) - static void cliDump(char *cmdline) +{ + printConfig(cmdline, false); +} + +static void cliDiff(char *cmdline) +{ + printConfig(cmdline, true); +} + +char *checkCommand(char *cmdLine, const char *command) +{ + if(!strncasecmp(cmdLine, command, strlen(command)) // command names match + && !isalnum((unsigned)cmdLine[strlen(command)])) { // next characted in bufffer is not alphanumeric (command is correctly terminated) + return cmdLine + strlen(command) + 1; + } else { + return 0; + } +} + +static void printConfig(char *cmdline, bool doDiff) { unsigned int i; char buf[16]; uint32_t mask; - -#ifndef USE_QUAD_MIXER_ONLY - float thr, roll, pitch, yaw; -#endif + uint32_t defaultMask; + bool equalsDefault; uint8_t dumpMask = DUMP_MASTER; - if (strcasecmp(cmdline, "master") == 0) { + char *options; + if ((options = checkCommand(cmdline, "master"))) { dumpMask = DUMP_MASTER; // only - } - if (strcasecmp(cmdline, "profile") == 0) { + } else if ((options = checkCommand(cmdline, "profile"))) { dumpMask = DUMP_PROFILE; // only - } - if (strcasecmp(cmdline, "rates") == 0) { - dumpMask = DUMP_RATES; + } else if ((options = checkCommand(cmdline, "rates"))) { + dumpMask = DUMP_RATES; // only + } else if ((options = checkCommand(cmdline, "all"))) { + dumpMask = DUMP_ALL; // all profiles and rates } - if (strcasecmp(cmdline, "all") == 0) { - dumpMask = DUMP_ALL; // All profiles and rates + master_t defaultConfig; + if (doDiff) { + dumpMask = dumpMask | DO_DIFF; + createDefaultConfig(&defaultConfig); + } + + if (checkCommand(options, "commented")) { + dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff } if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { - cliPrint("\r\n# version\r\n"); cliVersion(NULL); + cliPrint("\r\n"); + + if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { + cliPrint("\r\n# reset configuration to default settings\r\ndefaults\r\n"); + } printSectionBreak(); - cliPrint("\r\n# name\r\n"); - cliName(NULL); + cliPrint("# name\r\n"); + printName(dumpMask); cliPrint("\r\n# mixer\r\n"); - #ifndef USE_QUAD_MIXER_ONLY - cliPrintf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); + cliDumpPrintf(dumpMask, COMPARE_CONFIG(mixerMode), "mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); - cliPrintf("mmix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "mmix reset\r\n"); + float thr, roll, pitch, yaw, thrDefault, rollDefault, pitchDefault, yawDefault; for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { if (masterConfig.customMotorMixer[i].throttle == 0.0f) break; @@ -2083,31 +2349,45 @@ static void cliDump(char *cmdline) roll = masterConfig.customMotorMixer[i].roll; pitch = masterConfig.customMotorMixer[i].pitch; yaw = masterConfig.customMotorMixer[i].yaw; - cliPrintf("mmix %d", i); - if (thr < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(thr, buf)); - if (roll < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(roll, buf)); - if (pitch < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(pitch, buf)); - if (yaw < 0) - cliWrite(' '); - cliPrintf("%s\r\n", ftoa(yaw, buf)); + thrDefault = defaultConfig.customMotorMixer[i].throttle; + rollDefault = defaultConfig.customMotorMixer[i].roll; + pitchDefault = defaultConfig.customMotorMixer[i].pitch; + yawDefault = defaultConfig.customMotorMixer[i].yaw; + equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; + + if (cliDumpPrintf(dumpMask, equalsDefault, "mmix %d", i)) { + if (thr < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(thr, buf)); + if (roll < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(roll, buf)); + if (pitch < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(pitch, buf)); + if (yaw < 0) + cliWrite(' '); + cliPrintf("%s\r\n", ftoa(yaw, buf)); + } } #ifdef USE_SERVOS // print custom servo mixer if exists - cliPrintf("smix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customServoMixer[i].rate == 0, "smix reset\r\n"); for (i = 0; i < MAX_SERVO_RULES; i++) { - if (masterConfig.customServoMixer[i].rate == 0) break; - cliPrintf("smix %d %d %d %d %d %d %d %d\r\n", + equalsDefault = masterConfig.customServoMixer[i].targetChannel == defaultConfig.customServoMixer[i].targetChannel + && masterConfig.customServoMixer[i].inputSource == defaultConfig.customServoMixer[i].inputSource + && masterConfig.customServoMixer[i].rate == defaultConfig.customServoMixer[i].rate + && masterConfig.customServoMixer[i].speed == defaultConfig.customServoMixer[i].speed + && masterConfig.customServoMixer[i].min == defaultConfig.customServoMixer[i].min + && masterConfig.customServoMixer[i].max == defaultConfig.customServoMixer[i].max + && masterConfig.customServoMixer[i].box == masterConfig.customServoMixer[i].box; + + cliDumpPrintf(dumpMask, equalsDefault,"smix %d %d %d %d %d %d %d %d\r\n", i, masterConfig.customServoMixer[i].targetChannel, masterConfig.customServoMixer[i].inputSource, @@ -2118,102 +2398,96 @@ static void cliDump(char *cmdline) masterConfig.customServoMixer[i].box ); } - #endif #endif cliPrint("\r\n# feature\r\n"); + mask = featureMask(); + defaultMask = defaultConfig.enabledFeatures; for (i = 0; ; i++) { // disable all feature first if (featureNames[i] == NULL) break; - cliPrintf("feature -%s\r\n", featureNames[i]); + cliDumpPrintf(dumpMask, (mask | ~defaultMask) & (1 << i), "feature -%s\r\n", featureNames[i]); } for (i = 0; ; i++) { // reenable what we want. if (featureNames[i] == NULL) break; if (mask & (1 << i)) - cliPrintf("feature %s\r\n", featureNames[i]); + cliDumpPrintf(dumpMask, defaultMask & (1 << i), "feature %s\r\n", featureNames[i]); } #ifdef BEEPER cliPrint("\r\n# beeper\r\n"); uint8_t beeperCount = beeperTableEntryCount(); mask = getBeeperOffMask(); + defaultMask = defaultConfig.beeper_off_flags; for (int i = 0; i < (beeperCount-2); i++) { if (mask & (1 << i)) - cliPrintf("beeper -%s\r\n", beeperNameForTableIndex(i)); + cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper -%s\r\n", beeperNameForTableIndex(i)); else - cliPrintf("beeper %s\r\n", beeperNameForTableIndex(i)); + cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper %s\r\n", beeperNameForTableIndex(i)); } #endif cliPrint("\r\n# map\r\n"); - for (i = 0; i < 8; i++) + equalsDefault = true; + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; + equalsDefault = equalsDefault && (masterConfig.rxConfig.rcmap[i] == defaultConfig.rxConfig.rcmap[i]); + } buf[i] = '\0'; - cliPrintf("map %s\r\n", buf); + cliDumpPrintf(dumpMask, equalsDefault, "map %s\r\n", buf); cliPrint("\r\n# serial\r\n"); - cliSerial(""); + printSerial(dumpMask, &defaultConfig); #ifdef LED_STRIP cliPrint("\r\n# led\r\n"); - cliLed(""); + printLed(dumpMask, &defaultConfig); cliPrint("\r\n# color\r\n"); - cliColor(""); + printColor(dumpMask, &defaultConfig); cliPrint("\r\n# mode_color\r\n"); - cliModeColor(""); + printModeColor(dumpMask, &defaultConfig); #endif cliPrint("\r\n# aux\r\n"); - cliAux(""); + printAux(dumpMask, &defaultConfig); cliPrint("\r\n# adjrange\r\n"); - cliAdjustmentRange(""); + printAdjustmentRange(dumpMask, &defaultConfig); cliPrintf("\r\n# rxrange\r\n"); - cliRxRange(""); + printRxRange(dumpMask, &defaultConfig); #ifdef USE_SERVOS cliPrint("\r\n# servo\r\n"); - cliServo(""); - - // print servo directions - unsigned int channel; - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - if (servoDirection(i, channel) < 0) { - cliPrintf("smix reverse %d %d r\r\n", i , channel); - } - } - } + printServo(dumpMask, &defaultConfig); #endif #ifdef VTX cliPrint("\r\n# vtx\r\n"); - cliVtx(""); + printVtx(dumpMask, &defaultConfig); #endif - cliPrint("\r\n# master\r\n"); - dumpValues(MASTER_VALUE); - cliPrint("\r\n# rxfail\r\n"); - cliRxFail(""); + printRxFail(dumpMask, &defaultConfig); + + cliPrint("\r\n# master\r\n"); + dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); if (dumpMask & DUMP_ALL) { uint8_t activeProfile = masterConfig.current_profile_index; uint8_t profileCount; for (profileCount=0; profileCountactiveRateProfile; uint8_t rateCount; for (rateCount=0; rateCountactiveRateProfile); + cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig); + cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } } if (dumpMask & DUMP_PROFILE) { - cliDumpProfile(masterConfig.current_profile_index); + cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig); } if (dumpMask & DUMP_RATES) { - cliDumpRateProfile(currentProfile->activeRateProfile); + cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } } -void cliDumpProfile(uint8_t profileIndex) +static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; + changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); + printSectionBreak(); - dumpValues(PROFILE_VALUE); + dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); + + cliRateProfile(""); } -void cliDumpRateProfile(uint8_t rateProfileIndex) +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values return; @@ -2260,7 +2537,8 @@ void cliDumpRateProfile(uint8_t rateProfileIndex) cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE); + + dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } void cliEnter(serialPort_t *serialPort) @@ -2574,18 +2852,6 @@ static void cliMotor(char *cmdline) cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); } -static void cliName(char *cmdline) -{ - uint32_t len = strlen(cmdline); - if (len > 0) { - memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); - strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); - } - cliPrintf("name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); - - return; -} - static void cliPlaySound(char *cmdline) { #if FLASH_SIZE <= 64 @@ -2709,6 +2975,26 @@ static void cliPutp(void *p, char ch) bufWriterAppend(p, ch); } +static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...) +{ + bool printValue = !((dumpMask & DO_DIFF) && equalsDefault); + bool printComment = (dumpMask & DIFF_COMMENTED); + if (printValue || printComment) { + if (printComment && !printValue) { + cliWrite('#'); + } + + va_list va; + va_start(va, format); + tfp_format(cliWriter, cliPutp, format, va); + va_end(va); + + return true; + } + + return false; +} + static void cliPrintf(const char *fmt, ...) { va_list va; @@ -2731,14 +3017,7 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) int32_t value = 0; char buf[8]; - void *ptr = var->ptr; - if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); - } - - if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); - } + void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -3142,13 +3421,14 @@ void cliProcess(void) cliBuffer[bufferIndex] = 0; // null terminate const clicmd_t *cmd; + char *options; for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { - if(!strncasecmp(cliBuffer, cmd->name, strlen(cmd->name)) // command names match - && !isalnum((unsigned)cliBuffer[strlen(cmd->name)])) // next characted in bufffer is not alphanumeric (command is correctly terminated) + if ((options = checkCommand(cliBuffer, cmd->name))) { break; + } } if(cmd < cmdTable + CMD_COUNT) - cmd->func(cliBuffer + strlen(cmd->name) + 1); + cmd->func(options); else cliPrint("Unknown command, try 'help'"); bufferIndex = 0; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 2c61276d1..342e4c069 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -91,7 +91,7 @@ #define VBAT_ADC_PIN PA0 #define RSSI_ADC_PIN PB0 -#define LED_STRIP +//#define LED_STRIP #define WS2811_PIN PB4 #define WS2811_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 @@ -103,20 +103,20 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define SONAR -#define SONAR_ECHO_PIN PB0 -#define SONAR_TRIGGER_PIN PB5 +//#define SONAR +//#define SONAR_ECHO_PIN PB0 +//#define SONAR_TRIGGER_PIN PB5 #undef GPS #ifdef CC3D_OPBL -//#undef LED_STRIP +#undef LED_STRIP #define SKIP_CLI_COMMAND_HELP #define SKIP_PID_FLOAT #undef BARO #undef MAG #undef SONAR -#undef LED_STRIP +#undef USE_SOFTSERIAL1 #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0e8ba62be..212ea2c83 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -110,7 +110,7 @@ #define SONAR_TRIGGER_PIN_PWM PB8 #define SONAR_ECHO_PIN_PWM PB9 -#define DISPLAY +//#define DISPLAY #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 66e83fa3d..06a49b25a 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -81,13 +81,13 @@ #define USE_FLASHTOOLS #define USE_FLASH_M25P16 -#define SONAR +//#define SONAR #define SONAR_TRIGGER_PIN PB0 #define SONAR_ECHO_PIN PB1 #define SONAR_TRIGGER_PIN_PWM PB8 #define SONAR_ECHO_PIN_PWM PB9 -#define DISPLAY +//#define DISPLAY #define USE_UART1 #define USE_UART2 @@ -115,6 +115,12 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 +//#define LED_STRIP +//#define LED_STRIP_TIMER TIM3 + +#define SKIP_CLI_COMMAND_HELP +#define SKIP_PID_LUXFLOAT + #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f103RCT6 in 64pin package From 1300952472579016c4f28ce97414af22ed9df69f Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 14:52:22 +0200 Subject: [PATCH 07/26] Added MPU9250 ACC detection. --- src/main/io/serial_cli.c | 2 +- src/main/sensors/acceleration.h | 19 ++++++++++--------- src/main/sensors/initialisation.c | 14 ++++++++++++++ 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 463ffb72d..56a3f037a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -228,7 +228,7 @@ static const char * const sensorTypeNames[] = { static const char * const sensorHardwareNames[4][11] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, - { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL }, + { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, { "", "None", "HMC5883", "AK8975", "AK8963", NULL } }; diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index dc98ae78e..62bf40582 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -20,15 +20,16 @@ // Type of accelerometer used/detected typedef enum { ACC_DEFAULT = 0, - ACC_NONE = 1, - ACC_ADXL345 = 2, - ACC_MPU6050 = 3, - ACC_MMA8452 = 4, - ACC_BMA280 = 5, - ACC_LSM303DLHC = 6, - ACC_MPU6000 = 7, - ACC_MPU6500 = 8, - ACC_FAKE = 9, + ACC_NONE, + ACC_ADXL345, + ACC_MPU6050, + ACC_MMA8452, + ACC_BMA280, + ACC_LSM303DLHC, + ACC_MPU6000, + ACC_MPU6500, + ACC_MPU9250, + ACC_FAKE, ACC_MAX = ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bb46059ac..2529a8c33 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -384,6 +384,20 @@ retry: } #endif ; // fallthrough + case ACC_MPU9250: +#ifdef USE_ACC_SPI_MPU9250 + + if (mpu9250SpiAccDetect(&acc)) + { + accHardware = ACC_MPU9250; +#ifdef ACC_MPU9250_ALIGN + accAlign = ACC_MPU9250_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough case ACC_FAKE: #ifdef USE_FAKE_ACC if (fakeAccDetect(&acc)) { From cb2b63112d31604f2fe25d8f0c809c3cb231a3a6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 15:18:21 +0200 Subject: [PATCH 08/26] Cleanup some target junk --- src/main/io/ledstrip.c | 79 --------------------------- src/main/target/COLIBRI_RACE/target.h | 1 - src/main/target/MOTOLAB/target.h | 1 + 3 files changed, 1 insertion(+), 80 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index b707a2a01..03c4c5d1e 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -238,85 +238,6 @@ static const specialColorIndexes_t defaultSpecialColors[] = { static int scaledThrottle; - - - -/* -#ifdef USE_LED_RING_DEFAULT_CONFIG -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 2, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 2, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, -}; -#elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, -}; -#else -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY(15, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY(15, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(15, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY(15, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 8, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - - { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 0, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 7, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 7, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 7, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 8, 9), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 9, 10), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY(10, 11), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY(10, 12), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 9, 13), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 8, 14), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 7, 14), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 6, 13), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 5, 12), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 5, 11), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 6, 10), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 7, 9), 3, LED_FUNCTION_THRUST_RING}, - -}; -#endif -*/ - - - - static void updateLedRingCounts(void); STATIC_UNIT_TESTED void determineLedStripDimensions(void) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index d4db45a6d..a68d7a71d 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -113,7 +113,6 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG #define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index fe465da9c..beb5431c7 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -21,6 +21,7 @@ #define USE_CLI #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +#define TARGET_CONFIG #define LED0 PB5 // Blue LEDs - PB5 //#define LED1 PB9 // Green LEDs - PB9 From da9673b9825751facf4b82074c6945428ef3277b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 15:23:14 +0200 Subject: [PATCH 09/26] Increase sensor Array size --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f23aa37df..68bd96e0e 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -239,7 +239,7 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) -static const char * const sensorHardwareNames[4][11] = { +static const char * const sensorHardwareNames[4][12] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, From 4ff280209511e242205da2c38ee10b8b2a649735 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 16:18:06 +0200 Subject: [PATCH 10/26] Fix CPU bug due toe reinitialising of filters --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b8b5cf11e..6ffaf93d2 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dtermSetpointWeight = 120; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; - pidProfile->toleranceBand = 15; + pidProfile->toleranceBand = 0; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f91875e2..09364a8b3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -116,11 +116,13 @@ void initFilters(const pidProfile_t *pidProfile) { if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + dtermNotchInitialised = true; } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + dtermBiquadLpfInitialised = true; } } } From 629f96969c48f3d48be5c6959317b66bc569f147 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 16:38:38 +0200 Subject: [PATCH 11/26] Added MPU9250 to acc lookup table. Duplication. Missing in Allowed Values printout. Duplication... --- src/main/io/serial_cli.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 68bd96e0e..3ebafa4ec 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -450,6 +450,7 @@ static const char * const lookupTableAccHardware[] = { "LSM303DLHC", "MPU6000", "MPU6500", + "MPU9250", "FAKE" }; From 685ae3aee376879e536d009de3b37c4def054cce Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 19:47:52 +0200 Subject: [PATCH 12/26] Limit number of targets to build on Travis. --- .travis.yml | 69 ++++++++++++++++++++++++++++------------------------- 1 file changed, 36 insertions(+), 33 deletions(-) diff --git a/.travis.yml b/.travis.yml index ca73f8c14..787ac12de 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,51 +2,54 @@ env: # - RUNTESTS=True # - PUBLISHMETA=True # - PUBLISHDOCS=True - - TARGET=AFROMINI - - TARGET=AIORACERF3 - - TARGET=AIR32 - - TARGET=ALIENFLIGHTF1 +# - TARGET=AFROMINI +# - TARGET=AIORACERF3 +# - TARGET=AIR32 +# - TARGET=ALIENFLIGHTF1 - TARGET=ALIENFLIGHTF3 - TARGET=ALIENFLIGHTF4 - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL - - TARGET=CHEBUZZF3 - - TARGET=CJMCU - - TARGET=COLIBRI - - TARGET=COLIBRI_RACE - - TARGET=DOGE - - TARGET=EUSTM32F103RC - - TARGET=F4BY - - TARGET=FURYF3 +# - TARGET=CHEBUZZF3 +# - TARGET=CJMCU +# - TARGET=COLIBRI +# - TARGET=COLIBRI_RACE +# - TARGET=DOGE +# - TARGET=EUSTM32F103RC +# - TARGET=F4BY +# - TARGET=FURYF3 - TARGET=FURYF4 - - TARGET=IRCFUSIONF3 - - TARGET=KISSFC - - TARGET=LUX_RACE - - TARGET=MICROSCISKY - - TARGET=MOTOLAB +# - TARGET=IRCFUSIONF3 +# - TARGET=KISSFC +# - TARGET=LUX_RACE +# - TARGET=MICROSCISKY +# - TARGET=MOTOLAB - TARGET=NAZE - - TARGET=OLIMEXINO - - TARGET=OMNIBUS - - TARGET=OMNIBUSF4 - - TARGET=PIKOBLX - - TARGET=PORT103R +# - TARGET=OLIMEXINO +# - TARGET=OMNIBUS +# - TARGET=OMNIBUSF4 +# - TARGET=PIKOBLX +# - TARGET=PORT103R - TARGET=REVO - - TARGET=REVONANO - - TARGET=REVO_OPBL - - TARGET=RMDO - - TARGET=SINGULARITY - - TARGET=SIRINFPV +# - TARGET=REVONANO +# - TARGET=REVO_OPBL +# - TARGET=RMDO +# - TARGET=SINGULARITY +# - TARGET=SIRINFPV - TARGET=SPARKY - - TARGET=SPARKY2 - - TARGET=SPARKY_OPBL +# - TARGET=SPARKY2 +# - TARGET=SPARKY_OPBL - TARGET=SPRACINGF3 - TARGET=SPRACINGF3EVO - - TARGET=SPRACINGF3MINI +# - TARGET=SPRACINGF3MINI - TARGET=STM32F3DISCOVERY - - TARGET=VRRACE - - TARGET=X_RACERSPI - - TARGET=ZCOREF3 +# - TARGET=VRRACE +# - TARGET=X_RACERSPI +# - TARGET=ZCOREF3 +CC3D NAZE +ALIENFLIGHTF3 SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY +ALIENFLIGHTF4 BLUEJAYF4 FURYF4 REVO # use new docker environment sudo: false From 69aecd9e926fdb372366970a3984fb8a3949eebe Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 19:50:22 +0200 Subject: [PATCH 13/26] Limit number of targets to build on Travis. --- .travis.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 787ac12de..16b89723f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -47,9 +47,6 @@ env: # - TARGET=VRRACE # - TARGET=X_RACERSPI # - TARGET=ZCOREF3 -CC3D NAZE -ALIENFLIGHTF3 SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY -ALIENFLIGHTF4 BLUEJAYF4 FURYF4 REVO # use new docker environment sudo: false From a396ba0c119c6008b985730c742b18c7f9d7c36b Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Tue, 9 Aug 2016 19:53:18 +0200 Subject: [PATCH 14/26] cleanup cli commands outputs and over - deleted printSectionBreak and his #define (not more used) - cleaned cli commads output (dump, sd_info, version) --- src/main/io/serial_cli.c | 25 +++++-------------------- 1 file changed, 5 insertions(+), 20 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3ebafa4ec..185047279 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -927,7 +927,6 @@ const clivalue_t valueTable[] = { #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) - typedef union { int32_t int_value; float float_value; @@ -940,8 +939,6 @@ static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); -#define printSectionBreak() cliPrintf((char *)sectionBreak) - #define COMPARE_CONFIG(value) (masterConfig.value == defaultConfig.value) static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); @@ -1731,8 +1728,6 @@ static void printServo(uint8_t dumpMask, master_t *defaultConfig) ); } - printSectionBreak(); - // print servo directions unsigned int channel; for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -2022,10 +2017,9 @@ static void cliSdInfo(char *cmdline) { ; // Nothing more detailed to print break; } - - cliPrint("\r\n"); break; } + cliPrint("\r\n"); } #endif @@ -2326,14 +2320,12 @@ static void printConfig(char *cmdline, bool doDiff) if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { cliPrint("\r\n# version\r\n"); cliVersion(NULL); - cliPrint("\r\n"); if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { cliPrint("\r\n# reset configuration to default settings\r\ndefaults\r\n"); } - printSectionBreak(); - cliPrint("# name\r\n"); + cliPrint("\r\n# name\r\n"); printName(dumpMask); cliPrint("\r\n# mixer\r\n"); @@ -2460,7 +2452,7 @@ static void printConfig(char *cmdline, bool doDiff) cliPrint("\r\n# adjrange\r\n"); printAdjustmentRange(dumpMask, &defaultConfig); - cliPrintf("\r\n# rxrange\r\n"); + cliPrint("\r\n# rxrange\r\n"); printRxRange(dumpMask, &defaultConfig); #ifdef USE_SERVOS @@ -2519,26 +2511,19 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *def { if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; - changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); - - printSectionBreak(); dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); - - cliRateProfile(""); } static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values - return; + return; changeControlRateProfile(rateProfileIndex); cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); - printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } @@ -3338,7 +3323,7 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintf("# BetaFlight/%s %s %s / %s (%s)", + cliPrintf("# BetaFlight/%s %s %s / %s (%s)\r\n", targetName, FC_VERSION_STRING, buildDate, From 220145d788aacf612a656cb0a77445f852459503 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 22:57:54 +0200 Subject: [PATCH 15/26] Set back the max limit for rc rates --- src/main/io/serial_cli.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 68bd96e0e..b45ece523 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -789,8 +789,8 @@ const clivalue_t valueTable[] = { { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, #endif - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 300 } }, - { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 300 } }, + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, + { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, From 98eb820cdbd06b629335aa5f18f02531a7745938 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 23:34:12 +0200 Subject: [PATCH 16/26] defaults --- src/main/config/config.c | 13 ++++++------- src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 2 +- 4 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6ffaf93d2..fd7419620 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -206,7 +206,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 60; - pidProfile->D8[PITCH] = 25; + pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -232,15 +232,15 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 200; - pidProfile->yawItermIgnoreRate = 50; + pidProfile->rollPitchItermIgnoreRate = 130; + pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_cutoff = 150; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; - pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; + pidProfile->pidAtMinThrottle = PID_STABILISATION_OFF; // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; @@ -310,10 +310,9 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) { #ifdef BRUSHED_MOTORS escAndServoConfig->minthrottle = 1000; - escAndServoConfig->maxthrottle = 2000; #else - escAndServoConfig->minthrottle = 1150; - escAndServoConfig->maxthrottle = 1850; + escAndServoConfig->maxthrottle = 2000; + escAndServoConfig->minthrottle = 1070; #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 09364a8b3..2e136af63 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -274,7 +274,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); // Handle All windup Scenarios // limit maximum integrator value to prevent WindUp diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c12404ec5..12fa8324e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -91,7 +91,7 @@ typedef struct pidProfile_s { uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage - uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active. + uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. // Betaflight PID controller parameters uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5947e954c..7eedf297a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -842,7 +842,7 @@ const clivalue_t valueTable[] = { { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, - { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, + { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, From ea67304a1a7f683a3cba644ed68b5a20ea68e23f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 23:38:07 +0200 Subject: [PATCH 17/26] Fix mw.c --- src/main/mw.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/mw.c b/src/main/mw.c index b2c569e98..365210874 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -536,7 +536,7 @@ void processRx(void) This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetErrorGyroState(); - if (currentProfile->pidProfile.zeroThrottleStabilisation) + if (currentProfile->pidProfile.pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); From 1a23a4f023ac97d7910428e10b3cd19b024ce4cc Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 10 Aug 2016 11:10:44 +0200 Subject: [PATCH 18/26] Sparky 2.0 UART fix --- src/main/target/SPARKY2/target.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 0ed29e661..febebfa1f 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -77,18 +77,18 @@ #define USE_VCP #define VBUS_SENSING_PIN PA8 -#define USE_USART1 -#define USART1_RX_PIN PA10 -#define USART1_TX_PIN PA9 -#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 -#define USE_USART3 -#define USART3_RX_PIN PB11 -#define USART3_TX_PIN PB10 +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 -#define USE_USART6 -#define USART6_RX_PIN PC7 -#define USART6_TX_PIN PC6 //inverter +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 //inverter #define SERIAL_PORT_COUNT 4 From 66f1abe1be4ae571a9e071bccbe974d2e6e68709 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 10 Aug 2016 20:54:26 +1000 Subject: [PATCH 19/26] Added missing delay for F4 to CLI --- src/main/io/serial_cli.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7eedf297a..215fc67b1 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2975,7 +2975,11 @@ static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *form tfp_format(cliWriter, cliPutp, format, va); va_end(va); - return true; +#ifdef USE_SLOW_SERIAL_CLI + delay(1); +#endif + + return true; } return false; From 22a55ce313c2b19daaba54c5a8f89f0541cfdea7 Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Wed, 10 Aug 2016 15:38:25 +0200 Subject: [PATCH 20/26] Fix issue with newline for MAC users --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 215fc67b1..3a14434be 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3209,7 +3209,7 @@ static void cliGet(char *cmdline) val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, 0); - cliPrint("\n"); + cliPrint("\r\n"); cliPrintVarRange(val); cliPrint("\r\n"); From 511117d41c61f120560bf83de950cac0207c1fbd Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 11 Aug 2016 08:10:30 +1200 Subject: [PATCH 21/26] Fixed 'diff commented' CLI command. --- src/main/io/serial_cli.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3a14434be..e1abbbeca 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2305,16 +2305,18 @@ static void printConfig(char *cmdline, bool doDiff) dumpMask = DUMP_RATES; // only } else if ((options = checkCommand(cmdline, "all"))) { dumpMask = DUMP_ALL; // all profiles and rates + } else { + options = cmdline; } master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; - createDefaultConfig(&defaultConfig); - } + createDefaultConfig(&defaultConfig); - if (checkCommand(options, "commented")) { - dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff + if (checkCommand(cmdline, "commented")) { + dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff + } } if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { From 5f23efe70c237a0a07c2513b29080f3f4ac84c18 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Thu, 11 Aug 2016 06:34:04 +0100 Subject: [PATCH 22/26] Update Log Header for Betaflight PID Parameters Add new betaflight PID controller parameters to log header and debug_mode (so we know what is being logged in the debug fields) --- src/main/blackbox/blackbox.c | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 406ce05d3..09946a4ab 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1210,18 +1210,34 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); - BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); - BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation); - BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); - BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_filter_type); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_cutoff * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("deltaMethod:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.deltaMethod); + BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); + BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation); + BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidAtMinThrottle); + + // Betaflight PID controller parameters + BLACKBOX_PRINT_HEADER_LINE("toleranceBand:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBand); + BLACKBOX_PRINT_HEADER_LINE("toleranceBandReduction:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBandReduction); + BLACKBOX_PRINT_HEADER_LINE("zeroCrossAllowanceCount:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.zeroCrossAllowanceCount); + BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain); + BLACKBOX_PRINT_HEADER_LINE("ptermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawRateAccelLimit); + BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit); + // End of Betaflight controller parameters + BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); + BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyro_soft_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d", (int)(masterConfig.gyro_soft_notch_cutoff * 100.0f)); @@ -1237,6 +1253,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.use_unsyncedPwm); BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", masterConfig.motor_pwm_protocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", masterConfig.motor_pwm_rate); + BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", masterConfig.debug_mode); BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); default: From 0d057ddf0dece041ea7637d0643976591ee6fcc7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 11 Aug 2016 19:19:24 +0200 Subject: [PATCH 23/26] Fix crash bug on diff // dump command --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e1abbbeca..4164f4e14 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2309,7 +2309,7 @@ static void printConfig(char *cmdline, bool doDiff) options = cmdline; } - master_t defaultConfig; + static master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; createDefaultConfig(&defaultConfig); From 08776bc30984a92a8aeeff090883e1162e06e033 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 11 Aug 2016 20:07:55 +0200 Subject: [PATCH 24/26] Fix incorrect coefficient when pid_denom > 1 is used --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2e136af63..829b62c0f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -115,13 +115,13 @@ void initFilters(const pidProfile_t *pidProfile) { if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); - for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); dtermNotchInitialised = true; } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { - for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); dtermBiquadLpfInitialised = true; } } From 2c9776861d374d77ed1ff90d142be6c6eab4d34d Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 12 Aug 2016 05:17:54 +0200 Subject: [PATCH 25/26] AlienFlight config fix --- src/main/target/ALIENFLIGHTF1/config.c | 12 ++++++------ src/main/target/ALIENFLIGHTF3/config.c | 12 ++++++------ src/main/target/ALIENFLIGHTF4/config.c | 12 ++++++------ 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index b16a16444..b34f9d5a3 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -77,12 +77,12 @@ void targetConfiguration(void) masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentProfile->pidProfile.P8[ROLL] = 90; - currentProfile->pidProfile.I8[ROLL] = 44; - currentProfile->pidProfile.D8[ROLL] = 60; - currentProfile->pidProfile.P8[PITCH] = 90; - currentProfile->pidProfile.I8[PITCH] = 44; - currentProfile->pidProfile.D8[PITCH] = 60; + masterConfig.profile[0].pidProfile.P8[ROLL] = 90; + masterConfig.profile[0].pidProfile.I8[ROLL] = 44; + masterConfig.profile[0].pidProfile.D8[ROLL] = 60; + masterConfig.profile[0].pidProfile.P8[PITCH] = 90; + masterConfig.profile[0].pidProfile.I8[PITCH] = 44; + masterConfig.profile[0].pidProfile.D8[PITCH] = 60; masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 3aed5a3ce..650da37c1 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -85,12 +85,12 @@ void targetConfiguration(void) { masterConfig.failsafeConfig.failsafe_off_delay = 0; masterConfig.gyro_sync_denom = 2; masterConfig.pid_process_denom = 1; - currentProfile->pidProfile.P8[ROLL] = 90; - currentProfile->pidProfile.I8[ROLL] = 44; - currentProfile->pidProfile.D8[ROLL] = 60; - currentProfile->pidProfile.P8[PITCH] = 90; - currentProfile->pidProfile.I8[PITCH] = 44; - currentProfile->pidProfile.D8[PITCH] = 60; + masterConfig.profile[0].pidProfile.P8[ROLL] = 90; + masterConfig.profile[0].pidProfile.I8[ROLL] = 44; + masterConfig.profile[0].pidProfile.D8[ROLL] = 60; + masterConfig.profile[0].pidProfile.P8[PITCH] = 90; + masterConfig.profile[0].pidProfile.I8[PITCH] = 44; + masterConfig.profile[0].pidProfile.D8[PITCH] = 60; masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 5264a1673..b15623039 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -85,12 +85,12 @@ void targetConfiguration(void) { masterConfig.failsafeConfig.failsafe_off_delay = 0; masterConfig.gyro_sync_denom = 1; masterConfig.pid_process_denom = 1; - currentProfile->pidProfile.P8[ROLL] = 90; - currentProfile->pidProfile.I8[ROLL] = 44; - currentProfile->pidProfile.D8[ROLL] = 60; - currentProfile->pidProfile.P8[PITCH] = 90; - currentProfile->pidProfile.I8[PITCH] = 44; - currentProfile->pidProfile.D8[PITCH] = 60; + masterConfig.profile[0].pidProfile.P8[ROLL] = 90; + masterConfig.profile[0].pidProfile.I8[ROLL] = 44; + masterConfig.profile[0].pidProfile.D8[ROLL] = 60; + masterConfig.profile[0].pidProfile.P8[PITCH] = 90; + masterConfig.profile[0].pidProfile.I8[PITCH] = 44; + masterConfig.profile[0].pidProfile.D8[PITCH] = 60; masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R From d10e6a8f87a5536168fd440b244759f2b9b628b1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 12 Aug 2016 05:35:00 +0200 Subject: [PATCH 26/26] Fix maxthrottle value for brushed targets --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index fd7419620..07605b3f3 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -311,9 +311,9 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) #ifdef BRUSHED_MOTORS escAndServoConfig->minthrottle = 1000; #else - escAndServoConfig->maxthrottle = 2000; escAndServoConfig->minthrottle = 1070; #endif + escAndServoConfig->maxthrottle = 2000; escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; }