From 61392c5afca27efba162fc400f400f55f57d618d Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 12:55:23 -0500 Subject: [PATCH 01/17] Removing inline from constrin and constrainf. These functions are used elsewhere and the inline declaration can cause the compiler to make them static and unavailable outside of this file. Unless these functions are defined in a .h file, they cannot be inlined. --- src/main/common/maths.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 3be8eff722..f54aeacdb7 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -111,7 +111,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } -inline int constrain(int amt, int low, int high) +int constrain(int amt, int low, int high) { if (amt < low) return low; @@ -121,7 +121,7 @@ inline int constrain(int amt, int low, int high) return amt; } -inline float constrainf(float amt, float low, float high) +float constrainf(float amt, float low, float high) { if (amt < low) return low; From b3216439de1a2023efb352eb34506ea4cc540f6e Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 12:57:14 -0500 Subject: [PATCH 02/17] Allow the address of the start of the flash space to be defined externally. For testing, the flash memory can be allocated and defined to a different region than on an embedded processor. --- src/main/config/config.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index 116692ee13..07cb5abbaf 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -124,8 +124,15 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es #define FLASH_TO_RESERVE_FOR_CONFIG 0x1000 #endif +// use the last flash pages for storage +#ifdef CUSTOM_FLASH_MEMORY_ADDRESS +size_t custom_flash_memory_address = 0; +#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) +#else // use the last flash pages for storage #define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) +#endif + master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; From ac11732a869f57be1280938ab44acfaa0294e2b4 Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 12:58:37 -0500 Subject: [PATCH 03/17] Separate the initialization and main step into two different functions. For testing, it is useful to have the loop be separated from the initialization so that an external function can step through the main loop. --- src/main/main.c | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 49fc7c2466..56e9077d4e 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -26,7 +26,6 @@ #include "common/axis.h" #include "common/color.h" -#include "common/atomic.h" #include "common/maths.h" #include "drivers/nvic.h" @@ -658,7 +657,7 @@ void processLoopback(void) { #define processLoopback() #endif -int main(void) { +void main_init(void) { init(); /* Setup scheduler */ @@ -729,12 +728,22 @@ int main(void) { #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif +} - while (1) { - scheduler(); - processLoopback(); +void main_step(void) { + scheduler(); + processLoopback(); +} + +#ifndef NOMAIN +int main(void) +{ + main_init(); + while(1) { + main_step(); } } +#endif #ifdef DEBUG_HARDFAULTS //from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/ From 091aa24249a81ee70d4848513f1b15ecd9ccb906 Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Thu, 28 Apr 2016 13:00:16 -0500 Subject: [PATCH 04/17] For fake gyro/acc, set the output to an fake variable. For testing, the gyros and accelerometer values might needed to be artifically set. This change allows a testing loop to set the gyro and acc to values through a global variable. --- src/main/sensors/initialisation.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 9c49090d4f..415ebbc4a4 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -214,6 +214,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void) } #ifdef USE_FAKE_GYRO +int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 }; static void fakeGyroInit(uint16_t lpf) { UNUSED(lpf); @@ -221,7 +222,9 @@ static void fakeGyroInit(uint16_t lpf) static bool fakeGyroRead(int16_t *gyroADC) { - memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); + for (int i = 0; i < XYZ_AXIS_COUNT; ++i) { + gyroADC[i] = fake_gyro_values[i]; + } return true; } @@ -241,9 +244,13 @@ bool fakeGyroDetect(gyro_t *gyro) #endif #ifdef USE_FAKE_ACC +int16_t fake_acc_values[XYZ_AXIS_COUNT] = {0,0,0}; + static void fakeAccInit(void) {} static bool fakeAccRead(int16_t *accData) { - memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); + for(int i=0;i Date: Thu, 28 Apr 2016 13:50:03 -0500 Subject: [PATCH 05/17] Fixing null deref error when VBAT is not set. If VBAT is not configured, batteryConfig is null. This will possibly set the vbatPidCompensation to an unknown value. This change checkes for the validity of batteryConfig before defining the values to be used. --- src/main/flight/mixer.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 802e7e780e..813ec50f58 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -753,6 +753,11 @@ void mixTable(void) uint32_t i; fix12_t vbatCompensationFactor; static fix12_t mixReduction; + bool use_vbat_compensation = false; + if (batteryConfig && batteryConfig->vbatPidCompensation) { + use_vbat_compensation = true; + vbatCompensationFactor = calculateVbatPidCompensation(); + } bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code @@ -766,8 +771,6 @@ void mixTable(void) int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. int16_t rollPitchYawMixMin = 0; - if (batteryConfig->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Calculate voltage compensation - // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = @@ -775,7 +778,7 @@ void mixTable(void) axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; - if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation + if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; From b6ff69fedafcd9fd78ac9ea67e21736365e0edc5 Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Mon, 2 May 2016 10:03:43 -0500 Subject: [PATCH 06/17] Moving constrain and constrainf to the include file to be inlined. --- src/main/common/maths.c | 20 -------------------- src/main/common/maths.h | 22 ++++++++++++++++++++++ 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index f54aeacdb7..888847e075 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -111,26 +111,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } -int constrain(int amt, int low, int high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} - -float constrainf(float amt, float low, float high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} - void devClear(stdev_t *dev) { dev->m_n = 0; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 4a32e282c0..95f0ee8583 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -116,3 +116,25 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); int16_t qPercent(fix12_t q); int16_t qMultiply(fix12_t q, int16_t input); fix12_t qConstruct(int16_t num, int16_t den); + +// Defining constrain and constrainf as inline in the include file +// because these functions are used universally and should be fast. +inline int constrain(int amt, int low, int high) +{ + if (amt < low) + return low; + else if (amt > high) + return high; + else + return amt; +} + +inline float constrainf(float amt, float low, float high) +{ + if (amt < low) + return low; + else if (amt > high) + return high; + else + return amt; +} From d97d4dd544807ac2c6b82553c3cb77f484038eda Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:16:02 -0700 Subject: [PATCH 07/17] hardfault handler w/ debugging info and an automatic breakpoint --- Makefile | 17 +- src/main/main.c | 61 +++ .../startup_stm32f3_debug_hardfault_handler.S | 501 ++++++++++++++++++ 3 files changed, 576 insertions(+), 3 deletions(-) create mode 100644 src/main/startup/startup_stm32f3_debug_hardfault_handler.S diff --git a/Makefile b/Makefile index 90217b59a8..723545eee2 100644 --- a/Makefile +++ b/Makefile @@ -28,6 +28,10 @@ OPBL ?=no # Debugger optons, must be empty or GDB DEBUG ?= +# Insert the debugging hardfault debugger +# releases should not be built with this flag as it does not disable pwm output +DEBUG_HARDFAULTS ?= + # Serial port/Device for flashing SERIAL_DEVICE ?= $(firstword $(wildcard /dev/ttyUSB*) no-port-found) @@ -56,6 +60,13 @@ OPBL_VALID_TARGETS = CC3D_OPBL F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE +# note that there is no hardfault debugging startup file assembly handler for other platforms +ifeq ($(DEBUG_HARDFAULTS),F3) +CFLAGS += -DDEBUG_HARDFAULTS +STM32F30x_COMMON_SRC = startup_stm32f3_debug_hardfault_handler.S +else +STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S +endif # Configure default flash sizes for the targets ifeq ($(FLASH_SIZE),) @@ -534,8 +545,8 @@ CC3D_SRC = \ $(COMMON_SRC) \ $(VCP_SRC) -STM32F30x_COMMON_SRC = \ - startup_stm32f30x_md_gcc.S \ + +STM32F30x_COMMON_SRC += \ drivers/adc.c \ drivers/adc_stm32f30x.c \ drivers/bus_i2c_stm32f30x.c \ @@ -816,7 +827,7 @@ endif DEBUG_FLAGS = -ggdb3 -DDEBUG -CFLAGS = $(ARCH_FLAGS) \ +CFLAGS += $(ARCH_FLAGS) \ $(LTO_FLAGS) \ $(addprefix -D,$(OPTIONS)) \ $(addprefix -I,$(INCLUDE_DIRS)) \ diff --git a/src/main/main.c b/src/main/main.c index 59158b2a55..49fc7c2466 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -736,6 +736,66 @@ int main(void) { } } +#ifdef DEBUG_HARDFAULTS +//from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/ +/** + * hard_fault_handler_c: + * This is called from the HardFault_HandlerAsm with a pointer the Fault stack + * as the parameter. We can then read the values from the stack and place them + * into local variables for ease of reading. + * We then read the various Fault Status and Address Registers to help decode + * cause of the fault. + * The function ends with a BKPT instruction to force control back into the debugger + */ +void hard_fault_handler_c(unsigned long *hardfault_args){ + volatile unsigned long stacked_r0 ; + volatile unsigned long stacked_r1 ; + volatile unsigned long stacked_r2 ; + volatile unsigned long stacked_r3 ; + volatile unsigned long stacked_r12 ; + volatile unsigned long stacked_lr ; + volatile unsigned long stacked_pc ; + volatile unsigned long stacked_psr ; + volatile unsigned long _CFSR ; + volatile unsigned long _HFSR ; + volatile unsigned long _DFSR ; + volatile unsigned long _AFSR ; + volatile unsigned long _BFAR ; + volatile unsigned long _MMAR ; + + stacked_r0 = ((unsigned long)hardfault_args[0]) ; + stacked_r1 = ((unsigned long)hardfault_args[1]) ; + stacked_r2 = ((unsigned long)hardfault_args[2]) ; + stacked_r3 = ((unsigned long)hardfault_args[3]) ; + stacked_r12 = ((unsigned long)hardfault_args[4]) ; + stacked_lr = ((unsigned long)hardfault_args[5]) ; + stacked_pc = ((unsigned long)hardfault_args[6]) ; + stacked_psr = ((unsigned long)hardfault_args[7]) ; + + // Configurable Fault Status Register + // Consists of MMSR, BFSR and UFSR + _CFSR = (*((volatile unsigned long *)(0xE000ED28))) ; + + // Hard Fault Status Register + _HFSR = (*((volatile unsigned long *)(0xE000ED2C))) ; + + // Debug Fault Status Register + _DFSR = (*((volatile unsigned long *)(0xE000ED30))) ; + + // Auxiliary Fault Status Register + _AFSR = (*((volatile unsigned long *)(0xE000ED3C))) ; + + // Read the Fault Address Registers. These may not contain valid values. + // Check BFARVALID/MMARVALID to see if they are valid values + // MemManage Fault Address Register + _MMAR = (*((volatile unsigned long *)(0xE000ED34))) ; + // Bus Fault Address Register + _BFAR = (*((volatile unsigned long *)(0xE000ED38))) ; + + __asm("BKPT #0\n") ; // Break into the debugger +} + +#else void HardFault_Handler(void) { // fall out of the sky @@ -753,3 +813,4 @@ void HardFault_Handler(void) while (1); } +#endif diff --git a/src/main/startup/startup_stm32f3_debug_hardfault_handler.S b/src/main/startup/startup_stm32f3_debug_hardfault_handler.S new file mode 100644 index 0000000000..65ee39a670 --- /dev/null +++ b/src/main/startup/startup_stm32f3_debug_hardfault_handler.S @@ -0,0 +1,501 @@ +/** + ****************************************************************************** + * @file startup_stm32f30x.s + * @author MCD Application Team + * @version V1.0.0 + * @date 04-Spetember-2012 + * @brief STM32F30x Devices vector table for RIDE7 toolchain. + * This module performs: + * - Set the initial SP + * - Set the initial PC == Reset_Handler, + * - Set the vector table entries with the exceptions ISR address + * - Configure the clock system and the external SRAM mounted on + * STM3230C-EVAL board to be used as data memory (optional, + * to be enabled by user) + * - Branches to main in the C library (which eventually + * calls main()). + * After Reset the Cortex-M4 processor is in Thread mode, + * priority is Privileged, and the Stack is set to Main. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + .thumb + +.global g_pfnVectors +.global Default_Handler + +.global HardFault_Handler +.extern hard_fault_handler_c + +/* 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: + ldr r0, =0x20009FFC // HJI 11/9/2012 + ldr r1, =0xDEADBEEF // HJI 11/9/2012 + ldr r2, [r0, #0] // HJI 11/9/2012 + str r0, [r0, #0] // HJI 11/9/2012 + cmp r2, r1 // HJI 11/9/2012 + beq Reboot_Loader // HJI 11/9/2012 + +/* 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 + +/* Call the clock system intitialization function.*/ + bl SystemInit +/* Call the application's entry point.*/ + bl main + bx lr + +LoopForever: + b LoopForever + +Reboot_Loader: // HJI 11/9/2012 + + // Reboot to ROM // HJI 11/9/2012 + ldr r0, =0x1FFFD800 // HJI 4/26/2013 + ldr sp,[r0, #0] // HJI 11/9/2012 + ldr r0,[r0, #4] // HJI 11/9/2012 + bx r0 // HJI 11/9/2012 + +.size Reset_Handler, .-Reset_Handler + +.section .text.Reset_Handler +.weak HardFault_Handler +.type HardFault_Handler, %function +HardFault_Handler: + movs r0,#4 + movs r1, lr + tst r0, r1 + beq _MSP + mrs r0, psp + b _HALT +_MSP: + mrs r0, msp +_HALT: + ldr r1,[r0,#20] + b hard_fault_handler_c + bkpt #0 + +.size HardFault_Handler, .-HardFault_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 M4. 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 + .word WWDG_IRQHandler + .word PVD_IRQHandler + .word TAMPER_STAMP_IRQHandler + .word RTC_WKUP_IRQHandler + .word FLASH_IRQHandler + .word RCC_IRQHandler + .word EXTI0_IRQHandler + .word EXTI1_IRQHandler + .word EXTI2_TS_IRQHandler + .word EXTI3_IRQHandler + .word EXTI4_IRQHandler + .word DMA1_Channel1_IRQHandler + .word DMA1_Channel2_IRQHandler + .word DMA1_Channel3_IRQHandler + .word DMA1_Channel4_IRQHandler + .word DMA1_Channel5_IRQHandler + .word DMA1_Channel6_IRQHandler + .word DMA1_Channel7_IRQHandler + .word ADC1_2_IRQHandler + .word USB_HP_CAN1_TX_IRQHandler + .word USB_LP_CAN1_RX0_IRQHandler + .word CAN1_RX1_IRQHandler + .word CAN1_SCE_IRQHandler + .word EXTI9_5_IRQHandler + .word TIM1_BRK_TIM15_IRQHandler + .word TIM1_UP_TIM16_IRQHandler + .word TIM1_TRG_COM_TIM17_IRQHandler + .word TIM1_CC_IRQHandler + .word TIM2_IRQHandler + .word TIM3_IRQHandler + .word TIM4_IRQHandler + .word I2C1_EV_IRQHandler + .word I2C1_ER_IRQHandler + .word I2C2_EV_IRQHandler + .word I2C2_ER_IRQHandler + .word SPI1_IRQHandler + .word SPI2_IRQHandler + .word USART1_IRQHandler + .word USART2_IRQHandler + .word USART3_IRQHandler + .word EXTI15_10_IRQHandler + .word RTC_Alarm_IRQHandler + .word USBWakeUp_IRQHandler + .word TIM8_BRK_IRQHandler + .word TIM8_UP_IRQHandler + .word TIM8_TRG_COM_IRQHandler + .word TIM8_CC_IRQHandler + .word ADC3_IRQHandler + .word 0 + .word 0 + .word 0 + .word SPI3_IRQHandler + .word UART4_IRQHandler + .word UART5_IRQHandler + .word TIM6_DAC_IRQHandler + .word TIM7_IRQHandler + .word DMA2_Channel1_IRQHandler + .word DMA2_Channel2_IRQHandler + .word DMA2_Channel3_IRQHandler + .word DMA2_Channel4_IRQHandler + .word DMA2_Channel5_IRQHandler + .word ADC4_IRQHandler + .word 0 + .word 0 + .word COMP1_2_3_IRQHandler + .word COMP4_5_6_IRQHandler + .word COMP7_IRQHandler + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word 0 + .word USB_HP_IRQHandler + .word USB_LP_IRQHandler + .word USBWakeUp_RMP_IRQHandler + .word 0 + .word 0 + .word 0 + .word 0 + .word FPU_IRQHandler + +/******************************************************************************* +* +* 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 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 TAMPER_STAMP_IRQHandler + .thumb_set TAMPER_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_TS_IRQHandler + .thumb_set EXTI2_TS_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA1_Channel1_IRQHandler + .thumb_set DMA1_Channel1_IRQHandler,Default_Handler + + .weak DMA1_Channel2_IRQHandler + .thumb_set DMA1_Channel2_IRQHandler,Default_Handler + + .weak DMA1_Channel3_IRQHandler + .thumb_set DMA1_Channel3_IRQHandler,Default_Handler + + .weak DMA1_Channel4_IRQHandler + .thumb_set DMA1_Channel4_IRQHandler,Default_Handler + + .weak DMA1_Channel5_IRQHandler + .thumb_set DMA1_Channel5_IRQHandler,Default_Handler + + .weak DMA1_Channel6_IRQHandler + .thumb_set DMA1_Channel6_IRQHandler,Default_Handler + + .weak DMA1_Channel7_IRQHandler + .thumb_set DMA1_Channel7_IRQHandler,Default_Handler + + .weak ADC1_2_IRQHandler + .thumb_set ADC1_2_IRQHandler,Default_Handler + + .weak USB_HP_CAN1_TX_IRQHandler + .thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler + + .weak USB_LP_CAN1_RX0_IRQHandler + .thumb_set USB_LP_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_TIM15_IRQHandler + .thumb_set TIM1_BRK_TIM15_IRQHandler,Default_Handler + + .weak TIM1_UP_TIM16_IRQHandler + .thumb_set TIM1_UP_TIM16_IRQHandler,Default_Handler + + .weak TIM1_TRG_COM_TIM17_IRQHandler + .thumb_set TIM1_TRG_COM_TIM17_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 USBWakeUp_IRQHandler + .thumb_set USBWakeUp_IRQHandler,Default_Handler + + .weak TIM8_BRK_IRQHandler + .thumb_set TIM8_BRK_IRQHandler,Default_Handler + + .weak TIM8_UP_IRQHandler + .thumb_set TIM8_UP_IRQHandler,Default_Handler + + .weak TIM8_TRG_COM_IRQHandler + .thumb_set TIM8_TRG_COM_IRQHandler,Default_Handler + + .weak TIM8_CC_IRQHandler + .thumb_set TIM8_CC_IRQHandler,Default_Handler + + .weak ADC3_IRQHandler + .thumb_set ADC3_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_Channel1_IRQHandler + .thumb_set DMA2_Channel1_IRQHandler,Default_Handler + + .weak DMA2_Channel2_IRQHandler + .thumb_set DMA2_Channel2_IRQHandler,Default_Handler + + .weak DMA2_Channel3_IRQHandler + .thumb_set DMA2_Channel3_IRQHandler,Default_Handler + + .weak DMA2_Channel4_IRQHandler + .thumb_set DMA2_Channel4_IRQHandler,Default_Handler + + .weak DMA2_Channel5_IRQHandler + .thumb_set DMA2_Channel5_IRQHandler,Default_Handler + + .weak ADC4_IRQHandler + .thumb_set ADC4_IRQHandler,Default_Handler + + .weak COMP1_2_3_IRQHandler + .thumb_set COMP1_2_3_IRQHandler,Default_Handler + + .weak COMP4_5_6_IRQHandler + .thumb_set COMP4_5_6_IRQHandler,Default_Handler + + .weak COMP7_IRQHandler + .thumb_set COMP7_IRQHandler,Default_Handler + + .weak USB_HP_IRQHandler + .thumb_set USB_HP_IRQHandler,Default_Handler + + .weak USB_LP_IRQHandler + .thumb_set USB_LP_IRQHandler,Default_Handler + + .weak USBWakeUp_RMP_IRQHandler + .thumb_set USBWakeUp_RMP_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From 08e87a40cf1c88dfba8bfff57f4a2b5c21148c0f Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:34:55 -0700 Subject: [PATCH 08/17] dont try to check the gyro status if the gyro doesnt support interrupts --- src/main/drivers/gyro_sync.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 1047442c0b..934708f379 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -32,7 +32,8 @@ static uint8_t mpuDividerDrops; bool getMpuDataStatus(gyro_t *gyro) { bool mpuDataStatus; - + if (!gyro->intStatus) + return false; gyro->intStatus(&mpuDataStatus); return mpuDataStatus; } From 2264494fca03269fbf1ddd12e6b9513545f217e9 Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:35:09 -0700 Subject: [PATCH 09/17] duplicate file --- Makefile | 1 - 1 file changed, 1 deletion(-) diff --git a/Makefile b/Makefile index 723545eee2..d5bfb1d006 100644 --- a/Makefile +++ b/Makefile @@ -575,7 +575,6 @@ STM32F3DISCOVERY_COMMON_SRC = \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ drivers/accgyro_l3gd20.c \ - drivers/accgyro_l3gd20.c \ drivers/accgyro_lsm303dlhc.c \ drivers/compass_hmc5883l.c \ $(VCP_SRC) From 9b3aecbffcfb2f6bc5cc68cda60e4d76151d4d6e Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:41:43 -0700 Subject: [PATCH 10/17] print the target name so we can figure out which target is breaking w/ fake_travis_build.sh --- fake_travis_build.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 2a6a9b275e..9d1fe66169 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -26,6 +26,9 @@ export TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/simulated} for target in "${targets[@]}" do unset RUNTESTS PUBLISHMETA TARGET + echo + echo + echo "BUILDING '$target'" eval "export $target" make -f Makefile clean ./.travis.sh From e1f52d9c7bc60668a0118db92c61c731f308dcc5 Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 14:33:36 -0700 Subject: [PATCH 11/17] guess we need some defines --- src/main/target/STM32F3DISCOVERY/target.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 8a812d879c..190676ef3f 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -152,6 +152,20 @@ #define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 +#define LED_STRIP +#define LED_STRIP_TIMER TIM16 +#define WS2811_GPIO GPIOB +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM16 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER + #define BLACKBOX #define GPS //#define GTUNE From 3dd6424f34706f1bc62fd9b80c09f067b26e99bc Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 30 Apr 2016 12:54:15 +0200 Subject: [PATCH 12/17] Revert "Fixed a bug which caused printing of floats larger than 100 in the CLI to crash" This reverts commit 6ce8fe3c0fa7725e77d6ea61557bcec65d6cdb34. --- src/main/common/typeconversion.c | 5 ----- src/main/common/typeconversion.h | 10 ---------- src/main/io/serial_cli.c | 2 +- 3 files changed, 1 insertion(+), 16 deletions(-) diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 3a3730fd56..4e3f8b4c2e 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -144,11 +144,6 @@ char *itoa(int i, char *a, int base) #endif -/* Note: the floatString must be at least 13 bytes long to cover all cases. - * This includes up to 10 digits (32 bit ints can hold numbers up to 10 - * digits long) + the decimal point + negative sign or space + null - * terminator. - */ char *ftoa(float x, char *floatString) { int32_t value; diff --git a/src/main/common/typeconversion.h b/src/main/common/typeconversion.h index f2811d65ba..62b437875b 100644 --- a/src/main/common/typeconversion.h +++ b/src/main/common/typeconversion.h @@ -21,17 +21,7 @@ void li2a(long num, char *bf); void ui2a(unsigned int num, unsigned int base, int uc, char *bf); void i2a(int num, char *bf); char a2i(char ch, const char **src, int base, int *nump); - -/* Simple conversion of a float to a string. Will display completely - * inaccurate results for floats larger than about 2.15E6 (2^31 / 1000) - * (same thing for negative values < -2.15E6). - * Will always display 3 decimals, so anything smaller than 1E-3 will - * not be displayed. - * The floatString will be filled in with the result and will be - * returned. It must be at least 13 bytes in length to cover all cases! - */ char *ftoa(float x, char *floatString); - float fastA2F(const char *p); #ifndef HAVE_ITOA_FUNCTION diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2f837ed9ef..d3a7e79c20 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2448,7 +2448,7 @@ static void cliWrite(uint8_t ch) static void cliPrintVar(const clivalue_t *var, uint32_t full) { int32_t value = 0; - char buf[13]; + char buf[8]; void *ptr = var->ptr; if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { From 29865701daa6553aa4da2b18653aa4cb9a1f0aa5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 30 Apr 2016 23:19:00 +0200 Subject: [PATCH 13/17] Change Filter lowpass Frequency to int --- src/main/config/config.c | 9 ++++----- src/main/config/config_master.h | 4 ++-- src/main/flight/mixer.h | 2 +- src/main/flight/pid.h | 4 ++-- src/main/io/serial_cli.c | 6 +++--- 5 files changed, 12 insertions(+), 13 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 116692ee13..fc6fe12183 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 133; +static const uint8_t EEPROM_CONF_VERSION = 134; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -177,12 +177,11 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 75; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->yaw_lpf_hz = 70.0f; - pidProfile->dterm_differentiator = 1; + pidProfile->yaw_lpf_hz = 100; pidProfile->rollPitchItermResetRate = 200; pidProfile->rollPitchItermResetAlways = 0; pidProfile->yawItermResetRate = 50; - pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default + pidProfile->dterm_lpf_hz = 70; // filtering ON by default pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes @@ -403,7 +402,7 @@ static void resetConf(void) masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.gyro_lpf = 0; // 256HZ default masterConfig.gyro_sync_denom = 8; - masterConfig.gyro_soft_lpf_hz = 80.0f; + masterConfig.gyro_soft_lpf_hz = 80; masterConfig.pid_process_denom = 1; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4af7851e4f..281a7fa3d5 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -56,9 +56,9 @@ typedef struct master_t { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. - uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_sync_denom; // Gyro sample divider - float gyro_soft_lpf_hz; // Biqyad gyro lpf hz + uint8_t gyro_soft_lpf_hz; // Biqyad gyro lpf hz uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index eb324c6768..36dca92263 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -74,7 +74,7 @@ typedef struct mixerConfig_s { uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100) #ifdef USE_SERVOS uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq + uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq int8_t servo_lowpass_enable; // enable/disable lowpass filter #endif } mixerConfig_t; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 94addfb55d..3f9bb7ef72 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -74,8 +74,8 @@ typedef struct pidProfile_s { uint8_t H_sensitivity; - float dterm_lpf_hz; // Delta Filter in hz - float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy + uint16_t dterm_lpf_hz; // Delta Filter in hz + uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d3a7e79c20..dc75498913 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -653,7 +653,7 @@ const clivalue_t valueTable[] = { { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, + { "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, @@ -673,7 +673,7 @@ const clivalue_t valueTable[] = { { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, #ifdef USE_SERVOS { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, - { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, + { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, #endif @@ -726,7 +726,7 @@ const clivalue_t valueTable[] = { { "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, - { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, + { "yaw_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, From ad756bceb4f4cbe5eb357901145000e053f05d2d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 1 May 2016 00:10:03 +0200 Subject: [PATCH 14/17] New version --- src/main/version.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/version.h b/src/main/version.h index 587b442c73..922d94b1d7 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,8 +16,8 @@ */ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed +#define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From a4456ce6b9b8abcccfce5abe940bc3a7b27c2ba8 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 30 Apr 2016 23:20:06 +0200 Subject: [PATCH 15/17] Initial dynamic PID implementation New Defaults and some rework in dynamic PID Cli Fixes Copy / Paste Protection Change Stick threshold Remove differentiator Change Default PIDs --- src/main/blackbox/blackbox.c | 4 +-- src/main/config/config.c | 17 +++++----- src/main/flight/pid.c | 64 +++++++++++++++--------------------- src/main/flight/pid.h | 7 ++-- src/main/io/serial_cli.c | 10 +++--- src/main/io/serial_msp.c | 2 +- src/main/mw.c | 14 +------- 7 files changed, 48 insertions(+), 70 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index e73f69387e..a84b3768fb 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1280,8 +1280,8 @@ static bool blackboxWriteSysinfo() masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); break; case 36: - blackboxPrintfHeaderLine("dterm_differentiator:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator); + blackboxPrintfHeaderLine("dynamic_pterm:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pterm); break; case 37: blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", diff --git a/src/main/config/config.c b/src/main/config/config.c index fc6fe12183..4d6c046128 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -148,10 +148,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidController = 1; pidProfile->P8[ROLL] = 45; - pidProfile->I8[ROLL] = 30; + pidProfile->I8[ROLL] = 35; pidProfile->D8[ROLL] = 18; pidProfile->P8[PITCH] = 45; - pidProfile->I8[PITCH] = 30; + pidProfile->I8[PITCH] = 35; pidProfile->D8[PITCH] = 18; pidProfile->P8[YAW] = 90; pidProfile->I8[YAW] = 40; @@ -177,11 +177,12 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 75; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->yaw_lpf_hz = 100; + pidProfile->yaw_lpf_hz = 70; pidProfile->rollPitchItermResetRate = 200; pidProfile->rollPitchItermResetAlways = 0; pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70; // filtering ON by default + pidProfile->dynamic_pterm = 1; pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes @@ -307,9 +308,9 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcExpo8 = 60; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; - controlRateConfig->dynThrPID = 0; + controlRateConfig->dynThrPID = 20; controlRateConfig->rcYawExpo8 = 20; - controlRateConfig->tpa_breakpoint = 1500; + controlRateConfig->tpa_breakpoint = 1650; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { controlRateConfig->rates[axis] = 50; @@ -401,10 +402,10 @@ static void resetConf(void) masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.gyro_lpf = 0; // 256HZ default - masterConfig.gyro_sync_denom = 8; - masterConfig.gyro_soft_lpf_hz = 80; + masterConfig.gyro_sync_denom = 4; + masterConfig.gyro_soft_lpf_hz = 100; - masterConfig.pid_process_denom = 1; + masterConfig.pid_process_denom = 2; masterConfig.debug_mode = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0246fa71f1..cc3312820e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -93,6 +93,16 @@ float calculateExpoPlus(int axis, rxConfig_t *rxConfig) { return propFactor; } +uint16_t getDynamicKp(int axis, pidProfile_t *pidProfile) { + uint16_t dynamicKp; + + uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7); + + dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8; + + return dynamicKp; +} + void pidResetErrorAngle(void) { errorAngleI[ROLL] = 0; @@ -136,7 +146,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; - static float lastRate[3][PID_LAST_RATE_COUNT]; + static float lastRate[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -200,11 +210,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; + uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; + // -----calculate P component if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { - PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); + PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); } else { - PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor; + PTerm = luxPTermScale * RateError * kP * tpaFactor; } // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply @@ -220,7 +232,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } if (axis == YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); + if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); } if (antiWindupProtection || motorLimitReached) { @@ -238,20 +250,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->dterm_differentiator) { - // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) - // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ - // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 - delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; - for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { - lastRate[axis][i] = lastRate[axis][i-1]; - } - } else { - // When DTerm low pass filter disabled apply moving average to reduce noise - delta = -(gyroRate - lastRate[axis][0]); - } - - lastRate[axis][0] = gyroRate; + delta = -(gyroRate - lastRate[axis]); + lastRate[axis] = gyroRate; // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); @@ -288,7 +288,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co int axis; int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRate[3][PID_LAST_RATE_COUNT]; + static int32_t lastRate[3]; int32_t AngleRateTmp, RateError, gyroRate; int8_t horizonLevelStrength = 100; @@ -342,11 +342,13 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co gyroRate = gyroADC[axis] / 4; RateError = AngleRateTmp - gyroRate; + uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; + // -----calculate P component if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { - PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; + PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; } else { - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7; } // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply @@ -366,11 +368,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { - if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); + if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13); } if (axis == YAW) { - if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); + if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13); } if (antiWindupProtection || motorLimitReached) { @@ -386,20 +388,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->dterm_differentiator) { - // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) - // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ - // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 - delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; - for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { - lastRate[axis][i] = lastRate[axis][i-1]; - } - } else { - // When DTerm low pass filter disabled apply moving average to reduce noise - delta = -(gyroRate - lastRate[axis][0]); - } - - lastRate[axis][0] = gyroRate; + delta = -(gyroRate - lastRate[axis]); + lastRate[axis] = gyroRate; // Divide delta by targetLooptime to get differential (ie dr/dt) delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f9bb7ef72..3738f97633 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -22,9 +22,8 @@ #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter -#define PID_LAST_RATE_COUNT 7 -#define ITERM_RESET_THRESHOLD 20 -#define ITERM_RESET_THRESHOLD_YAW 10 +#define ITERM_RESET_THRESHOLD 15 +#define DYNAMIC_PTERM_STICK_THRESHOLD 400 typedef enum { PIDROLL, @@ -81,7 +80,7 @@ typedef struct pidProfile_s { uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm - uint8_t dterm_differentiator; + uint8_t dynamic_pterm; #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index dc75498913..71583fbad2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -653,7 +653,7 @@ const clivalue_t valueTable[] = { { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, + { "gyro_lowpass", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, @@ -721,12 +721,12 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, - { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } }, - { "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, + { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "dynamic_pterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pterm, .config.lookup = { TABLE_OFF_ON } }, + { "iterm_always_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, - { "yaw_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, + { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c782124307..d44dc8e9aa 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -152,7 +152,7 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.pid_process_denom = 1; if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2; if (looptime < 250) { - masterConfig.pid_process_denom = 3; + masterConfig.pid_process_denom = 4; } else if (looptime < 375) { if (currentProfile->pidProfile.pidController == 2) { masterConfig.pid_process_denom = 3; diff --git a/src/main/mw.c b/src/main/mw.c index 80d588baf4..7e8dfcd380 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -770,17 +770,6 @@ uint8_t setPidUpdateCountDown(void) { } } -// Check for oneshot125 protection. With fast looptimes oneshot125 pulse duration gets more near the pid looptime -bool shouldUpdateMotorsAfterPIDLoop(void) { - if (targetPidLooptime > 375 ) { - return true; - } else if ((masterConfig.use_multiShot || masterConfig.use_oneshot42) && feature(FEATURE_ONESHOT125)) { - return true; - } else { - return false; - } -} - // Function for loop trigger void taskMainPidLoopCheck(void) { static uint32_t previousTime; @@ -801,7 +790,6 @@ void taskMainPidLoopCheck(void) { static uint8_t pidUpdateCountdown; if (runTaskMainSubprocesses) { - if (!shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate(); subTasksMainPidLoop(); runTaskMainSubprocesses = false; } @@ -813,7 +801,7 @@ void taskMainPidLoopCheck(void) { } else { pidUpdateCountdown = setPidUpdateCountDown(); taskMainPidLoop(); - if (shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate(); + taskMotorUpdate(); runTaskMainSubprocesses = true; } From 5ffb3b5068568d49eba97ec023040ca10cea6a7e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 4 May 2016 00:00:04 +0200 Subject: [PATCH 16/17] Revert "Merge pull request #372 from aughey/betaflight" This reverts commit a6a5f50ffbd470175e8dae9e17e4d5f4424996f9, reversing changes made to 9cc55038516f52896ece099b6959a3413c25891f. --- src/main/common/maths.c | 20 ++++++++++++++++++++ src/main/common/maths.h | 22 ---------------------- src/main/config/config.c | 7 ------- src/main/flight/mixer.c | 9 +++------ src/main/main.c | 19 +++++-------------- src/main/sensors/initialisation.c | 11 ++--------- 6 files changed, 30 insertions(+), 58 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 888847e075..3be8eff722 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -111,6 +111,26 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } +inline int constrain(int amt, int low, int high) +{ + if (amt < low) + return low; + else if (amt > high) + return high; + else + return amt; +} + +inline float constrainf(float amt, float low, float high) +{ + if (amt < low) + return low; + else if (amt > high) + return high; + else + return amt; +} + void devClear(stdev_t *dev) { dev->m_n = 0; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 95f0ee8583..4a32e282c0 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -116,25 +116,3 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); int16_t qPercent(fix12_t q); int16_t qMultiply(fix12_t q, int16_t input); fix12_t qConstruct(int16_t num, int16_t den); - -// Defining constrain and constrainf as inline in the include file -// because these functions are used universally and should be fast. -inline int constrain(int amt, int low, int high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} - -inline float constrainf(float amt, float low, float high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} diff --git a/src/main/config/config.c b/src/main/config/config.c index 7844c0795b..4d6c046128 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -124,15 +124,8 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es #define FLASH_TO_RESERVE_FOR_CONFIG 0x1000 #endif -// use the last flash pages for storage -#ifdef CUSTOM_FLASH_MEMORY_ADDRESS -size_t custom_flash_memory_address = 0; -#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) -#else // use the last flash pages for storage #define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) -#endif - master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 813ec50f58..802e7e780e 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -753,11 +753,6 @@ void mixTable(void) uint32_t i; fix12_t vbatCompensationFactor; static fix12_t mixReduction; - bool use_vbat_compensation = false; - if (batteryConfig && batteryConfig->vbatPidCompensation) { - use_vbat_compensation = true; - vbatCompensationFactor = calculateVbatPidCompensation(); - } bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code @@ -771,6 +766,8 @@ void mixTable(void) int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. int16_t rollPitchYawMixMin = 0; + if (batteryConfig->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Calculate voltage compensation + // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = @@ -778,7 +775,7 @@ void mixTable(void) axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; - if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation + if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; diff --git a/src/main/main.c b/src/main/main.c index 56e9077d4e..49fc7c2466 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -26,6 +26,7 @@ #include "common/axis.h" #include "common/color.h" +#include "common/atomic.h" #include "common/maths.h" #include "drivers/nvic.h" @@ -657,7 +658,7 @@ void processLoopback(void) { #define processLoopback() #endif -void main_init(void) { +int main(void) { init(); /* Setup scheduler */ @@ -728,22 +729,12 @@ void main_init(void) { #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif -} -void main_step(void) { - scheduler(); - processLoopback(); -} - -#ifndef NOMAIN -int main(void) -{ - main_init(); - while(1) { - main_step(); + while (1) { + scheduler(); + processLoopback(); } } -#endif #ifdef DEBUG_HARDFAULTS //from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/ diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 415ebbc4a4..9c49090d4f 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -214,7 +214,6 @@ const extiConfig_t *selectMPUIntExtiConfig(void) } #ifdef USE_FAKE_GYRO -int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 }; static void fakeGyroInit(uint16_t lpf) { UNUSED(lpf); @@ -222,9 +221,7 @@ static void fakeGyroInit(uint16_t lpf) static bool fakeGyroRead(int16_t *gyroADC) { - for (int i = 0; i < XYZ_AXIS_COUNT; ++i) { - gyroADC[i] = fake_gyro_values[i]; - } + memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); return true; } @@ -244,13 +241,9 @@ bool fakeGyroDetect(gyro_t *gyro) #endif #ifdef USE_FAKE_ACC -int16_t fake_acc_values[XYZ_AXIS_COUNT] = {0,0,0}; - static void fakeAccInit(void) {} static bool fakeAccRead(int16_t *accData) { - for(int i=0;i Date: Wed, 4 May 2016 10:39:07 +0200 Subject: [PATCH 17/17] Fix data type for gyro lowpass // Defaults --- src/main/config/config.c | 4 ++-- src/main/io/serial_cli.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 4d6c046128..88bf765706 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -181,7 +181,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->rollPitchItermResetRate = 200; pidProfile->rollPitchItermResetAlways = 0; pidProfile->yawItermResetRate = 50; - pidProfile->dterm_lpf_hz = 70; // filtering ON by default + pidProfile->dterm_lpf_hz = 80; // filtering ON by default pidProfile->dynamic_pterm = 1; pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes @@ -403,7 +403,7 @@ static void resetConf(void) masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.gyro_lpf = 0; // 256HZ default masterConfig.gyro_sync_denom = 4; - masterConfig.gyro_soft_lpf_hz = 100; + masterConfig.gyro_soft_lpf_hz = 85; masterConfig.pid_process_denom = 2; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 71583fbad2..3f0bb134dc 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -653,7 +653,7 @@ const clivalue_t valueTable[] = { { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, + { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },