diff --git a/Makefile b/Makefile
index 1cc3068633..417fe68d8d 100644
--- a/Makefile
+++ b/Makefile
@@ -76,13 +76,14 @@ LINKER_DIR = $(ROOT)/src/main/target/link
# Build tools, so we all share the same versions
# import macros common to all supported build systems
include $(ROOT)/make/system-id.mk
+
# developer preferences, edit these at will, they'll be gitignored
-include $(ROOT)/make/local.mk
+-include $(ROOT)/make/local.mk
# configure some directories that are relative to wherever ROOT_DIR is located
TOOLS_DIR := $(ROOT)/tools
BUILD_DIR := $(ROOT)/build
-DL_DIR := $(ROOT)/downloads
+DL_DIR := $(ROOT)/downloads
export RM := rm
@@ -102,7 +103,6 @@ SAMPLE_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 CC3
ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk)))))
OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS))
-#VALID_TARGETS = $(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS)
VALID_TARGETS = $(dir $(wildcard $(ROOT)/src/main/target/*/target.mk))
VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS)))
VALID_TARGETS := $(VALID_TARGETS) $(ALT_TARGETS)
@@ -123,7 +123,7 @@ endif
-include $(ROOT)/src/main/target/$(BASE_TARGET)/target.mk
F4_TARGETS = $(F405_TARGETS) $(F411_TARGETS)
-F7_TARGETS = $(F7X5XE_TARGETS) $(F7X5XG_TARGETS) $(F7X5XI_TARGETS) $(F7X6XG_TARGETS)
+F7_TARGETS = $(F7X2RE_TARGETS) $(F7X5XE_TARGETS) $(F7X5XG_TARGETS) $(F7X5XI_TARGETS) $(F7X6XG_TARGETS)
ifeq ($(filter $(TARGET),$(VALID_TARGETS)),)
$(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?)
@@ -135,7 +135,7 @@ endif
128K_TARGETS = $(F1_TARGETS)
256K_TARGETS = $(F3_TARGETS)
-512K_TARGETS = $(F411_TARGETS) $(F7X5XE_TARGETS)
+512K_TARGETS = $(F411_TARGETS) $(F7X2RE_TARGETS) $(F7X5XE_TARGETS)
1024K_TARGETS = $(F405_TARGETS) $(F7X5XG_TARGETS) $(F7X6XG_TARGETS)
2048K_TARGETS = $(F7X5XI_TARGETS)
@@ -192,6 +192,7 @@ STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver
STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c))
EXCLUDES = stm32f30x_crc.c \
stm32f30x_can.c
+STARTUP_SRC = startup_stm32f30x_md_gcc.S
STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC)
@@ -256,12 +257,11 @@ EXCLUDES = stm32f4xx_crc.c \
stm32f4xx_cryp_tdes.c \
stm32f4xx_hash_sha1.c
-
ifeq ($(TARGET),$(filter $(TARGET), $(F411_TARGETS)))
-EXCLUDES += stm32f4xx_fsmc.c
+EXCLUDES += stm32f4xx_fsmc.c
endif
-STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
+STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
#USB
USBCORE_DIR = $(ROOT)/lib/main/STM32_USB_Device_Library/Core
@@ -312,9 +312,11 @@ ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu
ifeq ($(TARGET),$(filter $(TARGET),$(F411_TARGETS)))
DEVICE_FLAGS = -DSTM32F411xE
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f411.ld
+STARTUP_SRC = startup_stm32f411xe.s
else ifeq ($(TARGET),$(filter $(TARGET),$(F405_TARGETS)))
DEVICE_FLAGS = -DSTM32F40_41xxx
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f405.ld
+STARTUP_SRC = startup_stm32f40xx.s
else
$(error Unknown MCU for F4 target)
endif
@@ -329,10 +331,10 @@ else ifeq ($(TARGET),$(filter $(TARGET), $(F7_TARGETS)))
STDPERIPH_DIR = $(ROOT)/lib/main/STM32F7xx_HAL_Driver
STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/Src/*.c))
EXCLUDES = stm32f7xx_hal_timebase_rtc_wakeup_template.c \
- stm32f7xx_hal_timebase_rtc_alarm_template.c \
- stm32f7xx_hal_timebase_tim_template.c
+ stm32f7xx_hal_timebase_rtc_alarm_template.c \
+ stm32f7xx_hal_timebase_tim_template.c
-STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
+STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
#USB
USBCORE_DIR = $(ROOT)/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core
@@ -376,15 +378,21 @@ ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fs
ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS)))
DEVICE_FLAGS = -DSTM32F745xx -DUSE_HAL_DRIVER -D__FPU_PRESENT
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f745.ld
+STARTUP_SRC = startup_stm32f745xx.s
else ifeq ($(TARGET),$(filter $(TARGET),$(F7X6XG_TARGETS)))
DEVICE_FLAGS = -DSTM32F746xx -DUSE_HAL_DRIVER -D__FPU_PRESENT
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f746.ld
+STARTUP_SRC = startup_stm32f746xx.s
+else ifeq ($(TARGET),$(filter $(TARGET),$(F7X2RE_TARGETS)))
+DEVICE_FLAGS = -DSTM32F722xx -DUSE_HAL_DRIVER -D__FPU_PRESENT
+LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f722.ld
+STARTUP_SRC = startup_stm32f722xx.s
else
$(error Unknown MCU for F7 target)
endif
DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE)
-TARGET_FLAGS = -D$(TARGET)
+TARGET_FLAGS = -D$(TARGET)
# End F7 targets
#
@@ -396,7 +404,7 @@ STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c))
EXCLUDES = stm32f10x_crc.c \
stm32f10x_cec.c \
stm32f10x_can.c
-
+STARTUP_SRC = startup_stm32f10x_md_gcc.S
STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
# Search path and source files for the CMSIS sources
@@ -516,7 +524,7 @@ COMMON_SRC = \
fc/config.c \
fc/fc_tasks.c \
fc/fc_msp.c \
- fc/mw.c \
+ fc/fc_main.c \
fc/rc_controls.c \
fc/rc_curves.c \
fc/runtime_config.c \
@@ -578,7 +586,6 @@ HIGHEND_SRC = \
drivers/serial_escserial.c \
drivers/serial_softserial.c \
drivers/sonar_hcsr04.c \
- flight/gtune.c \
flight/navigation.c \
flight/gps_conversion.c \
io/dashboard.c \
@@ -735,7 +742,6 @@ VCP_SRC = \
endif
STM32F10x_COMMON_SRC = \
- startup_stm32f10x_md_gcc.S \
drivers/adc_stm32f10x.c \
drivers/bus_i2c_stm32f10x.c \
drivers/dma.c \
@@ -747,7 +753,6 @@ STM32F10x_COMMON_SRC = \
drivers/timer_stm32f10x.c
STM32F30x_COMMON_SRC = \
- startup_stm32f30x_md_gcc.S \
target/system_stm32f30x.c \
drivers/adc_stm32f30x.c \
drivers/bus_i2c_stm32f30x.c \
@@ -760,7 +765,6 @@ STM32F30x_COMMON_SRC = \
drivers/timer_stm32f30x.c
STM32F4xx_COMMON_SRC = \
- startup_stm32f40xx.s \
target/system_stm32f4xx.c \
drivers/accgyro_mpu.c \
drivers/adc_stm32f4xx.c \
@@ -775,7 +779,6 @@ STM32F4xx_COMMON_SRC = \
drivers/timer_stm32f4xx.c
STM32F7xx_COMMON_SRC = \
- startup_stm32f745xx.s \
target/system_stm32f7xx.c \
drivers/accgyro_mpu.c \
drivers/adc_stm32f7xx.c \
@@ -798,13 +801,13 @@ F7EXCLUDES = drivers/bus_spi.c \
# check if target.mk supplied
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
-TARGET_SRC := $(STM32F4xx_COMMON_SRC) $(TARGET_SRC)
+TARGET_SRC := $(STARTUP_SRC) $(STM32F4xx_COMMON_SRC) $(TARGET_SRC)
else ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
-TARGET_SRC := $(STM32F7xx_COMMON_SRC) $(TARGET_SRC)
+TARGET_SRC := $(STARTUP_SRC) $(STM32F7xx_COMMON_SRC) $(TARGET_SRC)
else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
-TARGET_SRC := $(STM32F30x_COMMON_SRC) $(TARGET_SRC)
+TARGET_SRC := $(STARTUP_SRC) $(STM32F30x_COMMON_SRC) $(TARGET_SRC)
else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
-TARGET_SRC := $(STM32F10x_COMMON_SRC) $(TARGET_SRC)
+TARGET_SRC := $(STARTUP_SRC) $(STM32F10x_COMMON_SRC) $(TARGET_SRC)
endif
ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
diff --git a/make/local.mk b/make/local.mk
deleted file mode 100644
index 1b66457a50..0000000000
--- a/make/local.mk
+++ /dev/null
@@ -1,2 +0,0 @@
-# override the toolchain version, should match the output from of your version of the toolchain: $(arm-none-eabi-gcc -dumpversion)
-#GCC_REQUIRED_VERSION=5.4.1
diff --git a/make/tools.mk b/make/tools.mk
index 1e4323c111..810716a57d 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -16,7 +16,7 @@
# Set up ARM (STM32) SDK
ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q3
# Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion)
-GCC_REQUIRED_VERSION := 5.4.1
+GCC_REQUIRED_VERSION ?= 5.4.1
## arm_sdk_install : Install Arm SDK
.PHONY: arm_sdk_install
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index f118ebcbb9..ddc83acab3 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -187,7 +187,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"debug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"debug", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
/* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */
- {"motor", 0, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
+ {"motor", 0, UNSIGNED, .Ipredict = PREDICT(MINMOTOR), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
/* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
{"motor", 1, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
{"motor", 2, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
@@ -289,6 +289,9 @@ typedef struct blackboxSlowState_s {
bool rxFlightChannelsValid;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
+//From mixer.c:
+extern uint16_t motorOutputHigh, motorOutputLow;
+
//From rc_controls.c
extern uint32_t rcModeActivationMask;
@@ -535,8 +538,8 @@ static void writeIntraframe(void)
blackboxWriteSigned16VBArray(blackboxCurrent->accSmooth, XYZ_AXIS_COUNT);
blackboxWriteSigned16VBArray(blackboxCurrent->debug, 4);
- //Motors can be below minthrottle when disarmed, but that doesn't happen much
- blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
+ //Motors can be below minimum output when disarmed, but that doesn't happen much
+ blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorOutputLow);
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
const int motorCount = getMotorCount();
@@ -1166,6 +1169,8 @@ static bool blackboxWriteSysinfo()
return false;
}
+ const profile_t *currentProfile = &masterConfig.profile[masterConfig.current_profile_index];
+ const controlRateConfig_t *currentControlRateProfile = ¤tProfile->controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile];
switch (xmitState.headerIndex) {
BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight");
BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
@@ -1175,6 +1180,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x", castFloatBytesToInt(1.0f));
+ BLACKBOX_PRINT_HEADER_LINE("motorOutput:%d,%d", motorOutputLow,motorOutputHigh);
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
@@ -1200,8 +1206,6 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", pidConfig()->pid_process_denom);
- const profile_t *currentProfile = &masterConfig.profile[masterConfig.current_profile_index];
- const controlRateConfig_t *currentControlRateProfile = ¤tProfile->controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile];
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", currentControlRateProfile->rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", currentControlRateProfile->rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", currentControlRateProfile->rcYawRate8);
@@ -1254,11 +1258,11 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentProfile->pidProfile.pidAtMinThrottle);
// Betaflight PID controller parameters
- BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", currentProfile->pidProfile.itermThrottleGain);
+ BLACKBOX_PRINT_HEADER_LINE("itermThrottleThreshold:%d", currentProfile->pidProfile.itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", currentProfile->pidProfile.setpointRelaxRatio);
BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentProfile->pidProfile.dtermSetpointWeight);
- BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", currentProfile->pidProfile.yawRateAccelLimit);
- BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", currentProfile->pidProfile.rateAccelLimit);
+ BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.yawRateAccelLimit));
+ BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.rateAccelLimit));
// End of Betaflight controller parameters
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband);
@@ -1282,6 +1286,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", motorConfig()->useUnsyncedPwm);
BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", motorConfig()->motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", motorConfig()->motorPwmRate);
+ BLACKBOX_PRINT_HEADER_LINE("digitalIdleOffset:%d", (int)(motorConfig()->digitalIdleOffsetPercent * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", masterConfig.debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);
@@ -1325,11 +1330,6 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
blackboxWriteSignedVB(data->inflightAdjustment.newValue);
}
break;
- case FLIGHT_LOG_EVENT_GTUNE_RESULT:
- blackboxWrite(data->gtuneCycleResult.gtuneAxis);
- blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG);
- blackboxWriteS16(data->gtuneCycleResult.gtuneNewP);
- break;
case FLIGHT_LOG_EVENT_LOGGING_RESUME:
blackboxWriteUnsignedVB(data->loggingResume.logIteration);
blackboxWriteUnsignedVB(data->loggingResume.currentTime);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index 490c271b2f..6260ddb426 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -80,7 +80,10 @@ typedef enum FlightLogFieldPredictor {
FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9,
//Predict the last time value written in the main stream
- FLIGHT_LOG_FIELD_PREDICTOR_LAST_MAIN_FRAME_TIME = 10
+ FLIGHT_LOG_FIELD_PREDICTOR_LAST_MAIN_FRAME_TIME = 10,
+
+ //Predict that this field is the minimum motor output
+ FLIGHT_LOG_FIELD_PREDICTOR_MINMOTOR = 11
} FlightLogFieldPredictor;
@@ -103,7 +106,6 @@ typedef enum FlightLogEvent {
FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
- FLIGHT_LOG_EVENT_GTUNE_RESULT = 20,
FLIGHT_LOG_EVENT_FLIGHTMODE = 30, // Add new event type for flight mode status.
FLIGHT_LOG_EVENT_LOG_END = 255
} FlightLogEvent;
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index c7faeb073e..1cd9806efa 100644
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -80,6 +80,7 @@ OSD_Entry menuOsdActiveElemsEntries[] =
{"GPS SATS.", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SATS], 0},
#endif // GPS
{"ALTITUDE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ALTITUDE], 0},
+ {"POWER", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_POWER], 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 40d9db4db8..c42ef32f0e 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -30,7 +30,6 @@
#define PG_CONTROL_RATE_PROFILES 12 // struct OK, needs to be split out of rc_controls.h into rate_profile.h
#define PG_SERIAL_CONFIG 13 // struct OK
#define PG_PID_PROFILE 14 // struct OK, CF differences
-#define PG_GTUNE_CONFIG 15 // is GTUNE still being used?
#define PG_ARMING_CONFIG 16 // structs OK, CF naming differences
#define PG_TRANSPONDER_CONFIG 17 // struct OK
#define PG_SYSTEM_CONFIG 18 // just has i2c_highspeed
diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h
index 9661b3a71c..2d1620ef14 100644
--- a/src/main/drivers/accgyro.h
+++ b/src/main/drivers/accgyro.h
@@ -42,12 +42,12 @@ typedef struct gyroDev_s {
sensorGyroInterruptStatusFuncPtr intStatus;
extiCallbackRec_t exti;
float scale; // scalefactor
- volatile bool dataReady;
- uint16_t lpf;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+ uint16_t lpf;
+ volatile bool dataReady;
sensor_align_e gyroAlign;
- const extiConfig_t *mpuIntExtiConfig;
mpuDetectionResult_t mpuDetectionResult;
+ const extiConfig_t *mpuIntExtiConfig;
mpuConfiguration_t mpuConfiguration;
} gyroDev_t;
diff --git a/src/main/drivers/accgyro_adxl345.h b/src/main/drivers/accgyro_adxl345.h
index ee664fc862..50c675fa0e 100644
--- a/src/main/drivers/accgyro_adxl345.h
+++ b/src/main/drivers/accgyro_adxl345.h
@@ -18,8 +18,8 @@
#pragma once
typedef struct drv_adxl345_config_s {
- bool useFifo;
uint16_t dataRate;
+ bool useFifo;
} drv_adxl345_config_t;
bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc);
diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h
index 111491598f..1a52f423d3 100644
--- a/src/main/drivers/accgyro_mpu.h
+++ b/src/main/drivers/accgyro_mpu.h
@@ -124,12 +124,12 @@ typedef void(*mpuResetFuncPtr)(void);
extern mpuResetFuncPtr mpuReset;
typedef struct mpuConfiguration_s {
- uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
mpuReadRegisterFunc read;
mpuWriteRegisterFunc write;
mpuReadRegisterFunc slowread;
mpuWriteRegisterFunc verifywrite;
mpuResetFuncPtr reset;
+ uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;
enum gyro_fsr_e {
diff --git a/src/main/drivers/io_def.h b/src/main/drivers/io_def.h
index 5360dfbc96..a200f4af40 100644
--- a/src/main/drivers/io_def.h
+++ b/src/main/drivers/io_def.h
@@ -40,7 +40,7 @@
#define DEFIO_REC_INDEXED(idx) (ioRecs + (idx))
// ioTag_t accessor macros
-#define DEFIO_TAG_MAKE(gpioid, pin) ((((gpioid) + 1) << 4) | (pin))
+#define DEFIO_TAG_MAKE(gpioid, pin) ((ioTag_t)((((gpioid) + 1) << 4) | (pin)))
#define DEFIO_TAG_ISEMPTY(tag) (!(tag))
#define DEFIO_TAG_GPIOID(tag) (((tag) >> 4) - 1)
#define DEFIO_TAG_PIN(tag) ((tag) & 0x0f)
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 24c6636c24..992e78d4e2 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -168,34 +168,21 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->pidSumLimit = PIDSUM_LIMIT;
pidProfile->yaw_lpf_hz = 0;
- pidProfile->rollPitchItermIgnoreRate = 130;
- pidProfile->yawItermIgnoreRate = 32;
+ pidProfile->rollPitchItermIgnoreRate = 200;
+ pidProfile->yawItermIgnoreRate = 55;
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->vbatPidCompensation = 0;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
-
- // Betaflight PID controller parameters
+ pidProfile->levelAngleLimit = 70.0f; // 70 degrees
pidProfile->setpointRelaxRatio = 30;
pidProfile->dtermSetpointWeight = 200;
- pidProfile->yawRateAccelLimit = 220;
- pidProfile->rateAccelLimit = 0;
- pidProfile->itermThrottleGain = 0;
- pidProfile->levelSensitivity = 2.0f;
-
-#ifdef GTUNE
- pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
- pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune.
- pidProfile->gtune_lolimP[YAW] = 10; // [0..200] Lower limit of YAW P during G tune.
- pidProfile->gtune_hilimP[ROLL] = 100; // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis.
- pidProfile->gtune_hilimP[PITCH] = 100; // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis.
- pidProfile->gtune_hilimP[YAW] = 100; // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis.
- pidProfile->gtune_pwr = 0; // [0..10] Strength of adjustment
- pidProfile->gtune_settle_time = 450; // [200..1000] Settle time in ms
- pidProfile->gtune_average_cycles = 16; // [8..128] Number of looptime cycles used for gyro average calculation
-#endif
+ pidProfile->yawRateAccelLimit = 20.0f;
+ pidProfile->rateAccelLimit = 0.0f;
+ pidProfile->itermThrottleThreshold = 350;
+ pidProfile->levelSensitivity = 100.0f;
}
void resetProfile(profile_t *profile)
@@ -624,7 +611,6 @@ void createDefaultConfig(master_t *config)
config->gyroConfig.gyro_sync_denom = 4;
config->pidConfig.pid_process_denom = 2;
#endif
- config->pidConfig.max_angle_inclination = 700; // 70 degrees
config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1;
config->gyroConfig.gyro_soft_lpf_hz = 90;
config->gyroConfig.gyro_soft_notch_hz_1 = 400;
@@ -1095,7 +1081,7 @@ void changeProfile(uint8_t profileIndex)
void changeControlRateProfile(uint8_t profileIndex)
{
- if (profileIndex > MAX_RATEPROFILES) {
+ if (profileIndex >= MAX_RATEPROFILES) {
profileIndex = MAX_RATEPROFILES - 1;
}
setControlRateProfile(profileIndex);
diff --git a/src/main/fc/mw.c b/src/main/fc/fc_main.c
similarity index 92%
rename from src/main/fc/mw.c
rename to src/main/fc/fc_main.c
index f2dcaff64e..e1bfee8b59 100644
--- a/src/main/fc/mw.c
+++ b/src/main/fc/fc_main.c
@@ -63,7 +63,6 @@
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
-#include "flight/gtune.h"
#include "flight/altitudehold.h"
#include "config/config_profile.h"
@@ -93,13 +92,28 @@ uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
-extern uint8_t PIDweight[3];
+static float throttlePIDAttenuation;
uint16_t filteredCycleTime;
bool isRXDataNew;
static bool armingCalibrationWasInitialised;
-float setpointRate[3];
-float rcInput[3];
+static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
+
+float getThrottlePIDAttenuation(void) {
+ return throttlePIDAttenuation;
+}
+
+float getSetpointRate(int axis) {
+ return setpointRate[axis];
+}
+
+float getRcDeflection(int axis) {
+ return rcDeflection[axis];
+}
+
+float getRcDeflectionAbs(int axis) {
+ return rcDeflectionAbs[axis];
+}
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
@@ -109,30 +123,6 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
saveConfigAndNotify();
}
-#ifdef GTUNE
-
-void updateGtuneState(void)
-{
- static bool GTuneWasUsed = false;
-
- if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
- if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
- ENABLE_FLIGHT_MODE(GTUNE_MODE);
- init_Gtune(¤tProfile->pidProfile);
- GTuneWasUsed = true;
- }
- if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
- saveConfigAndNotify();
- GTuneWasUsed = false;
- }
- } else {
- if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
- DISABLE_FLIGHT_MODE(GTUNE_MODE);
- }
- }
-}
-#endif
-
bool isCalibrating()
{
#ifdef BARO
@@ -162,11 +152,12 @@ void 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);
+ rcDeflection[axis] = rcCommandf;
+ rcDeflectionAbs[axis] = ABS(rcCommandf);
if (rcExpo) {
float expof = rcExpo / 100.0f;
- rcCommandf = rcCommandf * power3(rcInput[axis]) * expof + rcCommandf * (1-expof);
+ rcCommandf = rcCommandf * power3(rcDeflection[axis]) * expof + rcCommandf * (1-expof);
}
angleRate = 200.0f * rcRate * rcCommandf;
@@ -199,20 +190,45 @@ void scaleRcCommandToFpvCamAngle(void) {
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
+#define THROTTLE_BUFFER_MAX 20
+#define THROTTLE_DELTA_MS 100
+
+ void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
+ static int index;
+ static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
+ const int rxRefreshRateMs = rxRefreshRate / 1000;
+ const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
+ const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold;
+
+ rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
+ if (index >= indexMax)
+ index = 0;
+
+ const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
+
+ if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
+ pidResetErrorGyroState();
+}
+
void processRcCommand(void)
{
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
+ static uint16_t currentRxRefreshRate;
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
+ if (isRXDataNew) {
+ currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
+ checkForThrottleErrorResetState(currentRxRefreshRate);
+ }
+
if (rxConfig()->rcInterpolation || flightModeFlags) {
- if (isRXDataNew) {
- // Set RC refresh rate for sampling and channels to filter
- switch (rxConfig()->rcInterpolation) {
+ // Set RC refresh rate for sampling and channels to filter
+ switch(rxConfig()->rcInterpolation) {
case(RC_SMOOTHING_AUTO):
- rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps
+ rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
break;
case(RC_SMOOTHING_MANUAL):
rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
@@ -221,8 +237,9 @@ void processRcCommand(void)
case(RC_SMOOTHING_DEFAULT):
default:
rxRefreshRate = rxGetRefreshRate();
- }
+ }
+ if (isRXDataNew) {
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
if (debugMode == DEBUG_RC_INTERPOLATION) {
@@ -269,17 +286,18 @@ void updateRcCommands(void)
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop = 100;
+ throttlePIDAttenuation = 1.0f;
} else {
if (rcData[THROTTLE] < 2000) {
prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop = 100 - currentControlRateProfile->dynThrPID;
}
+ throttlePIDAttenuation = prop / 100.0f;
}
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
- PIDweight[axis] = prop;
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
@@ -678,9 +696,7 @@ void subTaskPidController(void)
// PID - note this is function pointer set by setPIDController()
pidController(
¤tProfile->pidProfile,
- pidConfig()->max_angle_inclination,
- &accelerometerConfig()->accelerometerTrims,
- rxConfig()->midrc
+ &accelerometerConfig()->accelerometerTrims
);
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;}
}
diff --git a/src/main/fc/mw.h b/src/main/fc/fc_main.h
similarity index 88%
rename from src/main/fc/mw.h
rename to src/main/fc/fc_main.h
index 27c5680249..1471e15a8d 100644
--- a/src/main/fc/mw.h
+++ b/src/main/fc/fc_main.h
@@ -34,3 +34,7 @@ void updateLEDs(void);
void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs);
+float getThrottlePIDAttenuation(void);
+float getSetpointRate(int axis);
+float getRcDeflection(int axis);
+float getRcDeflectionAbs(int axis);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index aceb7c8e34..5c14903d21 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -49,7 +49,7 @@
#include "drivers/serial_escserial.h"
#include "fc/config.h"
-#include "fc/mw.h"
+#include "fc/fc_main.h"
#include "fc/fc_msp.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
@@ -401,10 +401,6 @@ void initActiveBoxIds(void)
}
#endif
-#ifdef GTUNE
- activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
-#endif
-
#ifdef USE_SERVOS
if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
@@ -1167,9 +1163,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, currentProfile->pidProfile.dtermSetpointWeight);
sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved
- sbufWriteU8(dst, currentProfile->pidProfile.itermThrottleGain);
- sbufWriteU16(dst, currentProfile->pidProfile.rateAccelLimit);
- sbufWriteU16(dst, currentProfile->pidProfile.yawRateAccelLimit);
+ sbufWriteU8(dst, 0); // reserved
+ sbufWriteU16(dst, currentProfile->pidProfile.rateAccelLimit * 10);
+ sbufWriteU16(dst, currentProfile->pidProfile.yawRateAccelLimit * 10);
break;
case MSP_SENSOR_CONFIG:
@@ -1515,9 +1511,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
currentProfile->pidProfile.dtermSetpointWeight = sbufReadU8(src);
sbufReadU8(src); // reserved
sbufReadU8(src); // reserved
- currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src);
- currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src);
- currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src);
+ sbufReadU8(src); // reserved
+ currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src) / 10.0f;
+ currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src) / 10.0f;
pidInitConfig(¤tProfile->pidProfile);
break;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index d9943bd9a3..1678353ca6 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -36,7 +36,7 @@
#include "fc/config.h"
#include "fc/fc_msp.h"
#include "fc/fc_tasks.h"
-#include "fc/mw.h"
+#include "fc/fc_main.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index 1c8e51fe25..3cbca0b927 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -35,7 +35,7 @@
#include "drivers/system.h"
#include "fc/config.h"
-#include "fc/mw.h"
+#include "fc/fc_main.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/runtime_config.h"
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 04441c565e..f88e152a7b 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -42,8 +42,7 @@ typedef enum {
UNUSED_MODE = (1 << 7), // old autotune
PASSTHRU_MODE = (1 << 8),
SONAR_MODE = (1 << 9),
- FAILSAFE_MODE = (1 << 10),
- GTUNE_MODE = (1 << 11)
+ FAILSAFE_MODE = (1 << 10)
} flightModeFlags_e;
extern uint16_t flightModeFlags;
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
deleted file mode 100644
index e4e85fc62e..0000000000
--- a/src/main/flight/gtune.c
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-#include
-#include
-#include
-#include
-
-#include "platform.h"
-
-#ifdef GTUNE
-
-#include "common/axis.h"
-#include "common/maths.h"
-
-#include "drivers/system.h"
-#include "drivers/sensor.h"
-#include "drivers/accgyro.h"
-
-#include "sensors/sensors.h"
-#include "sensors/gyro.h"
-#include "sensors/acceleration.h"
-
-#include "flight/pid.h"
-#include "flight/imu.h"
-
-#include "blackbox/blackbox.h"
-
-#include "fc/config.h"
-#include "fc/rc_controls.h"
-#include "fc/runtime_config.h"
-
-
-extern uint16_t cycleTime;
-extern uint8_t motorCount;
-
-/*
- ****************************************************************************
- *** G_Tune ***
- ****************************************************************************
- G_Tune Mode
- This is the multiwii implementation of ZERO-PID Algorithm
- http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
- The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
-
- You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution.
- */
-
-/*
- version 1.0.0: MIN & Maxis & Tuned Band
- version 1.0.1:
- a. error is gyro reading not rc - gyro.
- b. OldError = Error no averaging.
- c. No Min Maxis BOUNDRY
- version 1.0.2:
- a. no boundaries
- b. I - Factor tune.
- c. time_skip
-
- Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore.
- Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well.
- See also:
- http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
- http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
- Gyrosetting 2000DPS
- GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s
-
- pidProfile->gtune_lolimP[ROLL] = 10; [0..200] Lower limit of ROLL P during G tune.
- pidProfile->gtune_lolimP[PITCH] = 10; [0..200] Lower limit of PITCH P during G tune.
- pidProfile->gtune_lolimP[YAW] = 10; [0..200] Lower limit of YAW P during G tune.
- pidProfile->gtune_hilimP[ROLL] = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
- pidProfile->gtune_hilimP[PITCH] = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
- pidProfile->gtune_hilimP[YAW] = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
- pidProfile->gtune_pwr = 0; [0..10] Strength of adjustment
- pidProfile->gtune_settle_time = 450; [200..1000] Settle time in ms
- pidProfile->gtune_average_cycles = 16; [8..128] Number of looptime cycles used for gyro average calculation
-*/
-
-static pidProfile_t *pidProfile;
-static int16_t delay_cycles;
-static int16_t time_skip[3];
-static int16_t OldError[3], result_P64[3];
-static int32_t AvgGyro[3];
-static bool floatPID;
-
-void updateDelayCycles(void)
-{
- delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime);
-}
-
-void init_Gtune(pidProfile_t *pidProfileToTune)
-{
- uint8_t i;
-
- pidProfile = pidProfileToTune;
- if (pidProfile->pidController == 2) {
- floatPID = true; // LuxFloat is using float values for PID settings
- } else {
- floatPID = false;
- }
- updateDelayCycles();
- for (i = 0; i < 3; i++) {
- if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
- pidProfile->gtune_hilimP[i] = 0; // Disable YAW tuning for everything below a quadcopter
- }
- if (floatPID) {
- if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) {
- pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f);
- }
- result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P.
- } else {
- if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) {
- pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
- }
- result_P64[i] = (int16_t)pidProfile->P8[i] << 6; // 6 bit extra resolution for P.
- }
- OldError[i] = 0;
- time_skip[i] = delay_cycles;
- }
-}
-
-void calculate_Gtune(uint8_t axis)
-{
- int16_t error, diff_G, threshP;
-
- if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) { // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode
- OldError[axis] = 0;
- time_skip[axis] = delay_cycles; // Some settle time after stick center. default 450ms
- } else {
- if (!time_skip[axis]) AvgGyro[axis] = 0;
- time_skip[axis]++;
- if (time_skip[axis] > 0) {
- if (axis == FD_YAW) {
- AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32); // Chop some jitter and average
- } else {
- AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128); // Chop some jitter and average
- }
- }
- if (time_skip[axis] == pidProfile->gtune_average_cycles) { // Looptime cycles for gyro average calculation. default 16.
- AvgGyro[axis] /= time_skip[axis]; // AvgGyro[axis] has now very clean gyrodata
- time_skip[axis] = 0;
- if (axis == FD_YAW) {
- threshP = 20;
- error = -AvgGyro[axis];
- } else {
- threshP = 10;
- error = AvgGyro[axis];
- }
- if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) { // Don't run when not needed or pointless to do so
- diff_G = ABS(error) - ABS(OldError[axis]);
- if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
- if (diff_G > threshP) {
- if (axis == FD_YAW) {
- result_P64[axis] += 256 + pidProfile->gtune_pwr; // YAW ends up at low limit on float PID, give it some more to work with.
- } else {
- result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side.
- }
- } else {
- if (diff_G < -threshP) {
- if (axis == FD_YAW) {
- result_P64[axis] -= 64 + pidProfile->gtune_pwr;
- } else {
- result_P64[axis] -= 32;
- }
- }
- }
- } else {
- if (ABS(diff_G) > threshP && axis != FD_YAW) {
- result_P64[axis] -= 32; // Don't use antiwobble for YAW
- }
- }
- int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
-
-#ifdef BLACKBOX
- if (feature(FEATURE_BLACKBOX)) {
- flightLogEvent_gtuneCycleResult_t eventData;
-
- eventData.gtuneAxis = axis;
- eventData.gtuneGyroAVG = AvgGyro[axis];
- eventData.gtuneNewP = newP; // for float PID the logged P value is still mutiplyed by 10
- blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
- }
-#endif
-
- if (floatPID) {
- pidProfile->P_f[axis] = (float)newP / 10.0f; // new P value for float PID
- } else {
- pidProfile->P8[axis] = newP; // new P value
- }
- }
- OldError[axis] = error;
- }
- }
-}
-
-#endif
-
diff --git a/src/main/flight/gtune.h b/src/main/flight/gtune.h
deleted file mode 100644
index f580c7c125..0000000000
--- a/src/main/flight/gtune.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-#pragma once
-
-void init_Gtune(pidProfile_t *pidProfileToTune);
-void calculate_Gtune(uint8_t axis);
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 37a6b6c507..59e505fb5b 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -236,7 +236,8 @@ const mixer_t mixers[] = {
static motorMixer_t *customMixers;
-static uint16_t disarmMotorOutput, motorOutputHigh, motorOutputLow, deadbandMotor3dHigh, deadbandMotor3dLow;
+static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
+uint16_t motorOutputHigh, motorOutputLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount()
@@ -533,19 +534,6 @@ void mixTable(pidProfile_t *pidProfile)
}
}
- // Anti Desync feature for ESC's. Limit rapid throttle changes
- if (motorConfig->maxEscThrottleJumpMs) {
- const int16_t maxThrottleStep = constrain(motorConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000);
-
- // Only makes sense when it's within the range
- if (maxThrottleStep < motorOutputRange) {
- static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
-
- motor[i] = constrain(motor[i], motorOutputMin, motorPrevious[i] + maxThrottleStep); // Only limit accelerating situation
- motorPrevious[i] = motor[i];
- }
- }
-
// Disarmed mode
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a534223850..deb9d8d4c9 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -28,28 +28,22 @@
#include "common/maths.h"
#include "common/filter.h"
+#include "fc/fc_main.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation.h"
-#include "flight/gtune.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
-extern float rcInput[3];
-extern float setpointRate[3];
-
uint32_t targetPidLooptime;
static bool pidStabilisationEnabled;
float axisPIDf[3];
-// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
-uint8_t PIDweight[3];
-
#ifdef BLACKBOX
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
@@ -146,8 +140,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
-static float Kp[3], Ki[3], Kd[3], c[3];
-static float rollPitchMaxVelocity, yawMaxVelocity, relaxFactor[3];
+static float Kp[3], Ki[3], Kd[3], c[3], levelGain, horizonGain, horizonTransition, maxVelocity[3], relaxFactor[3];
void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@@ -157,100 +150,96 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
}
- yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
- rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
+ levelGain = pidProfile->P8[PIDLEVEL] / 10.0f;
+ horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f;
+ horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL];
+ maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT;
+ maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
+}
+
+float calcHorizonLevelStrength(void) {
+ float horizonLevelStrength = 0.0f;
+ if (horizonTransition > 0.0f) {
+ const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
+ // Progressively turn off the horizon self level strength as the stick is banged over
+ horizonLevelStrength = constrainf(1 - mostDeflectedPos * horizonTransition, 0, 1);
+ }
+ return horizonLevelStrength;
+}
+
+float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
+ // calculate error angle and limit the angle to the max inclination
+ float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis);
+#ifdef GPS
+ errorAngle += GPS_angle[axis];
+#endif
+ errorAngle = constrainf(errorAngle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
+ errorAngle = (errorAngle - ((attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f));
+ if(FLIGHT_MODE(ANGLE_MODE)) {
+ // ANGLE mode - control is angle based, so control loop is needed
+ currentPidSetpoint = errorAngle * levelGain;
+ } 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
+ const float horizonLevelStrength = calcHorizonLevelStrength();
+ currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
+ }
+ return currentPidSetpoint;
+}
+
+float accelerationLimit(int axis, float currentPidSetpoint) {
+ static float previousSetpoint[3];
+ const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
+
+ if(ABS(currentVelocity) > maxVelocity[axis])
+ currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
+
+ previousSetpoint[axis] = currentPidSetpoint;
+ return currentPidSetpoint;
}
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
-void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
+void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
{
static float previousRateError[2];
static float previousSetpoint[3];
- float horizonLevelStrength = 1;
- if (FLIGHT_MODE(HORIZON_MODE)) {
- // Figure out the raw stick positions
- const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, midrc));
- const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, midrc));
- const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
- // Progressively turn off the horizon self level strength as the stick is banged over
- horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
- if(pidProfile->D8[PIDLEVEL] == 0){
- horizonLevelStrength = 0;
- } else {
- horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);
- }
- }
-
- // Yet Highly experimental and under test and development
- // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)
- static float kiThrottleGain = 1.0f;
- if (pidProfile->itermThrottleGain) {
- const uint16_t maxLoopCount = 20000 / targetPidLooptime;
- const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;
- static int16_t previousThrottle;
- static uint16_t loopIncrement;
-
- if (loopIncrement >= maxLoopCount) {
- kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5
- previousThrottle = rcCommand[THROTTLE];
- loopIncrement = 0;
- } else {
- loopIncrement++;
- }
- }
-
// ----------PID controller----------
- const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
+ const float tpaFactor = getThrottlePIDAttenuation();
+
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
- // Limit abrupt yaw inputs / stops
- const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
- if (maxVelocity) {
- const float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
- if (ABS(currentVelocity) > maxVelocity) {
- setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
- }
- }
+ float currentPidSetpoint = getSetpointRate(axis);
+
+ if(maxVelocity[axis])
+ currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
- // calculate error angle and limit the angle to the max inclination
- float errorAngle = pidProfile->levelSensitivity * rcCommand[axis];
-#ifdef GPS
- errorAngle += GPS_angle[axis];
-#endif
- errorAngle = constrainf(errorAngle, -max_angle_inclination, max_angle_inclination);
- errorAngle = (errorAngle - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
- if (FLIGHT_MODE(ANGLE_MODE)) {
- // ANGLE mode - control is angle based, so control loop is needed
- 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);
- }
+ currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
- const float PVRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
+ const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------
// -----calculate error rate
- const float errorRate = setpointRate[axis] - PVRate; // r - y
+ const float errorRate = currentPidSetpoint - gyroRate; // r - y
// -----calculate P component and add Dynamic Part based on stick input
float PTerm = Kp[axis] * errorRate * tpaFactor;
+ if (axis == FD_YAW) {
+ PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
+ }
// -----calculate I component
// Reduce strong Iterm accumulation during higher stick inputs
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
- const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
- const float itermScaler = setpointRateScaler * kiThrottleGain;
+ const float setpointRateScaler = constrainf(1.0f - (ABS(currentPidSetpoint) / accumulationThreshold), 0.0f, 1.0f);
float ITerm = previousGyroIf[axis];
- ITerm += Ki[axis] * errorRate * dT * itermScaler;;
+ ITerm += Ki[axis] * errorRate * dT * setpointRateScaler;
// limit maximum integrator value to prevent WindUp
ITerm = constrainf(ITerm, -250.0f, 250.0f);
previousGyroIf[axis] = ITerm;
@@ -260,16 +249,17 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
if (axis != FD_YAW) {
float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) {
+ const float rcDeflection = getRcDeflectionAbs(axis);
dynC = c[axis];
- if (setpointRate[axis] > 0) {
- if ((setpointRate[axis] - previousSetpoint[axis]) < previousSetpoint[axis])
- dynC = dynC * sq(rcInput[axis]) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
- } else if (setpointRate[axis] < 0) {
- if ((setpointRate[axis] - previousSetpoint[axis]) > previousSetpoint[axis])
- dynC = dynC * sq(rcInput[axis]) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
+ if (currentPidSetpoint > 0) {
+ if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis])
+ dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
+ } else if (currentPidSetpoint < 0) {
+ if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis])
+ dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
}
}
- const float rD = dynC * setpointRate[axis] - PVRate; // cr - y
+ const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
const float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD;
@@ -281,22 +271,14 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
- } else {
- PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
}
- previousSetpoint[axis] = setpointRate[axis];
+ previousSetpoint[axis] = currentPidSetpoint;
// -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm;
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
-#ifdef GTUNE
- if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
- calculate_Gtune(axis);
- }
-#endif
-
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 27387afbb5..b2e82c2208 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -72,31 +72,23 @@ typedef struct pidProfile_s {
uint8_t dterm_average_count; // Configurable delta count for dterm
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
+ float levelAngleLimit;
// Betaflight PID controller parameters
- uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
+ uint16_t itermThrottleThreshold; // max allowed throttle delta before errorGyroReset in ms
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
- uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
- uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
+ float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
+ float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
float levelSensitivity;
-
-#ifdef GTUNE
- uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
- uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
- uint8_t gtune_pwr; // [0..10] Strength of adjustment
- uint16_t gtune_settle_time; // [200..1000] Settle time in ms
- uint8_t gtune_average_cycles; // [8..128] Number of looptime cycles used for gyro average calculation
-#endif
} pidProfile_t;
typedef struct pidConfig_s {
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
- uint16_t max_angle_inclination;
} pidConfig_t;
union rollAndPitchTrims_u;
-void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, uint16_t midrc);
+void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
extern float axisPIDf[3];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h
index 246cb9925e..4362a3d555 100644
--- a/src/main/io/ledstrip.h
+++ b/src/main/io/ledstrip.h
@@ -152,6 +152,9 @@ hsvColor_t *colors;
modeColorIndexes_t *modeColors;
specialColorIndexes_t specialColors;
+#define LF(name) LED_FUNCTION_ ## name
+#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name)
+#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name)
#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params))
static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); }
diff --git a/src/main/io/motors.h b/src/main/io/motors.h
index ab55497da6..8486d507d2 100644
--- a/src/main/io/motors.h
+++ b/src/main/io/motors.h
@@ -24,7 +24,6 @@ typedef struct motorConfig_s {
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
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 maxEscThrottleJumpMs;
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
uint8_t motorPwmProtocol; // Pwm Protocol
uint8_t useUnsyncedPwm;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 563a6c8c26..41b511c47f 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -360,6 +360,12 @@ static void osdDrawSingleElement(uint8_t item)
break;
}
+ case OSD_POWER:
+ {
+ sprintf(buff, "%dW", amperage * vbat / 1000);
+ break;
+ }
+
default:
return;
}
@@ -402,6 +408,7 @@ void osdDrawElements(void)
osdDrawSingleElement(OSD_ROLL_PIDS);
osdDrawSingleElement(OSD_PITCH_PIDS);
osdDrawSingleElement(OSD_YAW_PIDS);
+ osdDrawSingleElement(OSD_POWER);
#ifdef GPS
#ifdef CMS
@@ -436,6 +443,7 @@ void osdResetConfig(osd_profile_t *osdProfile)
osdProfile->item_pos[OSD_ROLL_PIDS] = OSD_POS(2, 10) | VISIBLE_FLAG;
osdProfile->item_pos[OSD_PITCH_PIDS] = OSD_POS(2, 11) | VISIBLE_FLAG;
osdProfile->item_pos[OSD_YAW_PIDS] = OSD_POS(2, 12) | VISIBLE_FLAG;
+ osdProfile->item_pos[OSD_POWER] = OSD_POS(15, 1);
osdProfile->rssi_alarm = 20;
osdProfile->cap_alarm = 2200;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 881dba9901..6f9dbfec36 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -45,6 +45,7 @@ typedef enum {
OSD_ROLL_PIDS,
OSD_PITCH_PIDS,
OSD_YAW_PIDS,
+ OSD_POWER,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index b9130106df..dd61a8f0e6 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -715,8 +715,6 @@ const clivalue_t valueTable[] = {
#ifdef USE_DSHOT
{ "digital_idle_percent", VAR_FLOAT | MASTER_VALUE, &motorConfig()->digitalIdleOffsetPercent, .config.minmax = { 0, 20} },
#endif
- { "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxEscThrottleJumpMs, .config.minmax = { 0, 1000 } },
-
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &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, &flight3DConfig()->deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently,
{ "3d_neutral", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
@@ -757,18 +755,6 @@ const clivalue_t valueTable[] = {
{ "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &gpsProfile()->nav_slew_rate, .config.minmax = { 0, 100 } },
#endif
-#ifdef GTUNE
- { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } },
- { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 } },
- { "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10, 200 } },
- { "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0, 200 } },
- { "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0, 200 } },
- { "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0, 200 } },
- { "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0, 10 } },
- { "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200, 1000 } },
- { "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 } },
-#endif
-
#ifdef BEEPER
{ "beeper_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperConfig()->isInverted, .config.lookup = { TABLE_OFF_ON } },
{ "beeper_od", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperConfig()->isOpenDrain, .config.lookup = { TABLE_OFF_ON } },
@@ -847,7 +833,6 @@ const clivalue_t valueTable[] = {
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &mixerConfig()->yaw_motor_direction, .config.minmax = { -1, 1 } },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
{ "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
- { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &pidConfig()->max_angle_inclination, .config.minmax = { 100, 900 } },
#ifdef USE_SERVOS
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
@@ -906,11 +891,11 @@ 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 } },
- { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
+ { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .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 } },
+ { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } },
+ { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } },
{ "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
@@ -939,7 +924,8 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
- { "level_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 0.1, 3.0 } },
+ { "level_stick_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10.0f, 200.0f } },
+ { "level_angle_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10.0f, 120.0f } },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &blackboxConfig()->rate_num, .config.minmax = { 1, 32 } },
@@ -996,6 +982,7 @@ const clivalue_t valueTable[] = {
{ "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, UINT16_MAX } },
{ "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, UINT16_MAX } },
{ "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, UINT16_MAX } },
+ { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_POWER], .config.minmax = { 0, UINT16_MAX } },
#endif
#ifdef USE_MAX7456
{ "vcd_video_system", VAR_UINT8 | MASTER_VALUE, &vcdProfile()->video_system, .config.minmax = { 0, 2 } },
diff --git a/src/main/startup/startup_stm32f746xx.s b/src/main/startup/startup_stm32f746xx.s
new file mode 100644
index 0000000000..4b8a796af2
--- /dev/null
+++ b/src/main/startup/startup_stm32f746xx.s
@@ -0,0 +1,593 @@
+/**
+ ******************************************************************************
+ * @file startup_stm32f746xx.s
+ * @author MCD Application Team
+ * @version V1.1.0
+ * @date 22-April-2016
+ * @brief STM32F746xx Devices vector table for GCC toolchain based application.
+ * This module performs:
+ * - Set the initial SP
+ * - Set the initial PC == Reset_Handler,
+ * - Set the vector table entries with the exceptions ISR address
+ * - Branches to main in the C library (which eventually
+ * calls main()).
+ * After Reset the Cortex-M7 processor is in Thread mode,
+ * priority is Privileged, and the Stack is set to Main.
+ ******************************************************************************
+ * @attention
+ *
+ * © COPYRIGHT 2016 STMicroelectronics
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+ .syntax unified
+ .cpu cortex-m7
+ .fpu softvfp
+ .thumb
+
+.global g_pfnVectors
+.global Default_Handler
+
+/* start address for the initialization values of the .data section.
+defined in linker script */
+.word _sidata
+/* start address for the .data section. defined in linker script */
+.word _sdata
+/* end address for the .data section. defined in linker script */
+.word _edata
+/* start address for the .bss section. defined in linker script */
+.word _sbss
+/* end address for the .bss section. defined in linker script */
+.word _ebss
+/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
+
+/**
+ * @brief This is the code that gets called when the processor first
+ * starts execution following a reset event. Only the absolutely
+ * necessary set is performed, after which the application
+ * supplied main() routine is called.
+ * @param None
+ * @retval : None
+*/
+
+ .section .text.Reset_Handler
+ .weak Reset_Handler
+ .type Reset_Handler, %function
+Reset_Handler:
+/* Copy the data segment initializers from flash to SRAM */
+ movs r1, #0
+ b LoopCopyDataInit
+
+CopyDataInit:
+ ldr r3, =_sidata
+ ldr r3, [r3, r1]
+ str r3, [r0, r1]
+ adds r1, r1, #4
+
+LoopCopyDataInit:
+ ldr r0, =_sdata
+ ldr r3, =_edata
+ adds r2, r0, r1
+ cmp r2, r3
+ bcc CopyDataInit
+ ldr r2, =_sbss
+ b LoopFillZerobss
+/* Zero fill the bss segment. */
+FillZerobss:
+ movs r3, #0
+ str r3, [r2], #4
+
+LoopFillZerobss:
+ ldr r3, = _ebss
+ cmp r2, r3
+ bcc FillZerobss
+
+/*FPU settings*/
+ ldr r0, =0xE000ED88 /* Enable CP10,CP11 */
+ ldr r1,[r0]
+ orr r1,r1,#(0xF << 20)
+ str r1,[r0]
+
+/* Call the clock system initialization function.*/
+ bl SystemInit
+/* Call the application's entry point.*/
+ bl main
+ bx lr
+
+LoopForever:
+ b LoopForever
+
+.size Reset_Handler, .-Reset_Handler
+
+/**
+ * @brief This is the code that gets called when the processor receives an
+ * unexpected interrupt. This simply enters an infinite loop, preserving
+ * the system state for examination by a debugger.
+ * @param None
+ * @retval None
+*/
+ .section .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+ b Infinite_Loop
+ .size Default_Handler, .-Default_Handler
+/******************************************************************************
+*
+* The minimal vector table for a Cortex M7. Note that the proper constructs
+* must be placed on this to ensure that it ends up at physical address
+* 0x0000.0000.
+*
+*******************************************************************************/
+ .section .isr_vector,"a",%progbits
+ .type g_pfnVectors, %object
+ .size g_pfnVectors, .-g_pfnVectors
+
+
+g_pfnVectors:
+ .word _estack
+ .word Reset_Handler
+ .word NMI_Handler
+ .word HardFault_Handler
+ .word MemManage_Handler
+ .word BusFault_Handler
+ .word UsageFault_Handler
+ .word 0
+ .word 0
+ .word 0
+ .word 0
+ .word SVC_Handler
+ .word DebugMon_Handler
+ .word 0
+ .word PendSV_Handler
+ .word SysTick_Handler
+
+ /* External Interrupts */
+ .word WWDG_IRQHandler /* Window WatchDog */
+ .word PVD_IRQHandler /* PVD through EXTI Line detection */
+ .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */
+ .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */
+ .word FLASH_IRQHandler /* FLASH */
+ .word RCC_IRQHandler /* RCC */
+ .word EXTI0_IRQHandler /* EXTI Line0 */
+ .word EXTI1_IRQHandler /* EXTI Line1 */
+ .word EXTI2_IRQHandler /* EXTI Line2 */
+ .word EXTI3_IRQHandler /* EXTI Line3 */
+ .word EXTI4_IRQHandler /* EXTI Line4 */
+ .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */
+ .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */
+ .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */
+ .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */
+ .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */
+ .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */
+ .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */
+ .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */
+ .word CAN1_TX_IRQHandler /* CAN1 TX */
+ .word CAN1_RX0_IRQHandler /* CAN1 RX0 */
+ .word CAN1_RX1_IRQHandler /* CAN1 RX1 */
+ .word CAN1_SCE_IRQHandler /* CAN1 SCE */
+ .word EXTI9_5_IRQHandler /* External Line[9:5]s */
+ .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */
+ .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */
+ .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */
+ .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */
+ .word TIM2_IRQHandler /* TIM2 */
+ .word TIM3_IRQHandler /* TIM3 */
+ .word TIM4_IRQHandler /* TIM4 */
+ .word I2C1_EV_IRQHandler /* I2C1 Event */
+ .word I2C1_ER_IRQHandler /* I2C1 Error */
+ .word I2C2_EV_IRQHandler /* I2C2 Event */
+ .word I2C2_ER_IRQHandler /* I2C2 Error */
+ .word SPI1_IRQHandler /* SPI1 */
+ .word SPI2_IRQHandler /* SPI2 */
+ .word USART1_IRQHandler /* USART1 */
+ .word USART2_IRQHandler /* USART2 */
+ .word USART3_IRQHandler /* USART3 */
+ .word EXTI15_10_IRQHandler /* External Line[15:10]s */
+ .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */
+ .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */
+ .word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */
+ .word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */
+ .word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */
+ .word TIM8_CC_IRQHandler /* TIM8 Capture Compare */
+ .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */
+ .word FMC_IRQHandler /* FMC */
+ .word SDMMC1_IRQHandler /* SDMMC1 */
+ .word TIM5_IRQHandler /* TIM5 */
+ .word SPI3_IRQHandler /* SPI3 */
+ .word UART4_IRQHandler /* UART4 */
+ .word UART5_IRQHandler /* UART5 */
+ .word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */
+ .word TIM7_IRQHandler /* TIM7 */
+ .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */
+ .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */
+ .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */
+ .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */
+ .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */
+ .word ETH_IRQHandler /* Ethernet */
+ .word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */
+ .word CAN2_TX_IRQHandler /* CAN2 TX */
+ .word CAN2_RX0_IRQHandler /* CAN2 RX0 */
+ .word CAN2_RX1_IRQHandler /* CAN2 RX1 */
+ .word CAN2_SCE_IRQHandler /* CAN2 SCE */
+ .word OTG_FS_IRQHandler /* USB OTG FS */
+ .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */
+ .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */
+ .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */
+ .word USART6_IRQHandler /* USART6 */
+ .word I2C3_EV_IRQHandler /* I2C3 event */
+ .word I2C3_ER_IRQHandler /* I2C3 error */
+ .word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */
+ .word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */
+ .word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */
+ .word OTG_HS_IRQHandler /* USB OTG HS */
+ .word DCMI_IRQHandler /* DCMI */
+ .word 0 /* Reserved */
+ .word RNG_IRQHandler /* Rng */
+ .word FPU_IRQHandler /* FPU */
+ .word UART7_IRQHandler /* UART7 */
+ .word UART8_IRQHandler /* UART8 */
+ .word SPI4_IRQHandler /* SPI4 */
+ .word SPI5_IRQHandler /* SPI5 */
+ .word SPI6_IRQHandler /* SPI6 */
+ .word SAI1_IRQHandler /* SAI1 */
+ .word LTDC_IRQHandler /* LTDC */
+ .word LTDC_ER_IRQHandler /* LTDC_ER */
+ .word DMA2D_IRQHandler /* DMA2D */
+ .word SAI2_IRQHandler /* SAI2 */
+ .word QUADSPI_IRQHandler /* QUADSPI */
+ .word LPTIM1_IRQHandler /* LPTIM1 */
+ .word CEC_IRQHandler /* HDMI_CEC */
+ .word I2C4_EV_IRQHandler /* I2C4 Event */
+ .word I2C4_ER_IRQHandler /* I2C4 Error */
+ .word SPDIF_RX_IRQHandler /* SPDIF_RX */
+
+/*******************************************************************************
+*
+* Provide weak aliases for each Exception handler to the Default_Handler.
+* As they are weak aliases, any function with the same name will override
+* this definition.
+*
+*******************************************************************************/
+ .weak NMI_Handler
+ .thumb_set NMI_Handler,Default_Handler
+
+ .weak HardFault_Handler
+ .thumb_set HardFault_Handler,Default_Handler
+
+ .weak MemManage_Handler
+ .thumb_set MemManage_Handler,Default_Handler
+
+ .weak BusFault_Handler
+ .thumb_set BusFault_Handler,Default_Handler
+
+ .weak UsageFault_Handler
+ .thumb_set UsageFault_Handler,Default_Handler
+
+ .weak SVC_Handler
+ .thumb_set SVC_Handler,Default_Handler
+
+ .weak DebugMon_Handler
+ .thumb_set DebugMon_Handler,Default_Handler
+
+ .weak PendSV_Handler
+ .thumb_set PendSV_Handler,Default_Handler
+
+ .weak SysTick_Handler
+ .thumb_set SysTick_Handler,Default_Handler
+
+ .weak WWDG_IRQHandler
+ .thumb_set WWDG_IRQHandler,Default_Handler
+
+ .weak PVD_IRQHandler
+ .thumb_set PVD_IRQHandler,Default_Handler
+
+ .weak TAMP_STAMP_IRQHandler
+ .thumb_set TAMP_STAMP_IRQHandler,Default_Handler
+
+ .weak RTC_WKUP_IRQHandler
+ .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+
+ .weak FLASH_IRQHandler
+ .thumb_set FLASH_IRQHandler,Default_Handler
+
+ .weak RCC_IRQHandler
+ .thumb_set RCC_IRQHandler,Default_Handler
+
+ .weak EXTI0_IRQHandler
+ .thumb_set EXTI0_IRQHandler,Default_Handler
+
+ .weak EXTI1_IRQHandler
+ .thumb_set EXTI1_IRQHandler,Default_Handler
+
+ .weak EXTI2_IRQHandler
+ .thumb_set EXTI2_IRQHandler,Default_Handler
+
+ .weak EXTI3_IRQHandler
+ .thumb_set EXTI3_IRQHandler,Default_Handler
+
+ .weak EXTI4_IRQHandler
+ .thumb_set EXTI4_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream0_IRQHandler
+ .thumb_set DMA1_Stream0_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream1_IRQHandler
+ .thumb_set DMA1_Stream1_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream2_IRQHandler
+ .thumb_set DMA1_Stream2_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream3_IRQHandler
+ .thumb_set DMA1_Stream3_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream4_IRQHandler
+ .thumb_set DMA1_Stream4_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream5_IRQHandler
+ .thumb_set DMA1_Stream5_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream6_IRQHandler
+ .thumb_set DMA1_Stream6_IRQHandler,Default_Handler
+
+ .weak ADC_IRQHandler
+ .thumb_set ADC_IRQHandler,Default_Handler
+
+ .weak CAN1_TX_IRQHandler
+ .thumb_set CAN1_TX_IRQHandler,Default_Handler
+
+ .weak CAN1_RX0_IRQHandler
+ .thumb_set CAN1_RX0_IRQHandler,Default_Handler
+
+ .weak CAN1_RX1_IRQHandler
+ .thumb_set CAN1_RX1_IRQHandler,Default_Handler
+
+ .weak CAN1_SCE_IRQHandler
+ .thumb_set CAN1_SCE_IRQHandler,Default_Handler
+
+ .weak EXTI9_5_IRQHandler
+ .thumb_set EXTI9_5_IRQHandler,Default_Handler
+
+ .weak TIM1_BRK_TIM9_IRQHandler
+ .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler
+
+ .weak TIM1_UP_TIM10_IRQHandler
+ .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler
+
+ .weak TIM1_TRG_COM_TIM11_IRQHandler
+ .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler
+
+ .weak TIM1_CC_IRQHandler
+ .thumb_set TIM1_CC_IRQHandler,Default_Handler
+
+ .weak TIM2_IRQHandler
+ .thumb_set TIM2_IRQHandler,Default_Handler
+
+ .weak TIM3_IRQHandler
+ .thumb_set TIM3_IRQHandler,Default_Handler
+
+ .weak TIM4_IRQHandler
+ .thumb_set TIM4_IRQHandler,Default_Handler
+
+ .weak I2C1_EV_IRQHandler
+ .thumb_set I2C1_EV_IRQHandler,Default_Handler
+
+ .weak I2C1_ER_IRQHandler
+ .thumb_set I2C1_ER_IRQHandler,Default_Handler
+
+ .weak I2C2_EV_IRQHandler
+ .thumb_set I2C2_EV_IRQHandler,Default_Handler
+
+ .weak I2C2_ER_IRQHandler
+ .thumb_set I2C2_ER_IRQHandler,Default_Handler
+
+ .weak SPI1_IRQHandler
+ .thumb_set SPI1_IRQHandler,Default_Handler
+
+ .weak SPI2_IRQHandler
+ .thumb_set SPI2_IRQHandler,Default_Handler
+
+ .weak USART1_IRQHandler
+ .thumb_set USART1_IRQHandler,Default_Handler
+
+ .weak USART2_IRQHandler
+ .thumb_set USART2_IRQHandler,Default_Handler
+
+ .weak USART3_IRQHandler
+ .thumb_set USART3_IRQHandler,Default_Handler
+
+ .weak EXTI15_10_IRQHandler
+ .thumb_set EXTI15_10_IRQHandler,Default_Handler
+
+ .weak RTC_Alarm_IRQHandler
+ .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+
+ .weak OTG_FS_WKUP_IRQHandler
+ .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
+
+ .weak TIM8_BRK_TIM12_IRQHandler
+ .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler
+
+ .weak TIM8_UP_TIM13_IRQHandler
+ .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler
+
+ .weak TIM8_TRG_COM_TIM14_IRQHandler
+ .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler
+
+ .weak TIM8_CC_IRQHandler
+ .thumb_set TIM8_CC_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream7_IRQHandler
+ .thumb_set DMA1_Stream7_IRQHandler,Default_Handler
+
+ .weak FMC_IRQHandler
+ .thumb_set FMC_IRQHandler,Default_Handler
+
+ .weak SDMMC1_IRQHandler
+ .thumb_set SDMMC1_IRQHandler,Default_Handler
+
+ .weak TIM5_IRQHandler
+ .thumb_set TIM5_IRQHandler,Default_Handler
+
+ .weak SPI3_IRQHandler
+ .thumb_set SPI3_IRQHandler,Default_Handler
+
+ .weak UART4_IRQHandler
+ .thumb_set UART4_IRQHandler,Default_Handler
+
+ .weak UART5_IRQHandler
+ .thumb_set UART5_IRQHandler,Default_Handler
+
+ .weak TIM6_DAC_IRQHandler
+ .thumb_set TIM6_DAC_IRQHandler,Default_Handler
+
+ .weak TIM7_IRQHandler
+ .thumb_set TIM7_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream0_IRQHandler
+ .thumb_set DMA2_Stream0_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream1_IRQHandler
+ .thumb_set DMA2_Stream1_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream2_IRQHandler
+ .thumb_set DMA2_Stream2_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream3_IRQHandler
+ .thumb_set DMA2_Stream3_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream4_IRQHandler
+ .thumb_set DMA2_Stream4_IRQHandler,Default_Handler
+
+ .weak ETH_IRQHandler
+ .thumb_set ETH_IRQHandler,Default_Handler
+
+ .weak ETH_WKUP_IRQHandler
+ .thumb_set ETH_WKUP_IRQHandler,Default_Handler
+
+ .weak CAN2_TX_IRQHandler
+ .thumb_set CAN2_TX_IRQHandler,Default_Handler
+
+ .weak CAN2_RX0_IRQHandler
+ .thumb_set CAN2_RX0_IRQHandler,Default_Handler
+
+ .weak CAN2_RX1_IRQHandler
+ .thumb_set CAN2_RX1_IRQHandler,Default_Handler
+
+ .weak CAN2_SCE_IRQHandler
+ .thumb_set CAN2_SCE_IRQHandler,Default_Handler
+
+ .weak OTG_FS_IRQHandler
+ .thumb_set OTG_FS_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream5_IRQHandler
+ .thumb_set DMA2_Stream5_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream6_IRQHandler
+ .thumb_set DMA2_Stream6_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream7_IRQHandler
+ .thumb_set DMA2_Stream7_IRQHandler,Default_Handler
+
+ .weak USART6_IRQHandler
+ .thumb_set USART6_IRQHandler,Default_Handler
+
+ .weak I2C3_EV_IRQHandler
+ .thumb_set I2C3_EV_IRQHandler,Default_Handler
+
+ .weak I2C3_ER_IRQHandler
+ .thumb_set I2C3_ER_IRQHandler,Default_Handler
+
+ .weak OTG_HS_EP1_OUT_IRQHandler
+ .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler
+
+ .weak OTG_HS_EP1_IN_IRQHandler
+ .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler
+
+ .weak OTG_HS_WKUP_IRQHandler
+ .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler
+
+ .weak OTG_HS_IRQHandler
+ .thumb_set OTG_HS_IRQHandler,Default_Handler
+
+ .weak DCMI_IRQHandler
+ .thumb_set DCMI_IRQHandler,Default_Handler
+
+ .weak RNG_IRQHandler
+ .thumb_set RNG_IRQHandler,Default_Handler
+
+ .weak FPU_IRQHandler
+ .thumb_set FPU_IRQHandler,Default_Handler
+
+ .weak UART7_IRQHandler
+ .thumb_set UART7_IRQHandler,Default_Handler
+
+ .weak UART8_IRQHandler
+ .thumb_set UART8_IRQHandler,Default_Handler
+
+ .weak SPI4_IRQHandler
+ .thumb_set SPI4_IRQHandler,Default_Handler
+
+ .weak SPI5_IRQHandler
+ .thumb_set SPI5_IRQHandler,Default_Handler
+
+ .weak SPI6_IRQHandler
+ .thumb_set SPI6_IRQHandler,Default_Handler
+
+ .weak SAI1_IRQHandler
+ .thumb_set SAI1_IRQHandler,Default_Handler
+
+ .weak LTDC_IRQHandler
+ .thumb_set LTDC_IRQHandler,Default_Handler
+
+ .weak LTDC_ER_IRQHandler
+ .thumb_set LTDC_ER_IRQHandler,Default_Handler
+
+ .weak DMA2D_IRQHandler
+ .thumb_set DMA2D_IRQHandler,Default_Handler
+
+ .weak SAI2_IRQHandler
+ .thumb_set SAI2_IRQHandler,Default_Handler
+
+ .weak QUADSPI_IRQHandler
+ .thumb_set QUADSPI_IRQHandler,Default_Handler
+
+ .weak LPTIM1_IRQHandler
+ .thumb_set LPTIM1_IRQHandler,Default_Handler
+
+ .weak CEC_IRQHandler
+ .thumb_set CEC_IRQHandler,Default_Handler
+
+ .weak I2C4_EV_IRQHandler
+ .thumb_set I2C4_EV_IRQHandler,Default_Handler
+
+ .weak I2C4_ER_IRQHandler
+ .thumb_set I2C4_ER_IRQHandler,Default_Handler
+
+ .weak SPDIF_RX_IRQHandler
+ .thumb_set SPDIF_RX_IRQHandler,Default_Handler
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+
diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c
deleted file mode 100644
index 1a9a98d65a..0000000000
--- a/src/main/target/AIORACERF3/target.c
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-#include
-
-#include
-#include "drivers/io.h"
-
-#include "drivers/timer.h"
-#include "drivers/dma.h"
-
-const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
- // PPM / UART2 RX
- { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM
- { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1
- { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2
- { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3
- { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM4
- { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5
- { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6
- { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7
- { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8
- { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_TX (AF7)
- { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_RX (AF7)
- { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, //LED_STRIP
-};
diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h
index 17a1d389a2..ac0e534284 100644
--- a/src/main/target/BLUEJAYF4/target.h
+++ b/src/main/target/BLUEJAYF4/target.h
@@ -146,6 +146,7 @@
#define USE_ADC
#define VBAT_ADC_PIN PC3
+#define CURRENT_METER_ADC_PIN PC2
#define USE_ESC_SENSOR
#define LED_STRIP
@@ -162,10 +163,10 @@
#define SPEKTRUM_BIND
#define BIND_PIN PB11
-#define TARGET_IO_PORTA 0xffff
-#define TARGET_IO_PORTB 0xffff
-#define TARGET_IO_PORTC 0xffff
-#define TARGET_IO_PORTD (BIT(2))
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
-#define USABLE_TIMER_CHANNEL_COUNT 7
-#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9))
+#define USABLE_TIMER_CHANNEL_COUNT 7
+#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9) )
diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c
index 6fb4b830cf..ed8e8ab552 100644
--- a/src/main/target/COLIBRI_RACE/config.c
+++ b/src/main/target/COLIBRI_RACE/config.c
@@ -17,6 +17,7 @@
#include
#include
+#include
#include
@@ -25,12 +26,74 @@
#include "sensors/battery.h"
#include "config/config_master.h"
+#include "config/feature.h"
+#include "io/ledstrip.h"
+void targetApplyDefaultLedStripConfig(ledConfig_t *ledConfigs)
+{
+ const ledConfig_t defaultLedStripConfig[] = {
+ DEFINE_LED( 0, 0, 6, LD(WEST), LF(COLOR), LO(WARNING), 0 ),
+ DEFINE_LED( 0, 1, 6, LD(WEST), LF(COLOR), LO(WARNING), 0 ),
+ DEFINE_LED( 0, 8, 6, LD(WEST), LF(COLOR), LO(WARNING), 0 ),
+ DEFINE_LED( 7, 15, 6, 0, LF(COLOR), 0, 0 ),
+ DEFINE_LED( 8, 15, 6, 0, LF(COLOR), 0, 0 ),
+ DEFINE_LED( 7, 14, 6, 0, LF(COLOR), 0, 0 ),
+ DEFINE_LED( 8, 14, 6, 0, LF(COLOR), 0, 0 ),
+ DEFINE_LED( 15, 8, 6, LD(EAST), LF(COLOR), LO(WARNING), 0 ),
+ DEFINE_LED( 15, 1, 6, LD(EAST), LF(COLOR), LO(WARNING), 0 ),
+ DEFINE_LED( 15, 0, 6, LD(EAST), LF(COLOR), LO(WARNING), 0 ),
+ };
+ memcpy(ledConfigs, &defaultLedStripConfig, MIN(LED_MAX_STRIP_LENGTH, sizeof(defaultLedStripConfig)));
+}
// alternative defaults settings for COLIBRI RACE targets
void targetConfiguration(master_t *config)
{
+ config->motorConfig.minthrottle = 1025;
config->motorConfig.maxthrottle = 1980;
+ config->motorConfig.mincommand = 1000;
+ config->servoConfig.servoCenterPulse = 1500;
+
config->batteryConfig.vbatmaxcellvoltage = 45;
config->batteryConfig.vbatmincellvoltage = 30;
+ config->batteryConfig.vbatwarningcellvoltage = 35;
+
+ config->flight3DConfig.deadband3d_low = 1406;
+ config->flight3DConfig.deadband3d_high = 1514;
+ config->flight3DConfig.neutral3d = 1460;
+ config->flight3DConfig.deadband3d_throttle = 0;
+
+ config->failsafeConfig.failsafe_procedure = 1;
+ config->failsafeConfig.failsafe_throttle_low_delay = 10;
+
+ config->gyroConfig.gyro_sync_denom = 1;
+ config->pidConfig.pid_process_denom = 3;
+ config->blackboxConfig.rate_num = 1;
+ config->blackboxConfig.rate_denom = 1;
+
+ config->rcControlsConfig.deadband = 5;
+ config->rcControlsConfig.yaw_deadband = 5;
+
+ config->failsafeConfig.failsafe_delay = 10;
+
+ config->telemetryConfig.telemetry_inversion = 1;
+
+ config->profile[0].pidProfile.vbatPidCompensation = 1;
+
+ config->profile[0].pidProfile.P8[ROLL] = 46; // new PID with preliminary defaults test carefully
+ config->profile[0].pidProfile.I8[ROLL] = 48;
+ config->profile[0].pidProfile.D8[ROLL] = 23;
+ config->profile[0].pidProfile.P8[PITCH] = 89;
+ config->profile[0].pidProfile.I8[PITCH] = 59;
+ config->profile[0].pidProfile.D8[PITCH] = 25;
+ config->profile[0].pidProfile.P8[YAW] = 129;
+ config->profile[0].pidProfile.I8[YAW] = 50;
+ config->profile[0].pidProfile.D8[YAW] = 20;
+
+ config->profile[0].controlRateProfile[0].rates[FD_ROLL] = 86;
+ config->profile[0].controlRateProfile[0].rates[FD_PITCH] = 86;
+ config->profile[0].controlRateProfile[0].rates[FD_YAW] = 80;
+
+ targetApplyDefaultLedStripConfig(config->ledStripConfig.ledConfigs);
+
}
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index 1cf442dca4..da346b0952 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/src/main/target/COLIBRI_RACE/i2c_bst.c
@@ -29,7 +29,7 @@
#include "drivers/rx_pwm.h"
#include "fc/config.h"
-#include "fc/mw.h"
+#include "fc/fc_main.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
@@ -704,6 +704,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(currentControlRateProfile->thrExpo8);
bstWrite16(currentControlRateProfile->tpa_breakpoint);
bstWrite8(currentControlRateProfile->rcYawExpo8);
+ bstWrite8(currentControlRateProfile->rcYawRate8);
break;
case BST_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) {
@@ -1108,6 +1109,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
if (bstReadDataSize() >= 11) {
currentControlRateProfile->rcYawExpo8 = bstRead8();
}
+ if (bstReadDataSize() >= 12) {
+ currentControlRateProfile->rcYawRate8 = bstRead8();
+ }
} else {
ret = BST_FAILED;
}
@@ -1256,6 +1260,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
case BST_SET_FEATURE:
featureClearAll();
featureSet(bstRead32()); // features bitmap
+#ifdef SERIALRX_UART
+ if (featureConfigured(FEATURE_RX_SERIAL)) {
+ serialConfig()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL;
+ } else {
+ serialConfig()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_NONE;
+ }
+#endif
break;
case BST_SET_BOARD_ALIGNMENT:
boardAlignment()->rollDegrees = bstRead16();
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 4f7a6a484f..e2bec8d54a 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -36,8 +36,6 @@
#define MPU_INT_EXTI PA5
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
-
#define USE_SPI
#define USE_SPI_DEVICE_1
@@ -116,10 +114,10 @@
#define LED_STRIP
-#define DEFAULT_FEATURES FEATURE_VBAT
-#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
+#define DEFAULT_FEATURES (FEATURE_RX_PPM | FEATURE_VBAT | FEATURE_FAILSAFE | FEATURE_AIRMODE | FEATURE_LED_STRIP)
+#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define SERIALRX_PROVIDER SERIALRX_SBUS
-#define SERIALRX_UART SERIAL_PORT_USART2
+#define SERIALRX_UART SERIAL_PORT_USART3
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
diff --git a/src/main/target/ELLE0/target.c b/src/main/target/ELLE0/target.c
new file mode 100644
index 0000000000..b350ddf36b
--- /dev/null
+++ b/src/main/target/ELLE0/target.c
@@ -0,0 +1,51 @@
+/*
+ * 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/timer.h"
+#include "drivers/timer_def.h"
+#include "drivers/dma.h"
+
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+ DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN DMA1_ST1 (shared with RX1)
+ DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // SERVO1 DMA2_ST2
+ DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // SERVO2 DMA2_ST3
+ DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1 ), // SERVO3 DMA2_ST4
+ DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // SERVO4 DMA2_ST7
+ DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // SERVO5 DMA1_ST2
+ DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // SERVO6 DMA1_ST4
+ DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // SERVO7 DMA1_ST7
+ DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // SERVO8 DMA1_ST3
+};
+
+// Telemetry
+//UART1 RX: DMA2_ST5
+//UART1 TX: DMA2_ST7
+
+// RX1
+//UART2 RX: DMA1_ST5
+//UART2 TX: DMA1_ST6
+
+// I2C
+//UART3 RX: DMA1_ST1
+//UART3 TX: DMA1_ST3
+
+
diff --git a/src/main/target/ELLE0/target.h b/src/main/target/ELLE0/target.h
new file mode 100644
index 0000000000..cbe277a93f
--- /dev/null
+++ b/src/main/target/ELLE0/target.h
@@ -0,0 +1,105 @@
+/*
+ * 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 "ELL0"
+
+#define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8)
+#define TARGET_XTAL_MHZ 25
+
+#define USBD_PRODUCT_STRING "Elle0"
+
+#define LED0 PA8
+#define LED1 PB4
+#define LED2 PC2
+
+// MPU9250 interrupt
+#define USE_EXTI
+#define MPU_INT_EXTI PB5
+//#define DEBUG_MPU_DATA_READY_INTERRUPT
+#define USE_MPU_DATA_READY_SIGNAL
+#define ENSURE_MPU_DATA_READY_IS_LOW
+
+#define MPU6500_CS_PIN PB12
+#define MPU6500_SPI_INSTANCE SPI2
+
+// Using MPU6050 for the moment.
+#define GYRO
+#define USE_GYRO_SPI_MPU6500
+#define GYRO_MPU6500_ALIGN CW270_DEG
+
+#define ACC
+#define USE_ACC_SPI_MPU6500
+#define ACC_MPU6500_ALIGN CW270_DEG
+
+//#define BARO
+//#define USE_BARO_MS5611
+
+#define MAG
+#define USE_MAG_AK8963
+#define MAG_AK8963_ALIGN CW0_DEG_FLIP
+
+#define USE_VCP
+
+/* Telemetry (Overlaps with DMA from motors) */
+//#define USE_UART1
+//#define UART1_RX_PIN PA10
+//#define UART1_TX_PIN PA9
+//#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
+
+/* RX1 */
+#define USE_UART2
+#define UART2_RX_PIN PA3
+#define UART2_TX_PIN PA2
+
+/* I2C */
+#define USE_UART3
+#define UART3_RX_PIN PB11
+#define UART3_TX_PIN PB10
+
+/* RX2 */
+//#define USE_UART5
+//#define UART5_RX_PIN PD2
+//#define UART5_TX_PIN PC12
+
+#define SERIAL_PORT_COUNT 3
+
+#define USE_SPI
+
+#define USE_SPI_DEVICE_2 //MPU9250
+#define SPI2_NSS_PIN PB12
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB14
+#define SPI2_MOSI_PIN PB15
+
+#define USE_ADC
+#define VBAT_ADC_PIN PC4
+#define CURRENT_METER_ADC_PIN PC5
+
+#define DEFAULT_FEATURES (FEATURE_VBAT)
+#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048
+#define SERIALRX_UART SERIAL_PORT_USART2
+#define RX_CHANNELS_TAER
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
+
+#define USABLE_TIMER_CHANNEL_COUNT 9
+#define USED_TIMERS (TIM_N(2) | TIM_N(4) | TIM_N(5) | TIM_N(8))
diff --git a/src/main/target/ELLE0/target.mk b/src/main/target/ELLE0/target.mk
new file mode 100644
index 0000000000..b044290a7b
--- /dev/null
+++ b/src/main/target/ELLE0/target.mk
@@ -0,0 +1,9 @@
+F405_TARGETS += $(TARGET)
+FEATURES += VCP
+HSE_VALUE = 25000000
+
+TARGET_SRC = \
+ drivers/accgyro_mpu.c \
+ drivers/accgyro_mpu6500.c \
+ drivers/accgyro_spi_mpu6500.c \
+ drivers/compass_ak8963.c
diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h
index c366b5f23d..390f8f93ab 100644
--- a/src/main/target/F4BY/target.h
+++ b/src/main/target/F4BY/target.h
@@ -103,6 +103,9 @@
#define SERIAL_PORT_COUNT 6 //VCP, UART1, UART2, UART3, UART4, UART5, UART6
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 8 // PWM 9
+
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h
index eb83dec47d..a343c48b39 100644
--- a/src/main/target/FURYF4/target.h
+++ b/src/main/target/FURYF4/target.h
@@ -133,6 +133,9 @@
#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define USE_SPI
#define USE_SPI_DEVICE_1
diff --git a/src/main/target/FishDroneF4/target.h b/src/main/target/FishDroneF4/target.h
index 4d8c5f3a41..da9c69ec06 100644
--- a/src/main/target/FishDroneF4/target.h
+++ b/src/main/target/FishDroneF4/target.h
@@ -69,6 +69,9 @@
#define SERIAL_PORT_COUNT 4 // VCP, USART1, USART3, USART6
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
// *************** OSD *****************************
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PB12
diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h
index f6649fa5e5..93724bbacf 100644
--- a/src/main/target/IMPULSERCF3/target.h
+++ b/src/main/target/IMPULSERCF3/target.h
@@ -55,6 +55,9 @@
#define USE_UART3
#define SERIAL_PORT_COUNT 4
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h
index f3f2ee8119..116ae4e5de 100644
--- a/src/main/target/IRCFUSIONF3/target.h
+++ b/src/main/target/IRCFUSIONF3/target.h
@@ -47,6 +47,9 @@
#define USE_UART3
#define SERIAL_PORT_COUNT 3
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h
index 25cc506f5b..c631a54172 100644
--- a/src/main/target/ISHAPEDF3/target.h
+++ b/src/main/target/ISHAPEDF3/target.h
@@ -61,6 +61,9 @@
#define USE_SOFTSERIAL2
#define SERIAL_PORT_COUNT 5
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h
index a82ba41e11..421d32539d 100644
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@@ -93,6 +93,9 @@
#define SERIAL_PORT_COUNT 4
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define USE_SPI
#define USE_SPI_DEVICE_1 //ICM20689
diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h
index 52b772e268..ec19edcc1f 100644
--- a/src/main/target/LUX_RACE/target.h
+++ b/src/main/target/LUX_RACE/target.h
@@ -147,11 +147,16 @@
#define LED_STRIP
-#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
+#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_SBUS
+#define SERIALRX_UART SERIAL_PORT_USART2
+#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1
#ifdef LUXV2_RACE
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
-#define DEFAULT_FEATURES FEATURE_BLACKBOX
+#define DEFAULT_FEATURES ( FEATURE_BLACKBOX | FEATURE_TELEMETRY )
+#else
+#define DEFAULT_FEATURES FEATURE_TELEMETRY
#endif
#define SPEKTRUM_BIND
@@ -160,6 +165,9 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
// IO - assuming 303 in 64pin package, TODO
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h
index b413253f46..62a4de9952 100644
--- a/src/main/target/MOTOLAB/target.h
+++ b/src/main/target/MOTOLAB/target.h
@@ -61,6 +61,9 @@
#define USE_UART3
#define SERIAL_PORT_COUNT 4
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c
index e9e1126747..b15a062bc6 100755
--- a/src/main/target/NAZE/config.c
+++ b/src/main/target/NAZE/config.c
@@ -70,7 +70,7 @@ void targetConfiguration(master_t *config)
config->profile[profileId].pidProfile.I8[YAW] = 45;
config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50;
config->profile[profileId].pidProfile.D8[PIDLEVEL] = 50;
- config->profile[profileId].pidProfile.levelSensitivity = 1.0f;
+ config->profile[profileId].pidProfile.levelSensitivity = 50.0f;
for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) {
config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 100;
diff --git a/src/main/target/NERO7/NERO7.md b/src/main/target/NERO/NERO.md
similarity index 52%
rename from src/main/target/NERO7/NERO7.md
rename to src/main/target/NERO/NERO.md
index fef9761fb3..3016d558df 100644
--- a/src/main/target/NERO7/NERO7.md
+++ b/src/main/target/NERO/NERO.md
@@ -1,5 +1,5 @@
-# NERO7
+# NERO
-Placeholder for NERO7 (an STM32F722RE target) - in both 30.5x30.5 and 20x20 (mini) formats.
+Placeholder for NERO (an STM32F722RE target) - in both 30.5x30.5 and 20x20 (mini) formats.
Samples are being made now, with production run expected once STM32 release the 722RE in production quantities.
\ No newline at end of file
diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c
index 38fba42c97..9d1227d454 100644
--- a/src/main/target/OMNIBUSF4/target.c
+++ b/src/main/target/OMNIBUSF4/target.c
@@ -20,26 +20,33 @@
#include
#include "drivers/io.h"
-#include "drivers/timer.h"
#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
#ifdef OMNIBUSF4SD
- { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM4, NULL, 0, 0}, // PPM
- { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM4, NULL, 0, 0}, // S2_IN
+ DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM
+ DEF_TIM(TIM4, CH4, PB9, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S2_IN
#else
- { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM
- { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN
+ DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM
+ DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S2_IN
#endif
- { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN
- { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN
- { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN
- { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN
+ DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S3_IN
+ DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S4_IN
+ DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S5_IN
+ DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S6_IN
- { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT
- { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT
- { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT
- { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT
- { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - GPIO_PartialRemap_TIM3
- { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT
+ DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S1_OUT D1_ST7
+ DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S2_OUT D1_ST2
+ DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 1), // S3_OUT D1_ST6
+ DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1
+
+#ifdef OMNIBUSF4SD
+ DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT
+ DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S6_OUT
+#else
+ DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT
+ DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT
+#endif
};
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index dbe2ae9c61..94cd16bae9 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -55,10 +55,10 @@
#define USE_BARO_MS5611
#define USE_BARO_BMP085
-//#define MAG
-//#define USE_MAG_AK8975
-//#define USE_MAG_HMC5883
-//#define MAG_HMC5883_ALIGN CW270_DEG
+#define MAG
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+#define MAG_HMC5883_ALIGN CW270_DEG
#define USE_MAG_DATA_READY_SIGNAL
#define ENSURE_MAG_DATA_READY_IS_HIGH
diff --git a/src/main/target/SPRACINGF3EVO/AIORACERF3.mk b/src/main/target/SPRACINGF3EVO/AIORACERF3.mk
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c
index 65e9d3851d..80e7ac47b3 100644
--- a/src/main/target/SPRACINGF3EVO/target.c
+++ b/src/main/target/SPRACINGF3EVO/target.c
@@ -27,6 +27,16 @@
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
// PPM / UART2 RX
DEF_TIM(TIM8, CH1, PA15, TIM_USE_PPM, 0 ), // PPM
+#ifdef AIORACERF3
+ DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // PWM1
+ DEF_TIM(TIM3, CH2, PA7, TIM_USE_MOTOR, 1 ), // PWM2
+ DEF_TIM(TIM15, CH1, PA2, TIM_USE_MOTOR, 1 ), // PWM3
+ DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1 ), // PWM4
+ DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // PWM5
+ DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM6
+ DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM7
+ DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1 ), // PWM8
+#else
DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1 ), // PWM1 [TIM2_CH1 (D1_CH5)]
DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1 ), // PWM2 [TIM2_CH2 (D1_CH7)] [TIM15_CH1N (D1_CH5)]
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1 ), // PWM3 [TIM2_CH3 (D1_CH1)] [TIM15_CH1 (D1_CH5)]
@@ -35,6 +45,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM3, CH2, PA7, TIM_USE_MOTOR, 1 ), // PWM6
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // PWM7
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // PWM8
+#endif
DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 1 ), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7)
DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 1 ), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED | TIM_USE_TRANSPONDER, 1 ), // LED_STRIP / TRANSPONDER
diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h
index 7d379d57da..54381db2f9 100755
--- a/src/main/target/SPRACINGF3EVO/target.h
+++ b/src/main/target/SPRACINGF3EVO/target.h
@@ -17,7 +17,11 @@
#pragma once
+#ifdef AIORACERF3
+#define TARGET_BOARD_IDENTIFIER "ARF3"
+#else
#define TARGET_BOARD_IDENTIFIER "SPEV"
+#endif
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h
index 24bfcfc5e0..35b74f1569 100644
--- a/src/main/target/SPRACINGF3MINI/target.h
+++ b/src/main/target/SPRACINGF3MINI/target.h
@@ -51,11 +51,11 @@
#define BARO
#define USE_BARO_BMP280
-//#define MAG
-//#define USE_MPU9250_MAG // Enables bypass configuration
-//#define USE_MAG_AK8975
-//#define USE_MAG_HMC5883 // External
-//#define MAG_AK8975_ALIGN CW90_DEG_FLIP
+#define MAG
+#define USE_MPU9250_MAG // Enables bypass configuration
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883 // External
+#define MAG_AK8975_ALIGN CW90_DEG_FLIP
//#define SONAR
//#define SONAR_ECHO_PIN PB1
diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h
index 5f7e59a94b..565afa796b 100755
--- a/src/main/target/SPRACINGF3NEO/target.h
+++ b/src/main/target/SPRACINGF3NEO/target.h
@@ -72,6 +72,9 @@
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
+
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
@@ -179,7 +182,6 @@
#define HARDWARE_BIND_PLUG
#define BINDPLUG_PIN PD2
-
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
// IO - assuming 303 in 64pin package, TODO
diff --git a/src/main/target/TINYBEEF3/target.c b/src/main/target/TINYBEEF3/target.c
new file mode 100644
index 0000000000..ad5befebf4
--- /dev/null
+++ b/src/main/target/TINYBEEF3/target.c
@@ -0,0 +1,41 @@
+/*
+ * 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/timer.h"
+#include "drivers/timer_def.h"
+#include "drivers/dma.h"
+
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+ // PPM / UART2 RX
+ DEF_TIM(TIM8, CH1, PA15, TIM_USE_PPM, 0 ), // PPM
+ DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1 ), // PWM1
+ DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1 ), // PWM2
+ DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // PWM3
+ DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // PWM4
+ DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1 ), // PWM5
+ DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM6
+ DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM7
+ DEF_TIM(TIM3, CH2, PA7, TIM_USE_MOTOR, 1 ), // PWM8
+ DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 1 ), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7)
+ DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 1 ), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7)
+ DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED | TIM_USE_TRANSPONDER, 1 ), // LED_STRIP / TRANSPONDER
+};
diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/TINYBEEF3/target.h
similarity index 81%
rename from src/main/target/AIORACERF3/target.h
rename to src/main/target/TINYBEEF3/target.h
index bf112222f2..b7b04730e5 100644
--- a/src/main/target/AIORACERF3/target.h
+++ b/src/main/target/TINYBEEF3/target.h
@@ -17,7 +17,7 @@
#pragma once
-#define TARGET_BOARD_IDENTIFIER "ARF3"
+#define TARGET_BOARD_IDENTIFIER "TBF3"
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
@@ -26,18 +26,16 @@
#define BEEPER PC15
#define BEEPER_INVERTED
-#define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip
-
-#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect
-
#define USE_EXTI
#define MPU_INT_EXTI PC13
+#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
#define USE_MAG_DATA_READY_SIGNAL
#define ENSURE_MAG_DATA_READY_IS_HIGH
+#define USE_ESC_TELEMETRY
#define GYRO
#define USE_GYRO_SPI_MPU6500
@@ -45,14 +43,14 @@
#define ACC
#define USE_ACC_SPI_MPU6500
-#define ACC_MPU6500_ALIGN CW180_DEG
-#define GYRO_MPU6500_ALIGN CW180_DEG
+#define ACC_MPU6500_ALIGN CW270_DEG
+#define GYRO_MPU6500_ALIGN CW270_DEG
#define BARO
#define USE_BARO_BMP280
-#define MAG
-#define USE_MAG_AK8963
+//#define MAG
+//#define USE_MAG_AK8963
//#define USE_MAG_HMC5883 // External
#define MAG_AK8963_ALIGN CW90_DEG_FLIP
@@ -120,24 +118,13 @@
#define BOARD_HAS_VOLTAGE_DIVIDER
#define USE_ADC
#define ADC_INSTANCE ADC2
-#define VBAT_ADC_PIN PA5
-#define CURRENT_METER_ADC_PIN PA4
+#define VBAT_ADC_PIN PA4
+#define CURRENT_METER_ADC_PIN PA5
#define RSSI_ADC_PIN PB2
#define LED_STRIP
#define TRANSPONDER
-#define TRANSPONDER_GPIO GPIOA
-#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
-#define TRANSPONDER_GPIO_AF GPIO_AF_6
-#define TRANSPONDER_PIN GPIO_Pin_8
-#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8
-#define TRANSPONDER_TIMER TIM1
-#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
-#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2
-#define TRANSPONDER_IRQ DMA1_Channel2_IRQn
-#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2
-#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
@@ -156,5 +143,5 @@
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
+#define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15))
-
diff --git a/src/main/target/AIORACERF3/target.mk b/src/main/target/TINYBEEF3/target.mk
similarity index 90%
rename from src/main/target/AIORACERF3/target.mk
rename to src/main/target/TINYBEEF3/target.mk
index c5d14b1f50..937cd6bf4d 100644
--- a/src/main/target/AIORACERF3/target.mk
+++ b/src/main/target/TINYBEEF3/target.mk
@@ -7,6 +7,7 @@ TARGET_SRC = \
drivers/accgyro_spi_mpu6500.c \
drivers/barometer_bmp280.c \
drivers/compass_ak8963.c \
+ drivers/serial_usb_vcp.c \
drivers/transponder_ir.c \
drivers/transponder_ir_stm32f30x.c \
io/transponder_ir.c