diff --git a/lib/main/STM32_USB_Device_Library/Core/src/usbd_core.c b/lib/main/STM32_USB_Device_Library/Core/src/usbd_core.c index 978d7bd3bd..0613f5018a 100644 --- a/lib/main/STM32_USB_Device_Library/Core/src/usbd_core.c +++ b/lib/main/STM32_USB_Device_Library/Core/src/usbd_core.c @@ -98,19 +98,22 @@ __IO USB_OTG_DCTL_TypeDef SET_TEST_MODE; USBD_DCD_INT_cb_TypeDef USBD_DCD_INT_cb = { - USBD_DataOutStage, - USBD_DataInStage, - USBD_SetupStage, - USBD_SOF, - USBD_Reset, - USBD_Suspend, - USBD_Resume, - USBD_IsoINIncomplete, - USBD_IsoOUTIncomplete, + USBD_DataOutStage, + USBD_DataInStage, + USBD_SetupStage, + USBD_SOF, + USBD_Reset, + USBD_Suspend, + USBD_Resume, + USBD_IsoINIncomplete, + USBD_IsoOUTIncomplete, #ifdef VBUS_SENSING_ENABLED - USBD_DevConnected, - USBD_DevDisconnected, -#endif + USBD_DevConnected, + USBD_DevDisconnected, +#else + NULL, + NULL, +#endif }; USBD_DCD_INT_cb_TypeDef *USBD_DCD_INT_fops = &USBD_DCD_INT_cb; diff --git a/lib/main/STM32_USB_OTG_Driver/inc/usb_dcd_int.h b/lib/main/STM32_USB_OTG_Driver/inc/usb_dcd_int.h index e5f22bfa09..cd942e2b21 100644 --- a/lib/main/STM32_USB_OTG_Driver/inc/usb_dcd_int.h +++ b/lib/main/STM32_USB_OTG_Driver/inc/usb_dcd_int.h @@ -63,7 +63,7 @@ typedef struct _USBD_DCD_INT uint8_t (* DevConnected) (USB_OTG_CORE_HANDLE *pdev); uint8_t (* DevDisconnected) (USB_OTG_CORE_HANDLE *pdev); -}USBD_DCD_INT_cb_TypeDef; +} USBD_DCD_INT_cb_TypeDef; extern USBD_DCD_INT_cb_TypeDef *USBD_DCD_INT_fops; /** diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6d6db11a9d..d7d56736dd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -72,11 +72,13 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using -void setTargetPidLooptime(uint8_t pidProcessDenom) { - targetPidLooptime = targetLooptime * pidProcessDenom; +void setTargetPidLooptime(uint8_t pidProcessDenom) +{ + targetPidLooptime = targetLooptime * pidProcessDenom; } -uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) { +uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) +{ uint16_t dynamicKi; uint16_t resetRate; @@ -101,7 +103,8 @@ void pidResetErrorGyroState(void) } } -float getdT (void) { +float getdT (void) +{ static float dT; if (!dT) dT = (float)targetPidLooptime * 0.000001f; @@ -117,7 +120,7 @@ static filterStatePt1_t yawFilterState; static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { - float RateError, gyroRate, RateErrorSmooth; + float RateError = 0, gyroRate = 0, RateErrorSmooth = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; float delta; @@ -257,7 +260,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin int axis; int32_t PTerm, ITerm, DTerm, delta; static int32_t lastRateError[3]; - int32_t AngleRateTmp, AngleRateTmpSmooth, RateError, gyroRate, RateErrorSmooth; + int32_t AngleRateTmp = 0, AngleRateTmpSmooth = 0, RateError = 0, gyroRate = 0, RateErrorSmooth = 0; int8_t horizonLevelStrength = 100; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 62bd11dcae..4e31f76cef 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -148,13 +148,13 @@ void setGyroSamplingSpeed(uint16_t looptime) { gyroSampleRate = 125; maxDivider = 8; masterConfig.pid_process_denom = 1; - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LUX_FLOAT) { + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) { masterConfig.pid_process_denom = 2; } if (looptime < 250) { masterConfig.pid_process_denom = 4; } else if (looptime < 375) { - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LUX_FLOAT) { + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) { masterConfig.pid_process_denom = 3; } else { masterConfig.pid_process_denom = 2;