From d97d4dd544807ac2c6b82553c3cb77f484038eda Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 30 Apr 2016 12:16:02 -0700 Subject: [PATCH 1/9] 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 90217b59a..723545eee 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 59158b2a5..49fc7c246 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 000000000..65ee39a67 --- /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 2/9] 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 1047442c0..934708f37 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 3/9] duplicate file --- Makefile | 1 - 1 file changed, 1 deletion(-) diff --git a/Makefile b/Makefile index 723545eee..d5bfb1d00 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 4/9] 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 2a6a9b275..9d1fe6616 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 5/9] 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 8a812d879..190676ef3 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 6/9] 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 3a3730fd5..4e3f8b4c2 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 f2811d65b..62b437875 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 2f837ed9e..d3a7e79c2 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 7/9] 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 116692ee1..fc6fe1218 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 4af7851e4..281a7fa3d 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 eb324c676..36dca9226 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 94addfb55..3f9bb7ef7 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 d3a7e79c2..dc7549891 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 8/9] 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 587b442c7..922d94b1d 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 9/9] 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 e73f69387..a84b3768f 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 fc6fe1218..4d6c04612 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 0246fa71f..cc3312820 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 3f9bb7ef7..3738f9763 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 dc7549891..71583fbad 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 c78212430..d44dc8e9a 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 80d588baf..7e8dfcd38 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; }