From f208a7772398f0cc76a0f4286f76e2f5c1900dfa Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 27 Aug 2016 23:20:44 +1000 Subject: [PATCH 01/41] Repurpose some outputs on the SPRACINGF3EVO for Servo outputs. --- src/main/target/SPRACINGF3EVO/target.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..d128f7e8b3 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,10 +30,10 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), 0xFFFF }; @@ -44,10 +44,10 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), 0xFFFF }; From a8bfa54ae07cdfc37e3d3b81ac351629dc5c94a2 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 28 Aug 2016 01:07:01 +1200 Subject: [PATCH 02/41] Changed CLI parser to allow multiple whitespaces. Fixes problem with 'mmix' dump. --- src/main/io/serial_cli.c | 108 +++++++++++++++++++++------------------ 1 file changed, 59 insertions(+), 49 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3ce15c0f75..b6c0aa0841 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -968,14 +968,24 @@ static void cliShowArgumentRangeError(char *name, int min, int max) cliPrintf("%s must be between %d and %d\r\n", name, min, max); } +static char *nextArg(char *currentArg) +{ + char *ptr = strchr(currentArg, ' '); + while (ptr && *ptr == ' ') { + ptr++; + } + + return ptr; +} + static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount) { int val; for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); val = CHANNEL_VALUE_TO_STEP(val); if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { if (argIndex == 0) { @@ -1058,9 +1068,9 @@ static void cliRxFail(char *cmdline) rxFailsafeChannelMode_e mode = channelFailsafeConfiguration->mode; bool requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - char *p = strchr(rxFailsafeModeCharacters, *(++ptr)); + char *p = strchr(rxFailsafeModeCharacters, *(ptr)); if (p) { uint8_t requestedMode = p - rxFailsafeModeCharacters; mode = rxFailsafeModesTable[type][requestedMode]; @@ -1074,13 +1084,13 @@ static void cliRxFail(char *cmdline) requireValue = mode == RX_FAILSAFE_MODE_SET; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { if (!requireValue) { cliShowParseError(); return; } - value = atoi(++ptr); + value = atoi(ptr); value = CHANNEL_VALUE_TO_RXFAIL_STEP(value); if (value > MAX_RXFAIL_RANGE_STEP) { cliPrint("Value out of range\r\n"); @@ -1164,17 +1174,17 @@ static void cliAux(char *cmdline) if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; uint8_t validArgumentCount = 0; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < CHECKBOX_ITEM_COUNT) { mac->modeId = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { mac->auxChannelIndex = val; validArgumentCount++; @@ -1254,20 +1264,20 @@ static void cliSerial(char *cmdline) validArgumentCount++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); portConfig.functionMask = val & 0xFFFF; validArgumentCount++; } for (i = 0; i < 4; i ++) { - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (!ptr) { break; } - val = atoi(++ptr); + val = atoi(ptr); uint8_t baudRateIndex = lookupBaudRateIndex(val); if (baudRates[baudRateIndex] != (uint32_t) val) { @@ -1434,17 +1444,17 @@ static void cliAdjustmentRange(char *cmdline) adjustmentRange_t *ar = &masterConfig.adjustmentRanges[i]; uint8_t validArgumentCount = 0; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { ar->adjustmentIndex = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { ar->auxChannelIndex = val; validArgumentCount++; @@ -1453,17 +1463,17 @@ static void cliAdjustmentRange(char *cmdline) ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount); - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < ADJUSTMENT_FUNCTION_COUNT) { ar->adjustmentFunction = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { ar->auxSwitchChannelIndex = val; validArgumentCount++; @@ -1531,9 +1541,9 @@ static void cliMotorMix(char *cmdline) for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) masterConfig.customMotorMixer[i].throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = strchr(cmdline, ' '); + ptr = nextArg(cmdline); if (ptr) { - len = strlen(++ptr); + len = strlen(ptr); for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { cliPrint("Invalid name\r\n"); @@ -1551,30 +1561,30 @@ static void cliMotorMix(char *cmdline) ptr = cmdline; uint32_t i = atoi(ptr); // get motor number if (i < MAX_SUPPORTED_MOTORS) { - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].throttle = fastA2F(++ptr); + masterConfig.customMotorMixer[i].throttle = fastA2F(ptr); check++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].roll = fastA2F(++ptr); + masterConfig.customMotorMixer[i].roll = fastA2F(ptr); check++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].pitch = fastA2F(++ptr); + masterConfig.customMotorMixer[i].pitch = fastA2F(ptr); check++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].yaw = fastA2F(++ptr); + masterConfig.customMotorMixer[i].yaw = fastA2F(ptr); check++; } if (check != 4) { cliShowParseError(); } else { - cliMotorMix(""); + printMotorMix(DUMP_MASTER, NULL); } } else { cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); @@ -1622,15 +1632,15 @@ static void cliRxRange(char *cmdline) if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) { int rangeMin, rangeMax; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - rangeMin = atoi(++ptr); + rangeMin = atoi(ptr); validArgumentCount++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - rangeMax = atoi(++ptr); + rangeMax = atoi(ptr); validArgumentCount++; } @@ -1680,8 +1690,8 @@ static void cliLed(char *cmdline) ptr = cmdline; i = atoi(ptr); if (i < LED_MAX_STRIP_LENGTH) { - ptr = strchr(cmdline, ' '); - if (!parseLedStripConfig(i, ++ptr)) { + ptr = nextArg(cmdline); + if (!parseLedStripConfig(i, ptr)) { cliShowParseError(); } } else { @@ -1728,8 +1738,8 @@ static void cliColor(char *cmdline) ptr = cmdline; i = atoi(ptr); if (i < LED_CONFIGURABLE_COLOR_COUNT) { - ptr = strchr(cmdline, ' '); - if (!parseColor(i, ++ptr)) { + ptr = nextArg(cmdline); + if (!parseColor(i, ptr)) { cliShowParseError(); } } else { @@ -1982,9 +1992,9 @@ static void cliServoMix(char *cmdline) masterConfig.servoConf[i].reversedSources = 0; } } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = strchr(cmdline, ' '); + ptr = nextArg(cmdline); if (ptr) { - len = strlen(++ptr); + len = strlen(ptr); for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { cliPrintf("Invalid name\r\n"); @@ -2280,25 +2290,25 @@ static void cliVtx(char *cmdline) if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) { vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; uint8_t validArgumentCount = 0; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { cac->auxChannelIndex = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) { cac->band = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) { cac->channel = val; validArgumentCount++; From 75e07c1bb7a2aa2ce90edbd66da379dc5b006e18 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 27 Aug 2016 20:22:09 +0200 Subject: [PATCH 03/41] Cleanup super expo feature --- 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 b6c0aa0841..9509d0caa2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -220,7 +220,7 @@ static const char * const featureNames[] = { "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", NULL + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", NULL }; // sync this with rxFailsafeChannelMode_e From 85222c80fab448ecdba07401708f69c181424867 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Sun, 28 Aug 2016 23:53:10 +0200 Subject: [PATCH 04/41] New files for target RACEBASE FC from shop.rotoracer.com --- src/main/target/RACEBASE/config.c | 86 ++++++++++++++++++++ src/main/target/RACEBASE/readme.txt | 0 src/main/target/RACEBASE/target.c | 59 ++++++++++++++ src/main/target/RACEBASE/target.h | 120 ++++++++++++++++++++++++++++ src/main/target/RACEBASE/target.mk | 12 +++ 5 files changed, 277 insertions(+) create mode 100755 src/main/target/RACEBASE/config.c create mode 100755 src/main/target/RACEBASE/readme.txt create mode 100755 src/main/target/RACEBASE/target.c create mode 100755 src/main/target/RACEBASE/target.h create mode 100755 src/main/target/RACEBASE/target.mk diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c new file mode 100755 index 0000000000..95e678acac --- /dev/null +++ b/src/main/target/RACEBASE/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 COLIBRI RACE targets +void targetConfiguration(master_t *config) { + config->rxConfig.sbus_inversion = 0; + config->rxConfig.rssi_scale = 19; + config->escAndServoConfig.minthrottle = 1060; + config->escAndServoConfig.maxthrottle = 1940; + config->serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; + config->rxConfig.serialrx_provider = SERIALRX_SBUS; +} diff --git a/src/main/target/RACEBASE/readme.txt b/src/main/target/RACEBASE/readme.txt new file mode 100755 index 0000000000..e69de29bb2 diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c new file mode 100755 index 0000000000..0b6a8f1695 --- /dev/null +++ b/src/main/target/RACEBASE/target.c @@ -0,0 +1,59 @@ +/* + * 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 "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + 0xFFFF +}; + +const uint16_t airPWM[] = { + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, + + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - PC6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - PC7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PMW4 - PC8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PC9 +}; + + diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h new file mode 100755 index 0000000000..d36dd41cfb --- /dev/null +++ b/src/main/target/RACEBASE/target.h @@ -0,0 +1,120 @@ +/* + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "RBFC" +#define TARGET_CONFIG + +#define LED0 PB3 +#define LED0_INVERTED + +#define LED1 PB4 +#define LED1_INVERTED + +#define BEEPER PA12 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + + +#define MPU6000_CS_PIN PB5 +#define MPU6000_SPI_INSTANCE SPI2 + +#define GYRO +#define USE_GYRO_SPI_MPU6000 + +#define ACC +#define USE_ACC_SPI_MPU6000 + +#define ACC_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN CW90_DEG + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define SERIAL_PORT_COUNT 3 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN PA7 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PA6 + +#define LED_STRIP +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define OSD + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT + +#define USE_SERVOS + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_RSSI_ADC) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(5)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 5 +#define USED_TIMERS (TIM_N(2) | TIM_N(4)) +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM4) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + + diff --git a/src/main/target/RACEBASE/target.mk b/src/main/target/RACEBASE/target.mk new file mode 100755 index 0000000000..6e4711c5da --- /dev/null +++ b/src/main/target/RACEBASE/target.mk @@ -0,0 +1,12 @@ +F3_TARGETS += $(TARGET) + +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/max7456.c \ + io/osd.c From 7ff10640f5d2042f6d79d5578c35d92f4d60accb Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Mon, 29 Aug 2016 15:48:31 +0200 Subject: [PATCH 05/41] Wrong check in MAX7456 status register --- src/main/drivers/max7456.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) mode change 100644 => 100755 src/main/drivers/max7456.c diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c old mode 100644 new mode 100755 index 82f3f29af9..9fcf45e033 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -296,7 +296,7 @@ void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { max7456_send(MAX7456ADD_CMM, WRITE_NVR); // wait until bit 5 in the status register returns to 0 (12ms) - while ((spiTransferByte(MAX7456_SPI_INSTANCE, MAX7456ADD_STAT) & STATUS_REG_NVR_BUSY) != 0); + while ((max7456_send(MAX7456ADD_STAT, 0) & STATUS_REG_NVR_BUSY) != 0x00); max7456_send(VM0_REG, video_signal_type | 0x0C); DISABLE_MAX7456; From f49a396a7458288e0b7b933bfd7980c551c569d3 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Mon, 29 Aug 2016 15:54:43 +0200 Subject: [PATCH 06/41] Upps.. I did not save readme file :/ --- src/main/target/RACEBASE/readme.txt | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/main/target/RACEBASE/readme.txt b/src/main/target/RACEBASE/readme.txt index e69de29bb2..a2507bcd3e 100755 --- a/src/main/target/RACEBASE/readme.txt +++ b/src/main/target/RACEBASE/readme.txt @@ -0,0 +1,24 @@ +==RACABASE FC== + +Owner: Marcin Baliniak (marcin baliniak.pl) + +Available at: shop.rotoracer.com + +Board information: + +- CPU - STM32F303CCT6 +- MPU-6000 - connected to SPI2 +- SPI Flash - connected to SPI2 +- MAX7456 - connected to SPI2 (NO DMA) +- Build in 5V BEC + LC filter - board can be powered from main battery +- Input voltage: 5V - 17.4V +- RC input: PPM, S.BUS (build in inverter) - Uart2, Spectrum-Uart3 +- Other connectors: RSSI, Current sensor, LED strip, Buzzer, Video IN/Out + LC filter +- Weight: 6 g +- Size: 36 mm x 36 mm x 5 mm + +Photo: +https://cdn.shoplo.com/0639/products/th1024/aaaw/838-rr_flight_controller_rotoracer_fc_rotoracer_racebase.jpg + +Port description: +https://cdn.shoplo.com/0639/products/th1024/aaaw/844-racebase-render-en.png \ No newline at end of file From 7f325373d1004899166ea14b60aa656ddc8c2f31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Mon, 29 Aug 2016 16:48:33 +0200 Subject: [PATCH 07/41] Change baro detect order Probe BMP085/BMP180 before MS5611, because BMP085 can be misdetected as a MS5611. --- src/main/sensors/initialisation.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 2529a8c330..edddb18421 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -460,7 +460,14 @@ static void detectBaro(baroSensor_e baroHardwareToUse) switch (baroHardware) { case BARO_DEFAULT: ; // fallthough - + case BARO_BMP085: +#ifdef USE_BARO_BMP085 + if (bmp085Detect(bmp085Config, &baro)) { + baroHardware = BARO_BMP085; + break; + } +#endif + ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 if (ms5611Detect(&baro)) { @@ -469,14 +476,6 @@ static void detectBaro(baroSensor_e baroHardwareToUse) } #endif ; // fallthough - case BARO_BMP085: -#ifdef USE_BARO_BMP085 - if (bmp085Detect(bmp085Config, &baro)) { - baroHardware = BARO_BMP085; - break; - } -#endif - ; // fallthough case BARO_BMP280: #ifdef USE_BARO_BMP280 if (bmp280Detect(&baro)) { @@ -484,6 +483,7 @@ static void detectBaro(baroSensor_e baroHardwareToUse) break; } #endif + ; // fallthough case BARO_NONE: baroHardware = BARO_NONE; break; From 4423d20acc02b00dcc11226e53ab18a01bce5c8c Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 31 Aug 2016 13:33:26 +1200 Subject: [PATCH 08/41] Made AIRMODE and 3D override 'auto_disarm_delay', to make it consistent with handling of MOTOR_STOP. --- src/main/mw.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/mw.c b/src/main/mw.c index 20580b66a9..08bf53420c 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -572,6 +572,8 @@ void processRx(void) if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) + && !feature(FEATURE_3D) + && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { From 5d463ffabf7a1a2de129de14e4f81f89ad2a7cd7 Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 31 Aug 2016 13:50:56 +0200 Subject: [PATCH 09/41] Motor Outputs back again redo latest commit #1092 ... motor 1 and 2 not working correct (begin rotate only at mid throttle and above) --- src/main/target/SPRACINGF3EVO/target.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index d128f7e8b3..843ad3d9aa 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,10 +30,10 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; @@ -44,10 +44,10 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; From 4634fb0787e893c09f1ed00741b84700d2ef575d Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 31 Aug 2016 14:52:50 +0200 Subject: [PATCH 10/41] map only pwm8 and pwm9 to servo outputs see this in cleanflight --- src/main/target/SPRACINGF3EVO/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..488e9a7a9c 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,8 +30,8 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -44,8 +44,8 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF From 7963e0518f486030b6b6250d70bf6fcd76607743 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Thu, 1 Sep 2016 12:16:55 +0200 Subject: [PATCH 11/41] Remove unnecessary redefinition, and force SPI flash to use standard clock on this target --- src/main/target/RACEBASE/config.c | 2 -- src/main/target/RACEBASE/target.h | 1 + 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c index 95e678acac..b9e0e15702 100755 --- a/src/main/target/RACEBASE/config.c +++ b/src/main/target/RACEBASE/config.c @@ -79,8 +79,6 @@ void targetConfiguration(master_t *config) { config->rxConfig.sbus_inversion = 0; config->rxConfig.rssi_scale = 19; - config->escAndServoConfig.minthrottle = 1060; - config->escAndServoConfig.maxthrottle = 1940; config->serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; config->rxConfig.serialrx_provider = SERIALRX_SBUS; } diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index d36dd41cfb..484237c5e3 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -76,6 +76,7 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 +#define M25P16_SPI_SHARED #define USE_FLASHFS #define USE_FLASH_M25P16 From f6564c1bc259faefdbe44078ecba22a4fa5316c9 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Thu, 1 Sep 2016 13:42:06 +0200 Subject: [PATCH 12/41] Definition for serial RX changed - moved to target.h --- src/main/target/RACEBASE/config.c | 1 - src/main/target/RACEBASE/target.h | 3 +++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c index b9e0e15702..0ad304995c 100755 --- a/src/main/target/RACEBASE/config.c +++ b/src/main/target/RACEBASE/config.c @@ -79,6 +79,5 @@ void targetConfiguration(master_t *config) { config->rxConfig.sbus_inversion = 0; config->rxConfig.rssi_scale = 19; - config->serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; config->rxConfig.serialrx_provider = SERIALRX_SBUS; } diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 484237c5e3..8e892151d4 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -62,6 +62,9 @@ #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 +#define SERIALRX_UART SERIAL_PORT_USART2 + + #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 From b3ac8972396f40ebf49c11a608573061f64a9a46 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Thu, 1 Sep 2016 13:50:27 +0200 Subject: [PATCH 13/41] Markdown format for REDME.md --- src/main/target/RACEBASE/README.md | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100755 src/main/target/RACEBASE/README.md diff --git a/src/main/target/RACEBASE/README.md b/src/main/target/RACEBASE/README.md new file mode 100755 index 0000000000..1a06de5f27 --- /dev/null +++ b/src/main/target/RACEBASE/README.md @@ -0,0 +1,25 @@ +##RACABASE FC + +Owner: Marcin Baliniak (marcin baliniak.pl) + +Available at: shop.rotoracer.com + +###Board information: + +- CPU - STM32F303CCT6 +- MPU-6000 - connected to SPI2 +- SPI Flash - connected to SPI2 +- MAX7456 - connected to SPI2 (NO DMA) +- Build in 5V BEC + LC filter - board can be powered from main battery +- Input voltage: 5V - 17.4V +- RC input: PPM, S.BUS (build in inverter) - Uart2, Spectrum-Uart3 +- Other connectors: RSSI, Current sensor, LED strip, Buzzer, Video IN/Out + LC filter +- Weight: 6 g +- Size: 36 mm x 36 mm x 5 mm + +###Photo +![Board photo](https://cdn.shoplo.com/0639/products/th1024/aaaw/838-rr_flight_controller_rotoracer_fc_rotoracer_racebase.jpg) + + +###Port description +![Port description](https://cdn.shoplo.com/0639/products/th1024/aaaw/844-racebase-render-en.png) From 3f6860b83bbd78c2c7e0989fead48052e6137834 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 2 Sep 2016 22:04:53 +0200 Subject: [PATCH 14/41] Fix error in rc expo calculation // add configurable expo power --- src/main/common/maths.c | 7 +++++++ src/main/common/maths.h | 1 + src/main/config/config.c | 1 + src/main/io/rc_controls.h | 1 + src/main/io/serial_cli.c | 1 + src/main/io/serial_msp.c | 6 +++++- src/main/mw.c | 6 ++++-- 7 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 52990c6ac8..ee1b65ce67 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -99,6 +99,13 @@ float acos_approx(float x) } #endif +float powerf(float base, int exp) { + float result = base; + for (int count = 1; count < exp; count++) result *= base; + + return result; +} + int32_t applyDeadband(int32_t value, int32_t deadband) { if (ABS(value) < deadband) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 3251c237a8..3b24dd734d 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -69,6 +69,7 @@ typedef union { fp_angles_def angles; } fp_angles_t; +float powerf(float base, int exp); int32_t applyDeadband(int32_t value, int32_t deadband); void devClear(stdev_t *dev); diff --git a/src/main/config/config.c b/src/main/config/config.c index f5aabf3629..69b221b000 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -191,6 +191,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) controlRateConfig->dynThrPID = 10; controlRateConfig->rcYawExpo8 = 0; controlRateConfig->tpa_breakpoint = 1650; + controlRateConfig->rcExpoPwr = 3; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { controlRateConfig->rates[axis] = 70; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 38c093f537..55e3cecbaf 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -152,6 +152,7 @@ typedef struct controlRateConfig_s { uint8_t dynThrPID; uint8_t rcYawExpo8; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated + uint8_t rcExpoPwr; } controlRateConfig_t; extern int16_t rcCommand[4]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9509d0caa2..8a163f6ce3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -800,6 +800,7 @@ const clivalue_t valueTable[] = { { "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 } }, + { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 2, 4 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 52de0e4ed1..1e1c9bed4c 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -820,7 +820,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)gyro.targetLooptime); break; case MSP_RC_TUNING: - headSerialReply(12); + headSerialReply(13); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); for (i = 0 ; i < 3; i++) { @@ -832,6 +832,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentControlRateProfile->tpa_breakpoint); serialize8(currentControlRateProfile->rcYawExpo8); serialize8(currentControlRateProfile->rcYawRate8); + serialize8(currentControlRateProfile->rcExpoPwr); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -1424,6 +1425,9 @@ static bool processInCommand(void) if (currentPort->dataSize >= 12) { currentControlRateProfile->rcYawRate8 = read8(); } + if (currentPort->dataSize >=13) { + currentControlRateProfile->rcExpoPwr = read8(); + } } else { headSerialError(0); } diff --git a/src/main/mw.c b/src/main/mw.c index 08bf53420c..01322d98ca 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -189,13 +189,15 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); rcCommandf = rc / 500.0f; - rcInput[axis] = ABS(rcCommandf); if (rcExpo) { float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof)); + float absRc = ABS(rcCommandf); + rcCommandf = rcCommandf * (expof * (powerf(absRc, currentControlRateProfile->rcExpoPwr)) + absRc*(1-expof)); } + rcInput[axis] = ABS(rcCommandf); + angleRate = 200.0f * rcRate * rcCommandf; if (currentControlRateProfile->rates[axis]) { From 9e5f66909031d57e2f9d007a80fac568550c4dcc Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 2 Sep 2016 22:15:43 +0200 Subject: [PATCH 15/41] Bump EEPROM --- 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 69b221b000..8917f2c4df 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 144; +static const uint8_t EEPROM_CONF_VERSION = 145; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { From b6b0e57b52894fa363104e4027d09a3032f6c4b2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 3 Sep 2016 02:20:20 +0200 Subject: [PATCH 16/41] Change rcInput to store actual stick input --- src/main/mw.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 01322d98ca..05f20cb918 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -189,19 +189,17 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); rcCommandf = rc / 500.0f; + rcInput[axis] = ABS(rcCommandf); if (rcExpo) { float expof = rcExpo / 100.0f; - float absRc = ABS(rcCommandf); - rcCommandf = rcCommandf * (expof * (powerf(absRc, currentControlRateProfile->rcExpoPwr)) + absRc*(1-expof)); + rcCommandf = rcCommandf * (expof * (powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr)) + rcInput[axis]*(1-expof)); } - rcInput[axis] = ABS(rcCommandf); - angleRate = 200.0f * rcRate * rcCommandf; if (currentControlRateProfile->rates[axis]) { - rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate *= rcSuperfactor; } @@ -210,7 +208,7 @@ float calculateSetpointRate(int axis, int16_t rc) { } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } From 13b58cd64947cfccdb0f07d08df376d8573327dd Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 3 Sep 2016 14:33:15 +1200 Subject: [PATCH 17/41] Fixed RC Expo Power in CLI. --- 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 8a163f6ce3..5095ef0fe7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -800,7 +800,7 @@ const clivalue_t valueTable[] = { { "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 } }, - { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 2, 4 } }, + { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpoPwr, .config.minmax = { 2, 4 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, From e338279be418a6d360621dfa4dd712e8ca61a950 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 3 Sep 2016 19:04:58 +1000 Subject: [PATCH 18/41] DFU mode via CLI for F1 and F3 --- src/main/drivers/system_stm32f10x.c | 13 +++++++++++++ src/main/drivers/system_stm32f30x.c | 13 +++++++++++++ src/main/drivers/system_stm32f4xx.c | 4 ++-- 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 3e33c545b8..d77f9fec55 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -114,4 +114,17 @@ void systemInit(void) void checkForBootLoaderRequest(void) { + void(*bootJump)(void); + + if (*((uint32_t *)0x20004FF0) == 0xDEADBEEF) { + + *((uint32_t *)0x20004FF0) = 0x0; + + __enable_irq(); + __set_MSP(*((uint32_t *)0x1FFFF000)); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1FFFF004)); + bootJump(); + while (1); + } } diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index fb30936968..507e0811c5 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -107,4 +107,17 @@ void systemInit(void) void checkForBootLoaderRequest(void) { + void(*bootJump)(void); + + if (*((uint32_t *)0x20009FFC) == 0xDEADBEEF) { + + *((uint32_t *)0x20009FFC) = 0x0; + + __enable_irq(); + __set_MSP(*((uint32_t *)0x1FFFD800)); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1FFFD804)); + bootJump(); + while (1); + } } diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 5c09ebdf6a..69c22c3ffc 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -196,9 +196,9 @@ void checkForBootLoaderRequest(void) *((uint32_t *)0x2001FFFC) = 0x0; __enable_irq(); - __set_MSP(0x20001000); + __set_MSP(*((uint32_t *)0x1FFF0000)); - bootJump = (void(*)(void))(*((uint32_t *) 0x1fff0004)); + bootJump = (void(*)(void))(*((uint32_t *) 0x1FFF0004)); bootJump(); while (1); } From c4ace2c8df3fcf9199bbe2f6156fe93e51385ed7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Mon, 5 Sep 2016 18:01:29 +0200 Subject: [PATCH 19/41] Feedback FPVANGLEMIX mode --- src/main/io/serial_msp.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 1e1c9bed4c..21f68bebac 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -599,7 +599,8 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE; + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); From 2987c7d8c8db4f8fb9d71840c110153662a7a08b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 00:46:52 +0200 Subject: [PATCH 20/41] Fix expo curve --- 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 05f20cb918..d846085543 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -193,7 +193,7 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcExpo) { float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * (expof * (powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr)) + rcInput[axis]*(1-expof)); + rcCommandf = rcCommandf * powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr) * expof + rcCommandf * (1-expof); } angleRate = 200.0f * rcRate * rcCommandf; From efd43059b56ab23ecb99bdc9e3640305908cb0f2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 01:28:26 +0200 Subject: [PATCH 21/41] Remove unnecessary complicated expo power --- src/main/config/config.c | 3 +-- src/main/io/rc_controls.h | 1 - src/main/io/serial_cli.c | 1 - src/main/io/serial_msp.c | 6 +----- src/main/mw.c | 3 ++- 5 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 8917f2c4df..f5aabf3629 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 145; +static const uint8_t EEPROM_CONF_VERSION = 144; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -191,7 +191,6 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) controlRateConfig->dynThrPID = 10; controlRateConfig->rcYawExpo8 = 0; controlRateConfig->tpa_breakpoint = 1650; - controlRateConfig->rcExpoPwr = 3; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { controlRateConfig->rates[axis] = 70; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 55e3cecbaf..38c093f537 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -152,7 +152,6 @@ typedef struct controlRateConfig_s { uint8_t dynThrPID; uint8_t rcYawExpo8; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated - uint8_t rcExpoPwr; } controlRateConfig_t; extern int16_t rcCommand[4]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5095ef0fe7..9509d0caa2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -800,7 +800,6 @@ const clivalue_t valueTable[] = { { "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 } }, - { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpoPwr, .config.minmax = { 2, 4 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 21f68bebac..33bd4a74c9 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -821,7 +821,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)gyro.targetLooptime); break; case MSP_RC_TUNING: - headSerialReply(13); + headSerialReply(12); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); for (i = 0 ; i < 3; i++) { @@ -833,7 +833,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentControlRateProfile->tpa_breakpoint); serialize8(currentControlRateProfile->rcYawExpo8); serialize8(currentControlRateProfile->rcYawRate8); - serialize8(currentControlRateProfile->rcExpoPwr); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -1426,9 +1425,6 @@ static bool processInCommand(void) if (currentPort->dataSize >= 12) { currentControlRateProfile->rcYawRate8 = read8(); } - if (currentPort->dataSize >=13) { - currentControlRateProfile->rcExpoPwr = read8(); - } } else { headSerialError(0); } diff --git a/src/main/mw.c b/src/main/mw.c index d846085543..faed2088b0 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -174,6 +174,7 @@ bool isCalibrating() } #define RC_RATE_INCREMENTAL 14.54f +#define RC_EXPO_POWER 3 float calculateSetpointRate(int axis, int16_t rc) { float angleRate, rcRate, rcSuperfactor, rcCommandf; @@ -193,7 +194,7 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcExpo) { float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr) * expof + rcCommandf * (1-expof); + rcCommandf = rcCommandf * powerf(rcInput[axis], RC_EXPO_POWER) * expof + rcCommandf * (1-expof); } angleRate = 200.0f * rcRate * rcCommandf; From d0ceb1167aecd85d44d60b6596367373af4cbd9f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 01:48:49 +0200 Subject: [PATCH 22/41] Increase Pterm setpoint weight range --- 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 9509d0caa2..2c10cea9b7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -853,7 +853,7 @@ const clivalue_t valueTable[] = { { "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 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, - { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, + { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 255 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, From 2baa1e1d39d5e52ebbc0f282bc706c5e78118d9a Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 6 Sep 2016 22:44:19 +1200 Subject: [PATCH 23/41] Fixed broken LEDSTRIP 'west' indicator. --- src/main/io/ledstrip.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index c41c3a884b..090f56c946 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -257,9 +257,9 @@ STATIC_UNIT_TESTED void determineLedStripDimensions(void) STATIC_UNIT_TESTED void determineOrientationLimits(void) { - highestYValueForNorth = MIN((ledGridHeight / 2) - 1, 0); + highestYValueForNorth = MAX((ledGridHeight / 2) - 1, 0); lowestYValueForSouth = (ledGridHeight + 1) / 2; - highestXValueForWest = MIN((ledGridWidth / 2) - 1, 0); + highestXValueForWest = MAX((ledGridWidth / 2) - 1, 0); lowestXValueForEast = (ledGridWidth + 1) / 2; } From 9dd135ec802de71b57469468b2c1318dc50264a5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 22:02:55 +0200 Subject: [PATCH 24/41] Add back ESC jump limit // revert p weight limit --- src/main/config/config.c | 5 +++-- src/main/flight/mixer.c | 13 +++++++++++++ src/main/io/escservo.h | 1 + src/main/io/serial_cli.c | 3 ++- 4 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index f5aabf3629..078a3f5c27 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 144; +static const uint8_t EEPROM_CONF_VERSION = 145; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSetpointWeight = 80; + pidProfile->ptermSetpointWeight = 85; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -320,6 +320,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) escAndServoConfig->maxthrottle = 2000; escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; + escAndServoConfig->maxEscThrottleJumpMs = 0; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6f6dbb3ae2..6111a9883c 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -862,6 +862,19 @@ void mixTable(void *pidProfilePtr) } } + // Anti Desync feature for ESC's. Limit rapid throttle changes + if (escAndServoConfig->maxEscThrottleJumpMs) { + const int16_t maxThrottleStep = constrain(escAndServoConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 5, 10000); + + // Only makes sense when it's within the range + if (maxThrottleStep < throttleRange) { + static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; + + motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); + motorPrevious[i] = motor[i]; + } + } + // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index 66cd7db59e..fe37d26445 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -24,4 +24,5 @@ typedef struct escAndServoConfig_s { uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. + uint16_t maxEscThrottleJumpMs; } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2c10cea9b7..0b64d0526f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -670,6 +670,7 @@ const clivalue_t valueTable[] = { { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxEscThrottleJumpMs, .config.minmax = { 0, 1000 } }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently, @@ -853,7 +854,7 @@ const clivalue_t valueTable[] = { { "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 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, - { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 255 } }, + { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, From b0e182a3233f5522e014c281c1275a304fef6319 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 22:06:39 +0200 Subject: [PATCH 25/41] Change Defaults --- 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 078a3f5c27..b818805a75 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -211,7 +211,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 65; pidProfile->D8[PITCH] = 22; - pidProfile->P8[YAW] = 80; + pidProfile->P8[YAW] = 70; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; pidProfile->P8[PIDALT] = 50; From bf7b093c930c7ca0eec5662fe95f928f0ca7e299 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 10:19:57 +0200 Subject: [PATCH 26/41] Add BetaflightF3 --- src/main/target/BETAFLIGHTF3/readme.txt | 13 ++ src/main/target/BETAFLIGHTF3/target.c | 84 +++++++++++++ src/main/target/BETAFLIGHTF3/target.h | 151 ++++++++++++++++++++++++ src/main/target/BETAFLIGHTF3/target.mk | 17 +++ 4 files changed, 265 insertions(+) create mode 100755 src/main/target/BETAFLIGHTF3/readme.txt create mode 100755 src/main/target/BETAFLIGHTF3/target.c create mode 100755 src/main/target/BETAFLIGHTF3/target.h create mode 100755 src/main/target/BETAFLIGHTF3/target.mk diff --git a/src/main/target/BETAFLIGHTF3/readme.txt b/src/main/target/BETAFLIGHTF3/readme.txt new file mode 100755 index 0000000000..b6c5f279b0 --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/readme.txt @@ -0,0 +1,13 @@ +==BETAFLIGHTF3== + +Owner: BorisB + +Board information: + +- CPU - STM32F303CCT6 +- MPU-6000 +- SD Card Slot +- Build in 5V BEC + LC filter - board can be powered from main battery +- Built in current meter, PDB + +More info to come \ No newline at end of file diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c new file mode 100755 index 0000000000..9c177c4baa --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -0,0 +1,84 @@ +/* + * 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 . + */ + //Target code By Hector "Hectech FPV" Hind + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h new file mode 100755 index 0000000000..05b0807b6d --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -0,0 +1,151 @@ +/* + * 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 . + */ + //Target code By Hector "Hectech FPV" Hind + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BETAFC" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + + +//#define LED0 PC14 +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 17 + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_INSTANCE SPI1 + + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + +// MPU6000 interrupts +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PC13 +#define USE_EXTI + +#define USB_IO + +//#define USE_FLASHFS +//#define USE_FLASH_M25P16 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 5 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 6 + + +#undef USE_I2C + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +//GPIO_AF_1 + +#define SPI1_NSS_PIN PA15 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + + + + +#define USE_SDCARD +#define USE_SDCARD_SPI2 +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_INSTANCE SPI2 +//#define SDCARD_SPI_CS_GPIO SPI2_GPIO +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +//#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +//#define SDCARD_DMA_CHANNEL DMA_Channel_0 + + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define LED_STRIP + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER) + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) \ No newline at end of file diff --git a/src/main/target/BETAFLIGHTF3/target.mk b/src/main/target/BETAFLIGHTF3/target.mk new file mode 100755 index 0000000000..022c21a259 --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/target.mk @@ -0,0 +1,17 @@ + +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD +TARGET_FLAGS = -DSPRACINGF3 + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c + From 7df4fbe35716afe2bb98c24fded2d20553c6b5e5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 7 Sep 2016 15:21:52 +0200 Subject: [PATCH 27/41] Disable forced Enabling of Telemetry Inversion --- src/main/config/config.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b818805a75..3e058a6fec 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -891,12 +891,6 @@ void validateAndFixConfig(void) } #endif -#ifdef STM32F303xC - // hardware supports serial port inversion, make users life easier for those that want to connect SBus RX's - masterConfig.telemetryConfig.telemetry_inversion = 1; -#endif - - /*#if defined(LED_STRIP) && defined(TRANSPONDER) // TODO - Add transponder feature if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) { featureClear(FEATURE_LED_STRIP); From 35886233834a1b5480fef6fbcf4ad11ac7d98383 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 7 Sep 2016 15:34:15 +0200 Subject: [PATCH 28/41] Throttle Jump Limit only applied to accelerating situation --- src/main/flight/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6111a9883c..07418470a3 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -864,13 +864,13 @@ void mixTable(void *pidProfilePtr) // Anti Desync feature for ESC's. Limit rapid throttle changes if (escAndServoConfig->maxEscThrottleJumpMs) { - const int16_t maxThrottleStep = constrain(escAndServoConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 5, 10000); + const int16_t maxThrottleStep = constrain(escAndServoConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000); // Only makes sense when it's within the range if (maxThrottleStep < throttleRange) { static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; - motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); + motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, motorPrevious[i] + maxThrottleStep); // Only limit accelerating situation motorPrevious[i] = motor[i]; } } From 5096ef3aa196e278bccc4b1f5b0d6db001c0d4d6 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 25 Aug 2016 21:05:36 +0200 Subject: [PATCH 29/41] AlienFlight Target updates Add AlienFlight documentation --- src/main/target/ALIENFLIGHTF1/AlienFlight.md | 52 ++++++++++++++++++++ src/main/target/ALIENFLIGHTF1/target.h | 12 ++--- src/main/target/ALIENFLIGHTF3/AlienFlight.md | 52 ++++++++++++++++++++ src/main/target/ALIENFLIGHTF3/target.h | 5 +- src/main/target/ALIENFLIGHTF4/AlienFlight.md | 52 ++++++++++++++++++++ src/main/target/ALIENFLIGHTF4/target.h | 1 + 6 files changed, 164 insertions(+), 10 deletions(-) create mode 100644 src/main/target/ALIENFLIGHTF1/AlienFlight.md create mode 100644 src/main/target/ALIENFLIGHTF3/AlienFlight.md create mode 100644 src/main/target/ALIENFLIGHTF4/AlienFlight.md diff --git a/src/main/target/ALIENFLIGHTF1/AlienFlight.md b/src/main/target/ALIENFLIGHTF1/AlienFlight.md new file mode 100644 index 0000000000..cb6e154bdb --- /dev/null +++ b/src/main/target/ALIENFLIGHTF1/AlienFlight.md @@ -0,0 +1,52 @@ +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) + +AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: + +http://www.alienflight.com + +All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users or RC vendors to build this designs. + +Some variants of the AlienFlight controllers will be available for purchase from: + +http://www.microfpv.eu + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- STM32F405RGT6 MCU (ALIENFLIGHTF4) +- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) +- extra-wide traces on the PCB, for maximum power throughput (brushed variants) +- some new F4 boards using a 4-layer PCB for better power distribution +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) and SBUS +- CPPM input +- ground and 3.3V for the receiver, some boards have also the option to power an 5V receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from a single cell Lipoly battery for brushed versions +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- brushless versions are designed for 3S operation and also have an 5V power output +- battery monitoring with an LED for buzzer functionality (for some ALIENFLIGHTF3 and F4 variants only) + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. + +Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. + +## Flashing the firmware + +The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 6d945008f7..b80430b769 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -18,6 +18,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1. +#define TARGET_CONFIG #define LED0 PB3 #define LED1 PB4 @@ -25,7 +26,6 @@ #define BEEPER PA12 #define USE_EXTI -#define MAG_INT_EXTI PC14 #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL @@ -70,8 +70,9 @@ // USART2, PA3 #define BIND_PIN PA3 -// alternative defaults for AlienFlight F1 target -#define TARGET_CONFIG +#define HARDWARE_BIND_PLUG +// Hardware bind plug at PB5 (Pin 41) +#define BINDPLUG_PIN PB5 #define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP @@ -80,10 +81,7 @@ #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER -#define HARDWARE_BIND_PLUG -// Hardware bind plug at PB5 (Pin 41) -#define BINDPLUG_PIN PB5 - +#define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming all IOs on 48pin package #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/ALIENFLIGHTF3/AlienFlight.md b/src/main/target/ALIENFLIGHTF3/AlienFlight.md new file mode 100644 index 0000000000..cb6e154bdb --- /dev/null +++ b/src/main/target/ALIENFLIGHTF3/AlienFlight.md @@ -0,0 +1,52 @@ +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) + +AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: + +http://www.alienflight.com + +All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users or RC vendors to build this designs. + +Some variants of the AlienFlight controllers will be available for purchase from: + +http://www.microfpv.eu + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- STM32F405RGT6 MCU (ALIENFLIGHTF4) +- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) +- extra-wide traces on the PCB, for maximum power throughput (brushed variants) +- some new F4 boards using a 4-layer PCB for better power distribution +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) and SBUS +- CPPM input +- ground and 3.3V for the receiver, some boards have also the option to power an 5V receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from a single cell Lipoly battery for brushed versions +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- brushless versions are designed for 3S operation and also have an 5V power output +- battery monitoring with an LED for buzzer functionality (for some ALIENFLIGHTF3 and F4 variants only) + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. + +Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. + +## Flashing the firmware + +The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 28a896ff01..b92a100aaa 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -124,11 +124,10 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -// IO - assuming 303 in 64pin package, TODO +// IO - stm32f303cc in 48pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/AlienFlight.md b/src/main/target/ALIENFLIGHTF4/AlienFlight.md new file mode 100644 index 0000000000..cb6e154bdb --- /dev/null +++ b/src/main/target/ALIENFLIGHTF4/AlienFlight.md @@ -0,0 +1,52 @@ +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) + +AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: + +http://www.alienflight.com + +All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users or RC vendors to build this designs. + +Some variants of the AlienFlight controllers will be available for purchase from: + +http://www.microfpv.eu + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- STM32F405RGT6 MCU (ALIENFLIGHTF4) +- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) +- extra-wide traces on the PCB, for maximum power throughput (brushed variants) +- some new F4 boards using a 4-layer PCB for better power distribution +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) and SBUS +- CPPM input +- ground and 3.3V for the receiver, some boards have also the option to power an 5V receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from a single cell Lipoly battery for brushed versions +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- brushless versions are designed for 3S operation and also have an 5V power output +- battery monitoring with an LED for buzzer functionality (for some ALIENFLIGHTF3 and F4 variants only) + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. + +Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. + +## Flashing the firmware + +The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index a6fc694e6a..ef01181237 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -27,6 +27,7 @@ #define LED1 PD2 #define BEEPER PC13 +#define BEEPER_INVERTED #define INVERTER PC15 #define INVERTER_USART USART2 From 57c954e6088b90048a0223a853a6e8a91c4e2f76 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 7 Sep 2016 23:04:51 +0200 Subject: [PATCH 30/41] Slight notch default shift --- src/main/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 3e058a6fec..417a8b4969 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -240,8 +240,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default - pidProfile->dterm_notch_hz = 260; - pidProfile->dterm_notch_cutoff = 160; + pidProfile->dterm_notch_hz = 250; + pidProfile->dterm_notch_cutoff = 150; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; From bbd0b67134b30360038ed1d69f5490ab0a426268 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 8 Sep 2016 11:41:54 +0200 Subject: [PATCH 31/41] Revert "Slight notch default shift" This reverts commit 57c954e6088b90048a0223a853a6e8a91c4e2f76. --- src/main/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 417a8b4969..3e058a6fec 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -240,8 +240,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default - pidProfile->dterm_notch_hz = 250; - pidProfile->dterm_notch_cutoff = 150; + pidProfile->dterm_notch_hz = 260; + pidProfile->dterm_notch_cutoff = 160; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; From 5980c087b4a030d70ab4df40631191725bb84709 Mon Sep 17 00:00:00 2001 From: hrrr Date: Thu, 8 Sep 2016 19:06:45 +0200 Subject: [PATCH 32/41] Sparky 2 beeper correction --- src/main/target/SPARKY2/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index febebfa1fb..1b9a66974a 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -30,6 +30,8 @@ #define LED2 PB6 #define BEEPER PC9 +#define BEEPER_INVERTED + #define INVERTER PC6 #define INVERTER_USART USART6 From 0d1cc8f4484e9c5149b4471fdcf16a9e2c34e1bb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 8 Sep 2016 16:55:34 +0200 Subject: [PATCH 33/41] Reimplementation of P setpoint weight. Applied to S rates now for nicer feel --- src/main/blackbox/blackbox.c | 2 +- src/main/config/config.c | 10 +++++----- src/main/flight/pid.c | 11 +++-------- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 4 ++-- src/main/mw.c | 24 ++++++++++++++++++------ 7 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b9d60e1c10..5c0ca9f89a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1283,7 +1283,7 @@ static bool blackboxWriteSysinfo() 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("ptermSRateWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight); 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); diff --git a/src/main/config/config.c b/src/main/config/config.c index 3e058a6fec..ffb4367c95 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 145; +static const uint8_t EEPROM_CONF_VERSION = 146; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -235,7 +235,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 75; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->yaw_lpf_hz = 80; + pidProfile->yaw_lpf_hz = 0; pidProfile->rollPitchItermIgnoreRate = 130; pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSetpointWeight = 85; + pidProfile->ptermSRateWeight = 50; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -491,8 +491,8 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 0; - config->gyro_soft_notch_cutoff = 150; + config->gyro_soft_notch_hz = 210; + config->gyro_soft_notch_cutoff = 110; config->debug_mode = DEBUG_NONE; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8950db6ab1..234b98462b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,7 +49,7 @@ extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; +extern float setpointRate[3], ptermSetpointRate[3]; extern float rcInput[3]; static bool pidStabilisationEnabled; @@ -135,7 +135,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -187,7 +187,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); @@ -234,11 +233,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // ----- calculate error / angle rates ---------- errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; + rP = ptermSetpointRate[axis] - PVRate; // br - y // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount float dynReduction = tpaFactor; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index fd1fb08f5a..56082360eb 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -98,7 +98,7 @@ typedef struct pidProfile_s { uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm - uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) + uint8_t ptermSRateWeight; // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0b64d0526f..6099732163 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -854,7 +854,7 @@ const clivalue_t valueTable[] = { { "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 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, - { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 100 } }, + { "pterm_srate_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 33bd4a74c9..5d1367f999 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,7 +1268,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yaw_p_limit); serialize8(currentProfile->pidProfile.deltaMethod); serialize8(currentProfile->pidProfile.vbatPidCompensation); - serialize8(currentProfile->pidProfile.ptermSetpointWeight); + serialize8(currentProfile->pidProfile.ptermSRateWeight); serialize8(currentProfile->pidProfile.dtermSetpointWeight); serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); @@ -1866,7 +1866,7 @@ static bool processInCommand(void) currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); currentProfile->pidProfile.vbatPidCompensation = read8(); - currentProfile->pidProfile.ptermSetpointWeight = read8(); + currentProfile->pidProfile.ptermSRateWeight = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index faed2088b0..de92425d56 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -123,7 +123,7 @@ extern uint8_t PIDweight[3]; uint16_t filteredCycleTime; static bool isRXDataNew; static bool armingCalibrationWasInitialised; -float setpointRate[3]; +float setpointRate[3], ptermSetpointRate[3]; float rcInput[3]; extern pidControllerFuncPtr pid_controller; @@ -176,7 +176,7 @@ bool isCalibrating() #define RC_RATE_INCREMENTAL 14.54f #define RC_EXPO_POWER 3 -float calculateSetpointRate(int axis, int16_t rc) { +void calculateSetpointRate(int axis, int16_t rc) { float angleRate, rcRate, rcSuperfactor, rcCommandf; uint8_t rcExpo; @@ -201,7 +201,19 @@ float calculateSetpointRate(int axis, int16_t rc) { if (currentControlRateProfile->rates[axis]) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - angleRate *= rcSuperfactor; + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { + ptermSetpointRate[axis] = angleRate * rcSuperfactor; + if (currentProfile->pidProfile.ptermSRateWeight < 100) { + const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; + angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); + } else { + angleRate = ptermSetpointRate[axis]; + } + } else { + angleRate *= rcSuperfactor; + } + } else { + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) ptermSetpointRate[axis] = angleRate; } if (debugMode == DEBUG_ANGLERATE) { @@ -209,9 +221,9 @@ float calculateSetpointRate(int axis, int16_t rc) { } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection + setpointRate[axis] = constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else - return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) + setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { @@ -290,7 +302,7 @@ void processRcCommand(void) if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); - for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]); isRXDataNew = false; } From 13b189b4ffbd877012632ab0ed6fe64c26abaab7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 8 Sep 2016 22:37:35 +0200 Subject: [PATCH 34/41] Cleanup --- src/main/blackbox/blackbox.c | 3 --- src/main/config/config.c | 7 ++----- src/main/flight/pid.c | 34 +++------------------------------- src/main/flight/pid.h | 8 -------- src/main/io/serial_cli.c | 3 --- src/main/io/serial_msp.c | 8 ++++---- 6 files changed, 9 insertions(+), 54 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5c0ca9f89a..78d5ffc821 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1279,9 +1279,6 @@ static bool blackboxWriteSysinfo() 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("ptermSRateWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight); BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); diff --git a/src/main/config/config.c b/src/main/config/config.c index ffb4367c95..22cd6b94f1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,13 +247,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSRateWeight = 50; + pidProfile->ptermSRateWeight = 100; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; - pidProfile->toleranceBand = 0; - pidProfile->toleranceBandReduction = 40; - pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; #ifdef GTUNE @@ -491,7 +488,7 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 210; + config->gyro_soft_notch_hz = 220; config->gyro_soft_notch_cutoff = 110; config->debug_mode = DEBUG_NONE; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 234b98462b..523a50fc0c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -235,36 +235,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc errorRate = setpointRate[axis] - PVRate; // r - y rP = ptermSetpointRate[axis] - PVRate; // br - y - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; + PTerm = Kp[axis] * rP * tpaFactor; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -289,7 +261,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; // Filter delta if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); @@ -302,7 +274,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } } - DTerm = Kd[axis] * delta * dynReduction; + DTerm = Kd[axis] * delta * tpaFactor; // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56082360eb..df266e3273 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -63,11 +63,6 @@ typedef enum { SUPEREXPO_YAW_ALWAYS } pidSuperExpoYaw_e; -typedef enum { - NEGATIVE_ERROR = 0, - POSITIVE_ERROR -} pidErrorPolarity_e; - typedef enum { PID_STABILISATION_OFF = 0, PID_STABILISATION_ON @@ -94,9 +89,6 @@ typedef struct pidProfile_s { 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 - uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage - uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSRateWeight; // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6099732163..6cbb66ee5c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -850,9 +850,6 @@ const clivalue_t valueTable[] = { { "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 } }, { "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 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_srate_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5d1367f999..9b40111dbd 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1270,8 +1270,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.vbatPidCompensation); serialize8(currentProfile->pidProfile.ptermSRateWeight); serialize8(currentProfile->pidProfile.dtermSetpointWeight); - serialize8(currentProfile->pidProfile.toleranceBand); - serialize8(currentProfile->pidProfile.toleranceBandReduction); + serialize8(0); // reserved + serialize8(0); // reserved serialize8(currentProfile->pidProfile.itermThrottleGain); serialize16(currentProfile->pidProfile.rateAccelLimit); serialize16(currentProfile->pidProfile.yawRateAccelLimit); @@ -1868,8 +1868,8 @@ static bool processInCommand(void) currentProfile->pidProfile.vbatPidCompensation = read8(); currentProfile->pidProfile.ptermSRateWeight = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); - currentProfile->pidProfile.toleranceBand = read8(); - currentProfile->pidProfile.toleranceBandReduction = read8(); + read8(); // reserved + read8(); // reserved currentProfile->pidProfile.itermThrottleGain = read8(); currentProfile->pidProfile.rateAccelLimit = read16(); currentProfile->pidProfile.yawRateAccelLimit = read16(); From 389a27e3f156b6cb501bb28bfdfe6184d6d75bf9 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 09:21:49 +0200 Subject: [PATCH 35/41] Remove gyro notch as default // Remove yaw srate ratio --- src/main/config/config.c | 6 +++--- src/main/mw.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 22cd6b94f1..28b4dbc0e9 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSRateWeight = 100; + pidProfile->ptermSRateWeight = 85; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -488,8 +488,8 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 220; - config->gyro_soft_notch_cutoff = 110; + config->gyro_soft_notch_hz = 0; + config->gyro_soft_notch_cutoff = 130; config->debug_mode = DEBUG_NONE; diff --git a/src/main/mw.c b/src/main/mw.c index de92425d56..9c7bf171e7 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -203,7 +203,7 @@ void calculateSetpointRate(int axis, int16_t rc) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { ptermSetpointRate[axis] = angleRate * rcSuperfactor; - if (currentProfile->pidProfile.ptermSRateWeight < 100) { + if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW) { const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); } else { From cab7b562d8dec756d925380162f47a93f1e5e0eb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 09:55:46 +0200 Subject: [PATCH 36/41] Change gyro filter / debug order --- src/main/sensors/gyro.c | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a17b61df20..7c39168568 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -158,7 +158,6 @@ void gyroUpdate(void) } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; gyroADC[axis] = gyroADCRaw[axis]; } @@ -172,21 +171,21 @@ void gyroUpdate(void) if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float sample = (float) gyroADC[axis]; - if (gyroSoftNotchHz) { - sample = biquadFilterApply(&gyroFilterNotch[axis], sample); - } - if (debugMode == DEBUG_NOTCH && axis < 2){ - debug[axis*2 + 0] = gyroADC[axis]; - debug[axis*2 + 1] = lrintf(sample); - } + if (debugMode == DEBUG_GYRO) + debug[axis] = gyroADC[axis]; + + if (gyroSoftLpfType == FILTER_BIQUAD) + gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); + else + gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); + + if (debugMode == DEBUG_NOTCH) + debug[axis] = lrintf(gyroADCf[axis]); + + if (gyroSoftNotchHz) + gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]); - if (gyroSoftLpfType == FILTER_BIQUAD) { - gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample); - } else { - gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt); - } gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { From c595a789fcdf4347d6d1a3fbcbeeb75df4bac05f Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Fri, 9 Sep 2016 10:25:48 +0200 Subject: [PATCH 37/41] removed MAP_TO_SERVO_OUTPUT on PWM8/9 probably it fixed #1129 --- src/main/target/SPRACINGF3EVO/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 488e9a7a9c..843ad3d9aa 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,8 +30,8 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -44,8 +44,8 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF From cfa0d686790358cf6df1557b06a15dc346da689c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 11:38:46 +0200 Subject: [PATCH 38/41] Match rate limit for ptermSetpointRate --- 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 9c7bf171e7..54b605a120 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -202,7 +202,7 @@ void calculateSetpointRate(int axis, int16_t rc) { if (currentControlRateProfile->rates[axis]) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { - ptermSetpointRate[axis] = angleRate * rcSuperfactor; + ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f); if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW) { const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); From 0720fcf463f1671259fcecbaa8f14c9ad938cf3e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 12:04:31 +0200 Subject: [PATCH 39/41] Add Back Some Dirty target code See #1131 #1127 --- src/main/drivers/pwm_mapping.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2a484352e5..08e0ec0bcc 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -242,6 +242,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif +#if defined(SPRACINGF3EVO) + // remap PWM6+7 as servos + if ((timerIndex == PWM8 || timerIndex == PWM9) && timerHardwarePtr->tim == TIM3) + type = MAP_TO_SERVO_OUTPUT; +#endif + #if (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3)) // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer if (init->useSoftSerial) { From cb16d65532bdb582e51c8fa9364a6bff1524af27 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 10 Sep 2016 23:29:48 +0200 Subject: [PATCH 40/41] Fix level mode in betaflight PID controller --- src/main/flight/pid.c | 4 ++-- src/main/mw.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 523a50fc0c..39fa4b8064 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -218,11 +218,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + ptermSetpointRate[axis] = setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + ptermSetpointRate[axis] = setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); } } diff --git a/src/main/mw.c b/src/main/mw.c index 54b605a120..b112aa6678 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -203,7 +203,7 @@ void calculateSetpointRate(int axis, int16_t rc) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f); - if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW) { + if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW && !flightModeFlags) { const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); } else { From a21694b065e6305082925e6fee81d242535110b1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 11 Sep 2016 00:22:05 +0200 Subject: [PATCH 41/41] Enable Telemetry Inversion by Default --- 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 28b4dbc0e9..90effae403 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -331,7 +331,7 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) #ifdef TELEMETRY void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) { - telemetryConfig->telemetry_inversion = 0; + telemetryConfig->telemetry_inversion = 1; telemetryConfig->telemetry_switch = 0; telemetryConfig->gpsNoFixLatitude = 0; telemetryConfig->gpsNoFixLongitude = 0;