From c113ca81e0dea8b5480ce0f1024da884ff111131 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Mon, 31 Oct 2022 20:20:16 +0800 Subject: [PATCH] finish the beta version of dji_motor --- .vscode/settings.json | 4 +- .../Inc/stm32f4xx_hal_gpio.h | 464 +++++++++--------- Makefile | 1 - Src/main.c | 111 +++-- bsp/bsp_can.c | 4 +- bsp/bsp_can.h | 2 +- bsp/struct_typedef.h | 4 - modules/algorithm/controller.c | 348 +------------ modules/algorithm/controller.h | 156 ++---- modules/algorithm/user_lib.c | 204 +------- modules/algorithm/user_lib.h | 41 +- modules/imu/ins_task.c | 16 +- modules/motor/HT04.c | 6 +- modules/motor/LK9025.c | 2 +- modules/motor/dji_motor.c | 220 ++++++--- modules/motor/dji_motor.h | 43 +- modules/motor/motor_def.h | 85 ++-- modules/motor/motor_task.c | 31 +- modules/motor/motor_task.h | 5 - modules/remote/remote_control.h | 8 +- 20 files changed, 618 insertions(+), 1137 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 624a873..2c3ce52 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -36,6 +36,8 @@ "functional": "c", "stdexcept": "c", "tuple": "c", - "typeinfo": "c" + "typeinfo": "c", + "chrono": "c", + "complex": "c" } } \ No newline at end of file diff --git a/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h b/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h index 5f3d749..77a8d95 100644 --- a/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h +++ b/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h @@ -1,325 +1,325 @@ /** - ****************************************************************************** - * @file stm32f4xx_hal_gpio.h - * @author MCD Application Team - * @brief Header file of GPIO HAL module. - ****************************************************************************** - * @attention - * - * Copyright (c) 2017 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file stm32f4xx_hal_gpio.h + * @author MCD Application Team + * @brief Header file of GPIO HAL module. + ****************************************************************************** + * @attention + * + * Copyright (c) 2017 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F4xx_HAL_GPIO_H #define __STM32F4xx_HAL_GPIO_H #ifdef __cplusplus - extern "C" { +extern "C" +{ #endif /* Includes ------------------------------------------------------------------*/ #include "stm32f4xx_hal_def.h" -/** @addtogroup STM32F4xx_HAL_Driver - * @{ - */ + /** @addtogroup STM32F4xx_HAL_Driver + * @{ + */ -/** @addtogroup GPIO - * @{ - */ + /** @addtogroup GPIO + * @{ + */ -/* Exported types ------------------------------------------------------------*/ -/** @defgroup GPIO_Exported_Types GPIO Exported Types - * @{ - */ + /* Exported types ------------------------------------------------------------*/ + /** @defgroup GPIO_Exported_Types GPIO Exported Types + * @{ + */ -/** - * @brief GPIO Init structure definition - */ -typedef struct -{ - uint32_t Pin; /*!< Specifies the GPIO pins to be configured. - This parameter can be any value of @ref GPIO_pins_define */ + /** + * @brief GPIO Init structure definition + */ + typedef struct + { + uint32_t Pin; /*!< Specifies the GPIO pins to be configured. + This parameter can be any value of @ref GPIO_pins_define */ - uint32_t Mode; /*!< Specifies the operating mode for the selected pins. - This parameter can be a value of @ref GPIO_mode_define */ + uint32_t Mode; /*!< Specifies the operating mode for the selected pins. + This parameter can be a value of @ref GPIO_mode_define */ - uint32_t Pull; /*!< Specifies the Pull-up or Pull-Down activation for the selected pins. - This parameter can be a value of @ref GPIO_pull_define */ + uint32_t Pull; /*!< Specifies the Pull-up or Pull-Down activation for the selected pins. + This parameter can be a value of @ref GPIO_pull_define */ - uint32_t Speed; /*!< Specifies the speed for the selected pins. - This parameter can be a value of @ref GPIO_speed_define */ + uint32_t Speed; /*!< Specifies the speed for the selected pins. + This parameter can be a value of @ref GPIO_speed_define */ - uint32_t Alternate; /*!< Peripheral to be connected to the selected pins. - This parameter can be a value of @ref GPIO_Alternate_function_selection */ -}GPIO_InitTypeDef; + uint32_t Alternate; /*!< Peripheral to be connected to the selected pins. + This parameter can be a value of @ref GPIO_Alternate_function_selection */ + } GPIO_InitTypeDef; -/** - * @brief GPIO Bit SET and Bit RESET enumeration - */ -typedef enum -{ - GPIO_PIN_RESET = 0, - GPIO_PIN_SET -}GPIO_PinState; + /** + * @brief GPIO Bit SET and Bit RESET enumeration + */ + typedef enum + { + GPIO_PIN_RESET = 0, + GPIO_PIN_SET + } GPIO_PinState; /** - * @} - */ + * @} + */ /* Exported constants --------------------------------------------------------*/ /** @defgroup GPIO_Exported_Constants GPIO Exported Constants - * @{ - */ + * @{ + */ /** @defgroup GPIO_pins_define GPIO pins define - * @{ - */ -#define GPIO_PIN_0 ((uint16_t)0x0001) /* Pin 0 selected */ -#define GPIO_PIN_1 ((uint16_t)0x0002) /* Pin 1 selected */ -#define GPIO_PIN_2 ((uint16_t)0x0004) /* Pin 2 selected */ -#define GPIO_PIN_3 ((uint16_t)0x0008) /* Pin 3 selected */ -#define GPIO_PIN_4 ((uint16_t)0x0010) /* Pin 4 selected */ -#define GPIO_PIN_5 ((uint16_t)0x0020) /* Pin 5 selected */ -#define GPIO_PIN_6 ((uint16_t)0x0040) /* Pin 6 selected */ -#define GPIO_PIN_7 ((uint16_t)0x0080) /* Pin 7 selected */ -#define GPIO_PIN_8 ((uint16_t)0x0100) /* Pin 8 selected */ -#define GPIO_PIN_9 ((uint16_t)0x0200) /* Pin 9 selected */ -#define GPIO_PIN_10 ((uint16_t)0x0400) /* Pin 10 selected */ -#define GPIO_PIN_11 ((uint16_t)0x0800) /* Pin 11 selected */ -#define GPIO_PIN_12 ((uint16_t)0x1000) /* Pin 12 selected */ -#define GPIO_PIN_13 ((uint16_t)0x2000) /* Pin 13 selected */ -#define GPIO_PIN_14 ((uint16_t)0x4000) /* Pin 14 selected */ -#define GPIO_PIN_15 ((uint16_t)0x8000) /* Pin 15 selected */ -#define GPIO_PIN_All ((uint16_t)0xFFFF) /* All pins selected */ + * @{ + */ +#define GPIO_PIN_0 ((uint16_t)0x0001) /* Pin 0 selected */ +#define GPIO_PIN_1 ((uint16_t)0x0002) /* Pin 1 selected */ +#define GPIO_PIN_2 ((uint16_t)0x0004) /* Pin 2 selected */ +#define GPIO_PIN_3 ((uint16_t)0x0008) /* Pin 3 selected */ +#define GPIO_PIN_4 ((uint16_t)0x0010) /* Pin 4 selected */ +#define GPIO_PIN_5 ((uint16_t)0x0020) /* Pin 5 selected */ +#define GPIO_PIN_6 ((uint16_t)0x0040) /* Pin 6 selected */ +#define GPIO_PIN_7 ((uint16_t)0x0080) /* Pin 7 selected */ +#define GPIO_PIN_8 ((uint16_t)0x0100) /* Pin 8 selected */ +#define GPIO_PIN_9 ((uint16_t)0x0200) /* Pin 9 selected */ +#define GPIO_PIN_10 ((uint16_t)0x0400) /* Pin 10 selected */ +#define GPIO_PIN_11 ((uint16_t)0x0800) /* Pin 11 selected */ +#define GPIO_PIN_12 ((uint16_t)0x1000) /* Pin 12 selected */ +#define GPIO_PIN_13 ((uint16_t)0x2000) /* Pin 13 selected */ +#define GPIO_PIN_14 ((uint16_t)0x4000) /* Pin 14 selected */ +#define GPIO_PIN_15 ((uint16_t)0x8000) /* Pin 15 selected */ +#define GPIO_PIN_All ((uint16_t)0xFFFF) /* All pins selected */ -#define GPIO_PIN_MASK 0x0000FFFFU /* PIN mask for assert test */ +#define GPIO_PIN_MASK 0x0000FFFFU /* PIN mask for assert test */ /** - * @} - */ + * @} + */ /** @defgroup GPIO_mode_define GPIO mode define - * @brief GPIO Configuration Mode - * Elements values convention: 0x00WX00YZ - * - W : EXTI trigger detection on 3 bits - * - X : EXTI mode (IT or Event) on 2 bits - * - Y : Output type (Push Pull or Open Drain) on 1 bit - * - Z : GPIO mode (Input, Output, Alternate or Analog) on 2 bits - * @{ - */ -#define GPIO_MODE_INPUT MODE_INPUT /*!< Input Floating Mode */ -#define GPIO_MODE_OUTPUT_PP (MODE_OUTPUT | OUTPUT_PP) /*!< Output Push Pull Mode */ -#define GPIO_MODE_OUTPUT_OD (MODE_OUTPUT | OUTPUT_OD) /*!< Output Open Drain Mode */ -#define GPIO_MODE_AF_PP (MODE_AF | OUTPUT_PP) /*!< Alternate Function Push Pull Mode */ -#define GPIO_MODE_AF_OD (MODE_AF | OUTPUT_OD) /*!< Alternate Function Open Drain Mode */ + * @brief GPIO Configuration Mode + * Elements values convention: 0x00WX00YZ + * - W : EXTI trigger detection on 3 bits + * - X : EXTI mode (IT or Event) on 2 bits + * - Y : Output type (Push Pull or Open Drain) on 1 bit + * - Z : GPIO mode (Input, Output, Alternate or Analog) on 2 bits + * @{ + */ +#define GPIO_MODE_INPUT MODE_INPUT /*!< Input Floating Mode */ +#define GPIO_MODE_OUTPUT_PP (MODE_OUTPUT | OUTPUT_PP) /*!< Output Push Pull Mode */ +#define GPIO_MODE_OUTPUT_OD (MODE_OUTPUT | OUTPUT_OD) /*!< Output Open Drain Mode */ +#define GPIO_MODE_AF_PP (MODE_AF | OUTPUT_PP) /*!< Alternate Function Push Pull Mode */ +#define GPIO_MODE_AF_OD (MODE_AF | OUTPUT_OD) /*!< Alternate Function Open Drain Mode */ -#define GPIO_MODE_ANALOG MODE_ANALOG /*!< Analog Mode */ - -#define GPIO_MODE_IT_RISING (MODE_INPUT | EXTI_IT | TRIGGER_RISING) /*!< External Interrupt Mode with Rising edge trigger detection */ -#define GPIO_MODE_IT_FALLING (MODE_INPUT | EXTI_IT | TRIGGER_FALLING) /*!< External Interrupt Mode with Falling edge trigger detection */ -#define GPIO_MODE_IT_RISING_FALLING (MODE_INPUT | EXTI_IT | TRIGGER_RISING | TRIGGER_FALLING) /*!< External Interrupt Mode with Rising/Falling edge trigger detection */ - -#define GPIO_MODE_EVT_RISING (MODE_INPUT | EXTI_EVT | TRIGGER_RISING) /*!< External Event Mode with Rising edge trigger detection */ -#define GPIO_MODE_EVT_FALLING (MODE_INPUT | EXTI_EVT | TRIGGER_FALLING) /*!< External Event Mode with Falling edge trigger detection */ -#define GPIO_MODE_EVT_RISING_FALLING (MODE_INPUT | EXTI_EVT | TRIGGER_RISING | TRIGGER_FALLING) /*!< External Event Mode with Rising/Falling edge trigger detection */ +#define GPIO_MODE_ANALOG MODE_ANALOG /*!< Analog Mode */ + +#define GPIO_MODE_IT_RISING (MODE_INPUT | EXTI_IT | TRIGGER_RISING) /*!< External Interrupt Mode with Rising edge trigger detection */ +#define GPIO_MODE_IT_FALLING (MODE_INPUT | EXTI_IT | TRIGGER_FALLING) /*!< External Interrupt Mode with Falling edge trigger detection */ +#define GPIO_MODE_IT_RISING_FALLING (MODE_INPUT | EXTI_IT | TRIGGER_RISING | TRIGGER_FALLING) /*!< External Interrupt Mode with Rising/Falling edge trigger detection */ + +#define GPIO_MODE_EVT_RISING (MODE_INPUT | EXTI_EVT | TRIGGER_RISING) /*!< External Event Mode with Rising edge trigger detection */ +#define GPIO_MODE_EVT_FALLING (MODE_INPUT | EXTI_EVT | TRIGGER_FALLING) /*!< External Event Mode with Falling edge trigger detection */ +#define GPIO_MODE_EVT_RISING_FALLING (MODE_INPUT | EXTI_EVT | TRIGGER_RISING | TRIGGER_FALLING) /*!< External Event Mode with Rising/Falling edge trigger detection */ /** - * @} - */ + * @} + */ /** @defgroup GPIO_speed_define GPIO speed define - * @brief GPIO Output Maximum frequency - * @{ - */ -#define GPIO_SPEED_FREQ_LOW 0x00000000U /*!< IO works at 2 MHz, please refer to the product datasheet */ -#define GPIO_SPEED_FREQ_MEDIUM 0x00000001U /*!< range 12,5 MHz to 50 MHz, please refer to the product datasheet */ -#define GPIO_SPEED_FREQ_HIGH 0x00000002U /*!< range 25 MHz to 100 MHz, please refer to the product datasheet */ -#define GPIO_SPEED_FREQ_VERY_HIGH 0x00000003U /*!< range 50 MHz to 200 MHz, please refer to the product datasheet */ -/** - * @} - */ + * @brief GPIO Output Maximum frequency + * @{ + */ +#define GPIO_SPEED_FREQ_LOW 0x00000000U /*!< IO works at 2 MHz, please refer to the product datasheet */ +#define GPIO_SPEED_FREQ_MEDIUM 0x00000001U /*!< range 12,5 MHz to 50 MHz, please refer to the product datasheet */ +#define GPIO_SPEED_FREQ_HIGH 0x00000002U /*!< range 25 MHz to 100 MHz, please refer to the product datasheet */ +#define GPIO_SPEED_FREQ_VERY_HIGH 0x00000003U /*!< range 50 MHz to 200 MHz, please refer to the product datasheet */ + /** + * @} + */ - /** @defgroup GPIO_pull_define GPIO pull define + /** @defgroup GPIO_pull_define GPIO pull define * @brief GPIO Pull-Up or Pull-Down Activation * @{ - */ -#define GPIO_NOPULL 0x00000000U /*!< No Pull-up or Pull-down activation */ -#define GPIO_PULLUP 0x00000001U /*!< Pull-up activation */ -#define GPIO_PULLDOWN 0x00000002U /*!< Pull-down activation */ + */ +#define GPIO_NOPULL 0x00000000U /*!< No Pull-up or Pull-down activation */ +#define GPIO_PULLUP 0x00000001U /*!< Pull-up activation */ +#define GPIO_PULLDOWN 0x00000002U /*!< Pull-down activation */ /** - * @} - */ - + * @} + */ + /** - * @} - */ + * @} + */ /* Exported macro ------------------------------------------------------------*/ /** @defgroup GPIO_Exported_Macros GPIO Exported Macros - * @{ - */ + * @{ + */ /** - * @brief Checks whether the specified EXTI line flag is set or not. - * @param __EXTI_LINE__ specifies the EXTI line flag to check. - * This parameter can be GPIO_PIN_x where x can be(0..15) - * @retval The new state of __EXTI_LINE__ (SET or RESET). - */ + * @brief Checks whether the specified EXTI line flag is set or not. + * @param __EXTI_LINE__ specifies the EXTI line flag to check. + * This parameter can be GPIO_PIN_x where x can be(0..15) + * @retval The new state of __EXTI_LINE__ (SET or RESET). + */ #define __HAL_GPIO_EXTI_GET_FLAG(__EXTI_LINE__) (EXTI->PR & (__EXTI_LINE__)) /** - * @brief Clears the EXTI's line pending flags. - * @param __EXTI_LINE__ specifies the EXTI lines flags to clear. - * This parameter can be any combination of GPIO_PIN_x where x can be (0..15) - * @retval None - */ + * @brief Clears the EXTI's line pending flags. + * @param __EXTI_LINE__ specifies the EXTI lines flags to clear. + * This parameter can be any combination of GPIO_PIN_x where x can be (0..15) + * @retval None + */ #define __HAL_GPIO_EXTI_CLEAR_FLAG(__EXTI_LINE__) (EXTI->PR = (__EXTI_LINE__)) /** - * @brief Checks whether the specified EXTI line is asserted or not. - * @param __EXTI_LINE__ specifies the EXTI line to check. - * This parameter can be GPIO_PIN_x where x can be(0..15) - * @retval The new state of __EXTI_LINE__ (SET or RESET). - */ + * @brief Checks whether the specified EXTI line is asserted or not. + * @param __EXTI_LINE__ specifies the EXTI line to check. + * This parameter can be GPIO_PIN_x where x can be(0..15) + * @retval The new state of __EXTI_LINE__ (SET or RESET). + */ #define __HAL_GPIO_EXTI_GET_IT(__EXTI_LINE__) (EXTI->PR & (__EXTI_LINE__)) /** - * @brief Clears the EXTI's line pending bits. - * @param __EXTI_LINE__ specifies the EXTI lines to clear. - * This parameter can be any combination of GPIO_PIN_x where x can be (0..15) - * @retval None - */ + * @brief Clears the EXTI's line pending bits. + * @param __EXTI_LINE__ specifies the EXTI lines to clear. + * This parameter can be any combination of GPIO_PIN_x where x can be (0..15) + * @retval None + */ #define __HAL_GPIO_EXTI_CLEAR_IT(__EXTI_LINE__) (EXTI->PR = (__EXTI_LINE__)) /** - * @brief Generates a Software interrupt on selected EXTI line. - * @param __EXTI_LINE__ specifies the EXTI line to check. - * This parameter can be GPIO_PIN_x where x can be(0..15) - * @retval None - */ + * @brief Generates a Software interrupt on selected EXTI line. + * @param __EXTI_LINE__ specifies the EXTI line to check. + * This parameter can be GPIO_PIN_x where x can be(0..15) + * @retval None + */ #define __HAL_GPIO_EXTI_GENERATE_SWIT(__EXTI_LINE__) (EXTI->SWIER |= (__EXTI_LINE__)) /** - * @} - */ + * @} + */ /* Include GPIO HAL Extension module */ #include "stm32f4xx_hal_gpio_ex.h" -/* Exported functions --------------------------------------------------------*/ -/** @addtogroup GPIO_Exported_Functions - * @{ - */ + /* Exported functions --------------------------------------------------------*/ + /** @addtogroup GPIO_Exported_Functions + * @{ + */ -/** @addtogroup GPIO_Exported_Functions_Group1 - * @{ - */ -/* Initialization and de-initialization functions *****************************/ -void HAL_GPIO_Init(GPIO_TypeDef *GPIOx, GPIO_InitTypeDef *GPIO_Init); -void HAL_GPIO_DeInit(GPIO_TypeDef *GPIOx, uint32_t GPIO_Pin); -/** - * @} - */ + /** @addtogroup GPIO_Exported_Functions_Group1 + * @{ + */ + /* Initialization and de-initialization functions *****************************/ + void HAL_GPIO_Init(GPIO_TypeDef *GPIOx, GPIO_InitTypeDef *GPIO_Init); + void HAL_GPIO_DeInit(GPIO_TypeDef *GPIOx, uint32_t GPIO_Pin); + /** + * @} + */ -/** @addtogroup GPIO_Exported_Functions_Group2 - * @{ - */ -/* IO operation functions *****************************************************/ -GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); -void HAL_GPIO_WritePin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState); -void HAL_GPIO_TogglePin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); -HAL_StatusTypeDef HAL_GPIO_LockPin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); -void HAL_GPIO_EXTI_IRQHandler(uint16_t GPIO_Pin); -void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin); + /** @addtogroup GPIO_Exported_Functions_Group2 + * @{ + */ + /* IO operation functions *****************************************************/ + GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin); + void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState); + void HAL_GPIO_TogglePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin); + HAL_StatusTypeDef HAL_GPIO_LockPin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin); + void HAL_GPIO_EXTI_IRQHandler(uint16_t GPIO_Pin); + void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin); /** - * @} - */ + * @} + */ /** - * @} - */ + * @} + */ /* Private types -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ /* Private constants ---------------------------------------------------------*/ /** @defgroup GPIO_Private_Constants GPIO Private Constants - * @{ - */ -#define GPIO_MODE_Pos 0U -#define GPIO_MODE (0x3UL << GPIO_MODE_Pos) -#define MODE_INPUT (0x0UL << GPIO_MODE_Pos) -#define MODE_OUTPUT (0x1UL << GPIO_MODE_Pos) -#define MODE_AF (0x2UL << GPIO_MODE_Pos) -#define MODE_ANALOG (0x3UL << GPIO_MODE_Pos) -#define OUTPUT_TYPE_Pos 4U -#define OUTPUT_TYPE (0x1UL << OUTPUT_TYPE_Pos) -#define OUTPUT_PP (0x0UL << OUTPUT_TYPE_Pos) -#define OUTPUT_OD (0x1UL << OUTPUT_TYPE_Pos) -#define EXTI_MODE_Pos 16U -#define EXTI_MODE (0x3UL << EXTI_MODE_Pos) -#define EXTI_IT (0x1UL << EXTI_MODE_Pos) -#define EXTI_EVT (0x2UL << EXTI_MODE_Pos) -#define TRIGGER_MODE_Pos 20U -#define TRIGGER_MODE (0x7UL << TRIGGER_MODE_Pos) -#define TRIGGER_RISING (0x1UL << TRIGGER_MODE_Pos) -#define TRIGGER_FALLING (0x2UL << TRIGGER_MODE_Pos) + * @{ + */ +#define GPIO_MODE_Pos 0U +#define GPIO_MODE (0x3UL << GPIO_MODE_Pos) +#define MODE_INPUT (0x0UL << GPIO_MODE_Pos) +#define MODE_OUTPUT (0x1UL << GPIO_MODE_Pos) +#define MODE_AF (0x2UL << GPIO_MODE_Pos) +#define MODE_ANALOG (0x3UL << GPIO_MODE_Pos) +#define OUTPUT_TYPE_Pos 4U +#define OUTPUT_TYPE (0x1UL << OUTPUT_TYPE_Pos) +#define OUTPUT_PP (0x0UL << OUTPUT_TYPE_Pos) +#define OUTPUT_OD (0x1UL << OUTPUT_TYPE_Pos) +#define EXTI_MODE_Pos 16U +#define EXTI_MODE (0x3UL << EXTI_MODE_Pos) +#define EXTI_IT (0x1UL << EXTI_MODE_Pos) +#define EXTI_EVT (0x2UL << EXTI_MODE_Pos) +#define TRIGGER_MODE_Pos 20U +#define TRIGGER_MODE (0x7UL << TRIGGER_MODE_Pos) +#define TRIGGER_RISING (0x1UL << TRIGGER_MODE_Pos) +#define TRIGGER_FALLING (0x2UL << TRIGGER_MODE_Pos) /** - * @} - */ + * @} + */ /* Private macros ------------------------------------------------------------*/ /** @defgroup GPIO_Private_Macros GPIO Private Macros - * @{ - */ + * @{ + */ #define IS_GPIO_PIN_ACTION(ACTION) (((ACTION) == GPIO_PIN_RESET) || ((ACTION) == GPIO_PIN_SET)) -#define IS_GPIO_PIN(PIN) (((((uint32_t)PIN) & GPIO_PIN_MASK ) != 0x00U) && ((((uint32_t)PIN) & ~GPIO_PIN_MASK) == 0x00U)) -#define IS_GPIO_MODE(MODE) (((MODE) == GPIO_MODE_INPUT) ||\ - ((MODE) == GPIO_MODE_OUTPUT_PP) ||\ - ((MODE) == GPIO_MODE_OUTPUT_OD) ||\ - ((MODE) == GPIO_MODE_AF_PP) ||\ - ((MODE) == GPIO_MODE_AF_OD) ||\ - ((MODE) == GPIO_MODE_IT_RISING) ||\ - ((MODE) == GPIO_MODE_IT_FALLING) ||\ - ((MODE) == GPIO_MODE_IT_RISING_FALLING) ||\ - ((MODE) == GPIO_MODE_EVT_RISING) ||\ - ((MODE) == GPIO_MODE_EVT_FALLING) ||\ - ((MODE) == GPIO_MODE_EVT_RISING_FALLING) ||\ +#define IS_GPIO_PIN(PIN) (((((uint32_t)PIN) & GPIO_PIN_MASK) != 0x00U) && ((((uint32_t)PIN) & ~GPIO_PIN_MASK) == 0x00U)) +#define IS_GPIO_MODE(MODE) (((MODE) == GPIO_MODE_INPUT) || \ + ((MODE) == GPIO_MODE_OUTPUT_PP) || \ + ((MODE) == GPIO_MODE_OUTPUT_OD) || \ + ((MODE) == GPIO_MODE_AF_PP) || \ + ((MODE) == GPIO_MODE_AF_OD) || \ + ((MODE) == GPIO_MODE_IT_RISING) || \ + ((MODE) == GPIO_MODE_IT_FALLING) || \ + ((MODE) == GPIO_MODE_IT_RISING_FALLING) || \ + ((MODE) == GPIO_MODE_EVT_RISING) || \ + ((MODE) == GPIO_MODE_EVT_FALLING) || \ + ((MODE) == GPIO_MODE_EVT_RISING_FALLING) || \ ((MODE) == GPIO_MODE_ANALOG)) -#define IS_GPIO_SPEED(SPEED) (((SPEED) == GPIO_SPEED_FREQ_LOW) || ((SPEED) == GPIO_SPEED_FREQ_MEDIUM) || \ - ((SPEED) == GPIO_SPEED_FREQ_HIGH) || ((SPEED) == GPIO_SPEED_FREQ_VERY_HIGH)) +#define IS_GPIO_SPEED(SPEED) (((SPEED) == GPIO_SPEED_FREQ_LOW) || ((SPEED) == GPIO_SPEED_FREQ_MEDIUM) || \ + ((SPEED_LOOP) == GPIO_SPEED_FREQ_HIGH) || ((SPEED_LOOP) == GPIO_SPEED_FREQ_VERY_HIGH)) #define IS_GPIO_PULL(PULL) (((PULL) == GPIO_NOPULL) || ((PULL) == GPIO_PULLUP) || \ ((PULL) == GPIO_PULLDOWN)) -/** - * @} - */ + /** + * @} + */ -/* Private functions ---------------------------------------------------------*/ -/** @defgroup GPIO_Private_Functions GPIO Private Functions - * @{ - */ + /* Private functions ---------------------------------------------------------*/ + /** @defgroup GPIO_Private_Functions GPIO Private Functions + * @{ + */ -/** - * @} - */ + /** + * @} + */ -/** - * @} - */ + /** + * @} + */ -/** - * @} - */ + /** + * @} + */ #ifdef __cplusplus } #endif #endif /* __STM32F4xx_HAL_GPIO_H */ - diff --git a/Makefile b/Makefile index 65ad66d..6f800fc 100644 --- a/Makefile +++ b/Makefile @@ -107,7 +107,6 @@ bsp/bsp_log.c \ modules/algorithm/controller.c \ modules/algorithm/kalman_filter.c \ modules/algorithm/QuaternionEKF.c \ -modules/algorithm/user_lib.c\ \ modules/imu/BMI088driver.c \ modules/imu/BMI088Middleware.c \ modules/imu/ins_task.c \ diff --git a/Src/main.c b/Src/main.c index b278330..4b4ed16 100644 --- a/Src/main.c +++ b/Src/main.c @@ -1,20 +1,20 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * @file : main.c - * @brief : Main program body - ****************************************************************************** - * @attention - * - * Copyright (c) 2022 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" @@ -38,6 +38,7 @@ #include "can.h" #include "LK9025.h" #include "dji_motor.h" +#include "motor_task.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -73,9 +74,9 @@ void MX_FREERTOS_Init(void); /* USER CODE END 0 */ /** - * @brief The application entry point. - * @retval int - */ + * @brief The application entry point. + * @retval int + */ int main(void) { /* USER CODE BEGIN 1 */ @@ -115,13 +116,39 @@ int main(void) MX_USART1_UART_Init(); MX_USART6_UART_Init(); /* USER CODE BEGIN 2 */ + RC_init(&huart3); + can_instance_config can_config = {.can_handle = &hcan1, + .tx_id = 1}; + Motor_Control_Setting_s motor_config = {.angle_feedback_source = MOTOR_FEED, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .speed_feedback_source = MOTOR_FEED, + .reverse_flag = MOTOR_DIRECTION_NORMAL}; + Motor_Controller_Init_s controller_init = {.current_PID = { + .Improve = 0, + .Kp = 1, + .Ki = 0, + .Kd = 0, + .DeadBand = 0, + .MaxOut = 4000, + }, + .speed_PID = { + .Improve = 0, + .Kp = 1, + .Ki = 0, + .Kd = 0, + .DeadBand = 0, + .MaxOut = 4000, + }}; + Motor_Type_e type = M3508; + DWT_Init(168); + dji_motor_instance *djimotor = DJIMotorInit(can_config, motor_config, controller_init, type); /* USER CODE END 2 */ /* Call init function for freertos objects (in freertos.c) */ - MX_FREERTOS_Init(); + // MX_FREERTOS_Init(); - /* Start scheduler */ - osKernelStart(); + // /* Start scheduler */ + // osKernelStart(); /* We should never get here as control is now taken by the scheduler */ /* Infinite loop */ @@ -129,29 +156,32 @@ int main(void) while (1) { /* USER CODE END WHILE */ - + + DJIMotorSetRef(djimotor, get_remote_control_point()->rc.ch[0]); + MotorControlTask(); + HAL_Delay(20); /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ } /** - * @brief System Clock Configuration - * @retval None - */ + * @brief System Clock Configuration + * @retval None + */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Configure the main internal regulator output voltage - */ + */ __HAL_RCC_PWR_CLK_ENABLE(); __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); /** Initializes the RCC Oscillators according to the specified parameters - * in the RCC_OscInitTypeDef structure. - */ + * in the RCC_OscInitTypeDef structure. + */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; @@ -166,9 +196,8 @@ void SystemClock_Config(void) } /** Initializes the CPU, AHB and APB buses clocks - */ - RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK - |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; @@ -185,9 +214,9 @@ void SystemClock_Config(void) /* USER CODE END 4 */ /** - * @brief This function is executed in case of error occurrence. - * @retval None - */ + * @brief This function is executed in case of error occurrence. + * @retval None + */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ @@ -199,14 +228,14 @@ void Error_Handler(void) /* USER CODE END Error_Handler_Debug */ } -#ifdef USE_FULL_ASSERT +#ifdef USE_FULL_ASSERT /** - * @brief Reports the name of the source file and the source line number - * where the assert_param error has occurred. - * @param file: pointer to the source file name - * @param line: assert_param error line source number - * @retval None - */ + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index 23c4579..0a2fd58 100644 --- a/bsp/bsp_can.c +++ b/bsp/bsp_can.c @@ -57,14 +57,14 @@ static void CANServiceInit() /* -----------------------two extern callable function -----------------------*/ -can_instance *CANRegister(can_instance_config config) +void CANRegister(can_instance* ins,can_instance_config config) { static uint8_t idx; if (!idx) { CANServiceInit(); } - instance[idx] = (can_instance *)malloc(sizeof(can_instance)); + instance[idx]=ins; instance[idx]->txconf.StdId = config.tx_id; instance[idx]->txconf.IDE = CAN_ID_STD; diff --git a/bsp/bsp_can.h b/bsp/bsp_can.h index cbb25d3..5627a2d 100644 --- a/bsp/bsp_can.h +++ b/bsp/bsp_can.h @@ -52,7 +52,7 @@ void CANTransmit(can_instance* _instance); * @param config init config * @return can_instance* can instance owned by module */ -can_instance* CANRegister(can_instance_config config); +void CANRegister(can_instance* instance, can_instance_config config); #endif diff --git a/bsp/struct_typedef.h b/bsp/struct_typedef.h index ea7d46d..e80ba2b 100644 --- a/bsp/struct_typedef.h +++ b/bsp/struct_typedef.h @@ -1,10 +1,6 @@ #ifndef STRUCT_TYPEDEF_H #define STRUCT_TYPEDEF_H -#ifndef __packed -#define __packed -#endif - typedef signed char int8_t; typedef signed short int int16_t; diff --git a/modules/algorithm/controller.c b/modules/algorithm/controller.c index 4fc9e4a..1a4fdd2 100644 --- a/modules/algorithm/controller.c +++ b/modules/algorithm/controller.c @@ -1,16 +1,6 @@ -/** - ****************************************************************************** - * @file controller.c - * @author Wang Hongxi - * @version V1.1.3 - * @date 2021/7/3 - * @brief DWT定时器用于计算控制周期 OLS用于提取信号微分 - ****************************************************************************** - * @attention - * - ****************************************************************************** - */ + #include "controller.h" +#include /******************************* PID CONTROL *********************************/ // PID优化环节函数声明 @@ -25,67 +15,23 @@ static void f_Proportion_Limit(PID_t *pid); static void f_PID_ErrorHandle(PID_t *pid); /** - * @brief PID初始化 PID initialize - * @param[in] PID结构体 PID structure - * @param[in] 略 - * @retval 返回空 null + * @brief + * + * @param pid + * @param config */ -void PID_Init( - PID_t *pid, - float max_out, - float intergral_limit, - float deadband, - - float kp, - float Ki, - float Kd, - - float A, - float B, - - float output_lpf_rc, - float derivative_lpf_rc, - - uint16_t ols_order, - - uint8_t improve) +void PID_Init(PID_t* pid,PID_Init_config_s *config) { - pid->DeadBand = deadband; - pid->IntegralLimit = intergral_limit; - pid->MaxOut = max_out; - pid->Ref = 0; + memcpy(pid, config, sizeof(PID_Init_config_s)); + memset(&pid->Measure,0,sizeof(PID_t)-sizeof(PID_Init_config_s)); + + // // DWT定时器计数变量清零 + // // reset DWT Timer count counter + // pid->DWT_CNT = 0; - pid->Kp = kp; - pid->Ki = Ki; - pid->Kd = Kd; - pid->ITerm = 0; - - // 变速积分参数 - // coefficient of changing integration rate - pid->CoefA = A; - pid->CoefB = B; - - pid->Output_LPF_RC = output_lpf_rc; - - pid->Derivative_LPF_RC = derivative_lpf_rc; - - // 最小二乘提取信号微分初始化 - // differential signal is distilled by OLS - pid->OLS_Order = ols_order; - OLS_Init(&pid->OLS, ols_order); - - // DWT定时器计数变量清零 - // reset DWT Timer count counter - pid->DWT_CNT = 0; - - // 设置PID优化环节 - pid->Improve = improve; - - // 设置PID异常处理 目前仅包含电机堵转保护 - pid->ERRORHandler.ERRORCount = 0; - pid->ERRORHandler.ERRORType = PID_ERROR_NONE; - - pid->Output = 0; + // // 设置PID异常处理 目前仅包含电机堵转保护 + // pid->ERRORHandler.ERRORCount = 0; + // pid->ERRORHandler.ERRORType = PID_ERROR_NONE; } /** @@ -106,21 +52,11 @@ float PID_Calculate(PID_t *pid, float measure, float ref) pid->Ref = ref; pid->Err = pid->Ref - pid->Measure; - if (pid->User_Func1_f != NULL) - pid->User_Func1_f(pid); - if (abs(pid->Err) > pid->DeadBand) { - pid->Pout = pid->Kp * pid->Err; pid->ITerm = pid->Ki * pid->Err * pid->dt; - if (pid->OLS_Order > 2) - pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, pid->Err); - else - pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt; - - if (pid->User_Func2_f != NULL) - pid->User_Func2_f(pid); + pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt; // 梯形积分 if (pid->Improve & Trapezoid_Intergral) @@ -139,20 +75,14 @@ float PID_Calculate(PID_t *pid, float measure, float ref) f_Integral_Limit(pid); pid->Iout += pid->ITerm; - pid->Output = pid->Pout + pid->Iout + pid->Dout; // 输出滤波 if (pid->Improve & OutputFilter) f_Output_Filter(pid); - // 输出限幅 f_Output_Limit(pid); - - // 无关紧要 - f_Proportion_Limit(pid); } - pid->Last_Measure = pid->Measure; pid->Last_Output = pid->Output; pid->Last_Dout = pid->Dout; @@ -166,7 +96,6 @@ static void f_Trapezoid_Intergral(PID_t *pid) { pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt; - } static void f_Changing_Integration_Rate(PID_t *pid) @@ -213,11 +142,7 @@ static void f_Integral_Limit(PID_t *pid) static void f_Derivative_On_Measurement(PID_t *pid) { - if (pid->OLS_Order > 2) - pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, -pid->Measure); - else - pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt; - + pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt; } static void f_Derivative_Filter(PID_t *pid) @@ -244,18 +169,6 @@ static void f_Output_Limit(PID_t *pid) } } -static void f_Proportion_Limit(PID_t *pid) -{ - if (pid->Pout > pid->MaxOut) - { - pid->Pout = pid->MaxOut; - } - if (pid->Pout < -(pid->MaxOut)) - { - pid->Pout = -(pid->MaxOut); - } -} - // PID ERRORHandle Function static void f_PID_ErrorHandle(PID_t *pid) { @@ -278,227 +191,4 @@ static void f_PID_ErrorHandle(PID_t *pid) // Motor blocked over 1000times pid->ERRORHandler.ERRORType = Motor_Blocked; } -} - -/*************************** FEEDFORWARD CONTROL *****************************/ -/** - * @brief 前馈控制初始化 - * @param[in] 前馈控制结构体 - * @param[in] 略 - * @retval 返回空 - */ -void Feedforward_Init( - Feedforward_t *ffc, - float max_out, - float *c, - float lpf_rc, - uint16_t ref_dot_ols_order, - uint16_t ref_ddot_ols_order) -{ - ffc->MaxOut = max_out; - - // 设置前馈控制器参数 详见前馈控制结构体定义 - // set parameters of feed-forward controller (see struct definition) - if (c != NULL && ffc != NULL) - { - ffc->c[0] = c[0]; - ffc->c[1] = c[1]; - ffc->c[2] = c[2]; - } - else - { - ffc->c[0] = 0; - ffc->c[1] = 0; - ffc->c[2] = 0; - ffc->MaxOut = 0; - } - - ffc->LPF_RC = lpf_rc; - - // 最小二乘提取信号微分初始化 - // differential signal is distilled by OLS - ffc->Ref_dot_OLS_Order = ref_dot_ols_order; - ffc->Ref_ddot_OLS_Order = ref_ddot_ols_order; - if (ref_dot_ols_order > 2) - OLS_Init(&ffc->Ref_dot_OLS, ref_dot_ols_order); - if (ref_ddot_ols_order > 2) - OLS_Init(&ffc->Ref_ddot_OLS, ref_ddot_ols_order); - - ffc->DWT_CNT = 0; - - ffc->Output = 0; -} - -/** - * @brief PID计算 - * @param[in] PID结构体 - * @param[in] 测量值 - * @param[in] 期望值 - * @retval 返回空 - */ -float Feedforward_Calculate(Feedforward_t *ffc, float ref) -{ - ffc->dt = DWT_GetDeltaT((void *)&ffc->DWT_CNT); - - ffc->Ref = ref * ffc->dt / (ffc->LPF_RC + ffc->dt) + - ffc->Ref * ffc->LPF_RC / (ffc->LPF_RC + ffc->dt); - - // 计算一阶导数 - // calculate first derivative - if (ffc->Ref_dot_OLS_Order > 2) - ffc->Ref_dot = OLS_Derivative(&ffc->Ref_dot_OLS, ffc->dt, ffc->Ref); - else - ffc->Ref_dot = (ffc->Ref - ffc->Last_Ref) / ffc->dt; - - // 计算二阶导数 - // calculate second derivative - if (ffc->Ref_ddot_OLS_Order > 2) - ffc->Ref_ddot = OLS_Derivative(&ffc->Ref_ddot_OLS, ffc->dt, ffc->Ref_dot); - else - ffc->Ref_ddot = (ffc->Ref_dot - ffc->Last_Ref_dot) / ffc->dt; - - // 计算前馈控制输出 - // calculate feed-forward controller output - ffc->Output = ffc->c[0] * ffc->Ref + ffc->c[1] * ffc->Ref_dot + ffc->c[2] * ffc->Ref_ddot; - - ffc->Output = float_constrain(ffc->Output, -ffc->MaxOut, ffc->MaxOut); - - ffc->Last_Ref = ffc->Ref; - ffc->Last_Ref_dot = ffc->Ref_dot; - - return ffc->Output; -} - -/*************************LINEAR DISTURBANCE OBSERVER *************************/ -void LDOB_Init( - LDOB_t *ldob, - float max_d, - float deadband, - float *c, - float lpf_rc, - uint16_t measure_dot_ols_order, - uint16_t measure_ddot_ols_order) -{ - ldob->Max_Disturbance = max_d; - - ldob->DeadBand = deadband; - - // 设置线性扰动观测器参数 详见LDOB结构体定义 - // set parameters of linear disturbance observer (see struct definition) - if (c != NULL && ldob != NULL) - { - ldob->c[0] = c[0]; - ldob->c[1] = c[1]; - ldob->c[2] = c[2]; - } - else - { - ldob->c[0] = 0; - ldob->c[1] = 0; - ldob->c[2] = 0; - ldob->Max_Disturbance = 0; - } - - // 设置Q(s)带宽 Q(s)选用一阶惯性环节 - // set bandwidth of Q(s) Q(s) is chosen as a first-order low-pass form - ldob->LPF_RC = lpf_rc; - - // 最小二乘提取信号微分初始化 - // differential signal is distilled by OLS - ldob->Measure_dot_OLS_Order = measure_dot_ols_order; - ldob->Measure_ddot_OLS_Order = measure_ddot_ols_order; - if (measure_dot_ols_order > 2) - OLS_Init(&ldob->Measure_dot_OLS, measure_dot_ols_order); - if (measure_ddot_ols_order > 2) - OLS_Init(&ldob->Measure_ddot_OLS, measure_ddot_ols_order); - - ldob->DWT_CNT = 0; - - ldob->Disturbance = 0; -} - -float LDOB_Calculate(LDOB_t *ldob, float measure, float u) -{ - ldob->dt = DWT_GetDeltaT((void *)&ldob->DWT_CNT); - - ldob->Measure = measure; - - ldob->u = u; - - // 计算一阶导数 - // calculate first derivative - if (ldob->Measure_dot_OLS_Order > 2) - ldob->Measure_dot = OLS_Derivative(&ldob->Measure_dot_OLS, ldob->dt, ldob->Measure); - else - ldob->Measure_dot = (ldob->Measure - ldob->Last_Measure) / ldob->dt; - - // 计算二阶导数 - // calculate second derivative - if (ldob->Measure_ddot_OLS_Order > 2) - ldob->Measure_ddot = OLS_Derivative(&ldob->Measure_ddot_OLS, ldob->dt, ldob->Measure_dot); - else - ldob->Measure_ddot = (ldob->Measure_dot - ldob->Last_Measure_dot) / ldob->dt; - - // 估计总扰动 - // estimate external disturbances and internal disturbances caused by model uncertainties - ldob->Disturbance = ldob->c[0] * ldob->Measure + ldob->c[1] * ldob->Measure_dot + ldob->c[2] * ldob->Measure_ddot - ldob->u; - ldob->Disturbance = ldob->Disturbance * ldob->dt / (ldob->LPF_RC + ldob->dt) + - ldob->Last_Disturbance * ldob->LPF_RC / (ldob->LPF_RC + ldob->dt); - - ldob->Disturbance = float_constrain(ldob->Disturbance, -ldob->Max_Disturbance, ldob->Max_Disturbance); - - // 扰动输出死区 - // deadband of disturbance output - if (abs(ldob->Disturbance) > ldob->DeadBand * ldob->Max_Disturbance) - ldob->Output = ldob->Disturbance; - else - ldob->Output = 0; - - ldob->Last_Measure = ldob->Measure; - ldob->Last_Measure_dot = ldob->Measure_dot; - ldob->Last_Disturbance = ldob->Disturbance; - - return ldob->Output; -} - -/*************************** Tracking Differentiator ***************************/ -void TD_Init(TD_t *td, float r, float h0) -{ - td->r = r; - td->h0 = h0; - - td->x = 0; - td->dx = 0; - td->ddx = 0; - td->last_dx = 0; - td->last_ddx = 0; -} -float TD_Calculate(TD_t *td, float input) -{ - static float d, a0, y, a1, a2, a, fhan; - - td->dt = DWT_GetDeltaT((void *)&td->DWT_CNT); - - if (td->dt > 0.5f) - return 0; - - td->Input = input; - - d = td->r * td->h0 * td->h0; - a0 = td->dx * td->h0; - y = td->x - td->Input + a0; - a1 = sqrt(d * (d + 8 * abs(y))); - a2 = a0 + sign(y) * (a1 - d) / 2; - a = (a0 + y) * (sign(y + d) - sign(y - d)) / 2 + a2 * (1 - (sign(y + d) - sign(y - d)) / 2); - fhan = -td->r * a / d * (sign(a + d) - sign(a - d)) / 2 - - td->r * sign(a) * (1 - (sign(a + d) - sign(a - d)) / 2); - - td->ddx = fhan; - td->dx += (td->ddx + td->last_ddx) * td->dt / 2; - td->x += (td->dx + td->last_dx) * td->dt / 2; - - td->last_ddx = td->ddx; - td->last_dx = td->dx; - - return td->x; -} +} \ No newline at end of file diff --git a/modules/algorithm/controller.h b/modules/algorithm/controller.h index 2cf59ba..544d754 100644 --- a/modules/algorithm/controller.h +++ b/modules/algorithm/controller.h @@ -19,7 +19,6 @@ #include "string.h" #include "stdlib.h" #include "bsp_dwt.h" -#include "user_lib.h" #include "arm_math.h" #include @@ -27,13 +26,6 @@ #define abs(x) ((x > 0) ? x : -x) #endif -#ifndef user_malloc -#ifdef _CMSIS_OS_H -#define user_malloc pvPortMalloc -#else -#define user_malloc malloc -#endif -#endif /******************************* PID CONTROL *********************************/ typedef enum pid_Improvement_e @@ -55,19 +47,31 @@ typedef enum errorType_e Motor_Blocked = 0x01U } ErrorType_e; -typedef __packed struct +typedef struct { uint64_t ERRORCount; ErrorType_e ERRORType; } PID_ErrorHandler_t; -typedef __packed struct pid_t +typedef struct { - float Ref; +//---------------------------------- init config block + // config parameter float Kp; float Ki; float Kd; + float MaxOut; + float IntegralLimit; + float DeadBand; + float CoefA; //For Changing Integral + float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|> 1); - y = *(float *)&i; - y = y * (1.5f - (halfnum * y * y)); - return y; -}*/ -/** - * @brief 斜波函数初始化 - * @author RM - * @param[in] 斜波函数结构体 - * @param[in] 间隔的时间，单位 s - * @param[in] 最大值 - * @param[in] 最小值 - * @retval 返回空 - */ -void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min) -{ - ramp_source_type->frame_period = frame_period; - ramp_source_type->max_value = max; - ramp_source_type->min_value = min; - ramp_source_type->input = 0.0f; - ramp_source_type->out = 0.0f; -} + /** * @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值 @@ -217,176 +189,4 @@ int float_rounding(float raw) if (decimal > 0.5f) integer++; return integer; -} - -/** - * @brief 最小二乘法初始化 - * @param[in] 最小二乘法结构体 - * @param[in] 样本数 - * @retval 返回空 - */ -void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order) -{ - OLS->Order = order; - OLS->Count = 0; - OLS->x = (float *)user_malloc(sizeof(float) * order); - OLS->y = (float *)user_malloc(sizeof(float) * order); - OLS->k = 0; - OLS->b = 0; - memset((void *)OLS->x, 0, sizeof(float) * order); - memset((void *)OLS->y, 0, sizeof(float) * order); - memset((void *)OLS->t, 0, sizeof(float) * 4); -} - -/** - * @brief 最小二乘法拟合 - * @param[in] 最小二乘法结构体 - * @param[in] 信号新样本距上一个样本时间间隔 - * @param[in] 信号值 - */ -void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y) -{ - static float temp = 0; - temp = OLS->x[1]; - for (uint16_t i = 0; i < OLS->Order - 1; ++i) - { - OLS->x[i] = OLS->x[i + 1] - temp; - OLS->y[i] = OLS->y[i + 1]; - } - OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax; - OLS->y[OLS->Order - 1] = y; - - if (OLS->Count < OLS->Order) - { - OLS->Count++; - } - memset((void *)OLS->t, 0, sizeof(float) * 4); - for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i) - { - OLS->t[0] += OLS->x[i] * OLS->x[i]; - OLS->t[1] += OLS->x[i]; - OLS->t[2] += OLS->x[i] * OLS->y[i]; - OLS->t[3] += OLS->y[i]; - } - - OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]); - OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]); - - OLS->StandardDeviation = 0; - for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i) - { - OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]); - } - OLS->StandardDeviation /= OLS->Order; -} - -/** - * @brief 最小二乘法提取信号微分 - * @param[in] 最小二乘法结构体 - * @param[in] 信号新样本距上一个样本时间间隔 - * @param[in] 信号值 - * @retval 返回斜率k - */ -float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y) -{ - static float temp = 0; - temp = OLS->x[1]; - for (uint16_t i = 0; i < OLS->Order - 1; ++i) - { - OLS->x[i] = OLS->x[i + 1] - temp; - OLS->y[i] = OLS->y[i + 1]; - } - OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax; - OLS->y[OLS->Order - 1] = y; - - if (OLS->Count < OLS->Order) - { - OLS->Count++; - } - - memset((void *)OLS->t, 0, sizeof(float) * 4); - for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i) - { - OLS->t[0] += OLS->x[i] * OLS->x[i]; - OLS->t[1] += OLS->x[i]; - OLS->t[2] += OLS->x[i] * OLS->y[i]; - OLS->t[3] += OLS->y[i]; - } - - OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]); - - OLS->StandardDeviation = 0; - for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i) - { - OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]); - } - OLS->StandardDeviation /= OLS->Order; - - return OLS->k; -} - -/** - * @brief 获取最小二乘法提取信号微分 - * @param[in] 最小二乘法结构体 - * @retval 返回斜率k - */ -float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS) -{ - return OLS->k; -} - -/** - * @brief 最小二乘法平滑信号 - * @param[in] 最小二乘法结构体 - * @param[in] 信号新样本距上一个样本时间间隔 - * @param[in] 信号值 - * @retval 返回平滑输出 - */ -float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y) -{ - static float temp = 0; - temp = OLS->x[1]; - for (uint16_t i = 0; i < OLS->Order - 1; ++i) - { - OLS->x[i] = OLS->x[i + 1] - temp; - OLS->y[i] = OLS->y[i + 1]; - } - OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax; - OLS->y[OLS->Order - 1] = y; - - if (OLS->Count < OLS->Order) - { - OLS->Count++; - } - - memset((void *)OLS->t, 0, sizeof(float) * 4); - for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i) - { - OLS->t[0] += OLS->x[i] * OLS->x[i]; - OLS->t[1] += OLS->x[i]; - OLS->t[2] += OLS->x[i] * OLS->y[i]; - OLS->t[3] += OLS->y[i]; - } - - OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]); - OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]); - - OLS->StandardDeviation = 0; - for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i) - { - OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]); - } - OLS->StandardDeviation /= OLS->Order; - - return OLS->k * OLS->x[OLS->Order - 1] + OLS->b; -} - -/** - * @brief 获取最小二乘法平滑信号 - * @param[in] 最小二乘法结构体 - * @retval 返回平滑输出 - */ -float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS) -{ - return OLS->k * OLS->x[OLS->Order - 1] + OLS->b; -} +} \ No newline at end of file diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h index 00016bf..7f5f940 100644 --- a/modules/algorithm/user_lib.h +++ b/modules/algorithm/user_lib.h @@ -89,11 +89,11 @@ extern uint8_t GlobalDebugMode; typedef struct { - float input; // - float out; // - float min_value; //޷Сֵ - float max_value; //޷ֵ - float frame_period; //ʱ + float input; //�������� + float out; //������� + float min_value; //�޷���Сֵ + float max_value; //�޷����ֵ + float frame_period; //ʱ���� } ramp_function_source_t; typedef __packed struct @@ -112,41 +112,34 @@ typedef __packed struct float t[4]; } Ordinary_Least_Squares_t; -//ٿ +//���ٿ��� float Sqrt(float x); -//бʼ +//б��������ʼ�� void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min); -//б +//б���������� float ramp_calc(ramp_function_source_t *ramp_source_type, float input); -// +//�������� float abs_limit(float num, float Limit); -//жϷλ +//�жϷ���λ float sign(float value); -// +//�������� float float_deadband(float Value, float minValue, float maxValue); -// int26 +// int26���� int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue); -//޷ +//�޷����� float float_constrain(float Value, float minValue, float maxValue); -//޷ +//�޷����� int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue); -//ѭ޷ +//ѭ���޷����� float loop_float_constrain(float Input, float minValue, float maxValue); -//Ƕ ޷ 180 ~ -180 +//�Ƕ� ���޷� 180 ~ -180 float theta_format(float Ang); int float_rounding(float raw); -//ȸʽΪ-PI~PI +//���ȸ�ʽ��Ϊ-PI~PI #define rad_format(Ang) loop_float_constrain((Ang), -PI, PI) -void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order); -void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y); -float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y); -float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y); -float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS); -float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS); - #endif diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 8cd97c5..29d4f0e 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -16,7 +16,7 @@ #include "bsp_temperature.h" #include "spi.h" -INS_t INS; +INS_t INS; IMU_Param_t IMU_Param; PID_t TempCtrl = {0}; @@ -44,7 +44,14 @@ void INS_Init(void) IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0); // imu heat init - PID_Init(&TempCtrl, 2000, 300, 0, 1000, 20, 0, 0, 0, 0, 0, 0, 0); + PID_Init_config_s config = {.MaxOut = 2000, + .IntegralLimit = 300, + .DeadBand = 0, + .Kp = 1000, + .Ki = 20, + .Kd = 0, + .Improve = 0x01}; // enable integratiaon limit + PID_Init(&TempCtrl,&config); // noise of accel is relatively big and of high freq,thus lpf is used INS.AccelLPF = 0.0085; @@ -99,7 +106,7 @@ void INS_Task(void) INS.Yaw = QEKF_INS.Yaw; INS.Pitch = QEKF_INS.Pitch; INS.Roll = QEKF_INS.Roll; - INS.YawTotalAngle = QEKF_INS.YawTotalAngle; + INS.YawTotalAngle = QEKF_INS.YawTotalAngle; } // temperature control @@ -117,7 +124,6 @@ void INS_Task(void) count++; } - /** * @brief Transform 3dvector from BodyFrame to EarthFrame * @param[1] vector in BodyFrame @@ -232,7 +238,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[ /** * @brief 温度控制 - * + * */ void IMU_Temperature_Ctrl(void) { diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index 031d590..a670cc6 100644 --- a/modules/motor/HT04.c +++ b/modules/motor/HT04.c @@ -40,7 +40,7 @@ joint_instance* HTMotorInit(can_instance_config config) { static uint8_t idx; joint_motor_info[idx]=(joint_instance*)malloc(sizeof(joint_instance)); - joint_motor_info[idx++]->motor_can_instace=CANRegister(config); + CANRegister(&joint_motor_info[idx++]->motor_can_instace,config); } void JointControl(joint_instance* _instance,float current) @@ -50,7 +50,7 @@ void JointControl(joint_instance* _instance,float current) tmp = float_to_uint(current, T_MIN, T_MAX, 12); _instance->motor_can_instace->rx_buff[6] = tmp>>8; _instance->motor_can_instace->rx_buff[7] = tmp&0xff; - CANTransmit(&_instance->motor_can_instace); + CANTransmit(_instance->motor_can_instace); } void SetJointMode(joint_mode cmd,joint_instance* _instance) @@ -58,5 +58,5 @@ void SetJointMode(joint_mode cmd,joint_instance* _instance) static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; buf[7]=(uint8_t)cmd; memcpy(_instance->motor_can_instace->rx_buff,buf,8*sizeof(uint8_t)); - CANTransmit(&_instance->motor_can_instace); + CANTransmit(_instance->motor_can_instace); } diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index ac8ff7d..f8d4d25 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -23,7 +23,7 @@ driven_instance* LKMotroInit(can_instance_config config) static uint8_t idx; driven_motor_info[idx]=(driven_instance*)malloc(sizeof(driven_instance)); config.can_module_callback=DecodeDriven; - driven_motor_info[idx++]->motor_can_instance=CANRegister(config); + CANRegister(driven_motor_info[idx++]->motor_can_instance,config); } void DrivenControl(int16_t motor1_current,int16_t motor2_current) diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 0b1ff35..2b36df8 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -1,6 +1,7 @@ #include "dji_motor.h" -static uint8_t idx = 0; // register idx +static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 + /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; @@ -29,60 +30,8 @@ static can_instance sender_assignment[6] = static uint8_t sender_enable_flag[6] = {0}; /** - * @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令 - * - * @param config - */ -static void MotorSenderGrouping(can_instance_config *config) -{ - uint8_t motor_id = config->tx_id - 1; - uint8_t motor_rx_id; - uint8_t motor_send_num; - uint8_t motor_grouping; - - switch (dji_motor_info[idx]->motor_type) - { - case M2006: - case M3508: - if (motor_id < 4) - { - dji_motor_info[idx]->message_num = motor_id; - dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 1 : 4; - } - else - { - dji_motor_info[idx]->message_num = motor_id - 4; - dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3; - } - config->rx_id = 0x200 + motor_id; - sender_enable_flag[dji_motor_info[idx]->sender_group] = 1; - break; - - case GM6020: - if (motor_id < 4) - { - dji_motor_info[idx]->message_num = motor_id; - dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3; - } - else - { - dji_motor_info[idx]->message_num = motor_id - 4; - dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 2 : 5; - } - config->rx_id = 0x204 + motor_id; - sender_enable_flag[dji_motor_info[idx]->sender_group] = 1; - break; - // other motors should not be registered here - default: - break; - } -} - -/** - * @todo 待添加此功能. - * * @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID - * + * @todo 通过segger jlink 发送日志 */ static void IDcrash_Handler() { @@ -91,17 +40,86 @@ static void IDcrash_Handler() }; } +/** + * @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令 + * + * @param config + */ +static void MotorSenderGrouping(can_instance_config *config) +{ + uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值 + uint8_t motor_send_num; + uint8_t motor_grouping; + + switch (dji_motor_info[idx]->motor_type) + { + case M2006: + case M3508: + if (motor_id < 4) // 根据ID分组 + { + motor_send_num = motor_id; + motor_grouping = config->can_handle == &hcan1 ? 1 : 4; + } + else + { + motor_send_num = motor_id - 4; + motor_grouping = config->can_handle == &hcan1 ? 0 : 3; + } + + // 计算接收id并设置分组发送id + config->rx_id = 0x200 + motor_id+1; + sender_enable_flag[motor_grouping] = 1; + dji_motor_info[idx]->message_num = motor_send_num; + dji_motor_info[idx]->sender_group = motor_grouping; + + // 检查是否发生id冲突 + for (size_t i = 0; i < idx; i++) + { + if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id) + IDcrash_Handler(); + } + break; + + case GM6020: + if (motor_id < 4) + { + motor_send_num = motor_id; + motor_grouping = config->can_handle == &hcan1 ? 0 : 3; + } + else + { + motor_send_num = motor_id - 4; + motor_grouping = config->can_handle == &hcan1 ? 2 : 5; + } + + config->rx_id = 0x204 + motor_id; + sender_enable_flag[motor_grouping] = 1; + dji_motor_info[idx]->message_num = motor_send_num; + dji_motor_info[idx]->sender_group = motor_grouping; + + for (size_t i = 0; i < idx; i++) + { + if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id) + IDcrash_Handler(); + } + break; + + default: // other motors should not be registered here + break; + } +} + /** * @brief 根据返回的can_instance对反馈报文进行解析 * - * @param _instance 收到数据的instance,通过遍历与所有电机进行对比 + * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例 */ static void DecodeDJIMotor(can_instance *_instance) { uint8_t *rxbuff = _instance->rx_buff; for (size_t i = 0; i < DJI_MOTOR_CNT; i++) { - if (dji_motor_info[i]->motor_can_instance == _instance) + if (&dji_motor_info[i]->motor_can_instance == _instance) { dji_motor_info[i]->motor_measure.last_ecd = dji_motor_info[i]->motor_measure.ecd; dji_motor_info[i]->motor_measure.ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); @@ -113,52 +131,108 @@ static void DecodeDJIMotor(can_instance *_instance) } } -dji_motor_instance *DJIMotorInit(can_instance_config config, +// 电机初始化,返回一个电机实例 +dji_motor_instance *DJIMotorInit(can_instance_config can_config, Motor_Control_Setting_s motor_setting, Motor_Controller_Init_s controller_init, Motor_Type_e type) { dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance)); - // motor setting + + // motor basic setting dji_motor_info[idx]->motor_type = type; dji_motor_info[idx]->motor_settings = motor_setting; // motor controller init - // @todo : PID init + PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &controller_init.current_PID); + PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &controller_init.speed_PID); + PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &controller_init.angle_PID); dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = controller_init.other_angle_feedback_ptr; dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = controller_init.other_speed_feedback_ptr; // group motors, because 4 motors share the same CAN control message - MotorSenderGrouping(&config); + MotorSenderGrouping(&can_config); // register motor to CAN bus - config.can_module_callback = DecodeDJIMotor; - dji_motor_info[idx]->motor_can_instance = CANRegister(config); + can_config.can_module_callback = DecodeDJIMotor; // set callback + CANRegister(&dji_motor_info[idx]->motor_can_instance, can_config); return dji_motor_info[idx++]; } +// 改变反馈来源 +void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type) +{ + if(loop==ANGLE_LOOP) + { + motor->motor_settings.angle_feedback_source=type; + } + if(loop==SPEED_LOOP) + { + motor->motor_settings.speed_feedback_source=type; + } +} + +// 设置参考值 void DJIMotorSetRef(dji_motor_instance *motor, float ref) { motor->motor_controller.pid_ref = ref; } + void DJIMotorControl() -{ - static uint8_t group, num, set; +{ + // 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销 + static uint8_t group, num; + static int16_t set; + static dji_motor_instance* motor; + static Motor_Control_Setting_s *motor_setting; + static Motor_Controller_s *motor_controller; + static dji_motor_measure *motor_measure; + static float pid_measure; // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 for (size_t i = 0; i < DJI_MOTOR_CNT; i++) { if (dji_motor_info[i]) { - // @todo: 计算PID - // calculate pid output - // ... - group = dji_motor_info[i]->sender_group; - num = dji_motor_info[i]->message_num; - set = (int16_t)dji_motor_info[i]->motor_controller.pid_output; - // sender_assignment[group].rx_buff[num]= 0xff & PIDoutPIDoutput>>8; - // sender_assignment[group].rx_buff[num]= 0xff & PIDoutput; + motor=dji_motor_info[i]; + motor_setting=&motor->motor_settings; + motor_controller=&motor->motor_controller; + motor_measure=&motor->motor_measure; + + if (motor_setting->close_loop_type & ANGLE_LOOP) // 计算位置环 + { + if (motor_setting->angle_feedback_source == OTHER_FEED) + pid_measure=*motor_controller->other_angle_feedback_ptr; + else // MOTOR_FEED + pid_measure=motor_measure->total_angle; + // 更新pid_ref进入下一个环 + motor_controller->pid_ref=PID_Calculate(&motor_controller->angle_PID,pid_measure,motor_controller->pid_ref); + } + + if (motor_setting->close_loop_type & SPEED_LOOP) // 计算速度环 + { + if (motor_setting->speed_feedback_source == OTHER_FEED) + pid_measure=*motor_controller->other_speed_feedback_ptr; + else + pid_measure=motor_measure->speed_rpm; + motor_controller->pid_ref=PID_Calculate(&motor_controller->speed_PID,pid_measure,motor_controller->pid_ref); + } + + if (motor_setting->close_loop_type & CURRENT_LOOP) //计算电流环 + { + motor_controller->pid_ref=PID_Calculate(&motor_controller->current_PID,motor_measure->given_current,motor_controller->pid_ref); + } + + set = (int16_t)motor_controller->pid_ref; + if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) //设置反转 + set *= -1; + + // 分组填入发送数据 + group = motor->sender_group; + num = motor->message_num; + sender_assignment[group].tx_buff[num] = 0xff & set >> 8; + sender_assignment[group].tx_buff[num+1] = 0xff & set; } - else + else // 遇到空指针说明所有遍历结束,退出循环 break; } diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index 5d5b57a..5916b9e 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -7,6 +7,18 @@ #include "controller.h" #include "motor_def.h" +/* DJI电机CAN反馈信息*/ +typedef struct +{ + uint16_t ecd; + uint16_t last_ecd; + int16_t speed_rpm; + int16_t given_current; + uint8_t temperate; + int16_t total_round; + int32_t total_angle; +} dji_motor_measure; + /** * @brief DJI intelligent motor typedef * @@ -14,16 +26,7 @@ typedef struct { /* motor measurement recv from CAN feedback */ - struct - { - uint16_t ecd; - uint16_t last_ecd; - int16_t speed_rpm; - int16_t given_current; - uint8_t temperate; - int16_t total_round; - int32_t total_angle; - } motor_measure; + dji_motor_measure motor_measure; /* basic config of a motor*/ Motor_Control_Setting_s motor_settings; @@ -32,7 +35,7 @@ typedef struct Motor_Controller_s motor_controller; /* the CAN instance own by motor instance*/ - can_instance *motor_can_instance; + can_instance motor_can_instance; /* sender assigment*/ uint8_t sender_group; @@ -44,8 +47,6 @@ typedef struct /** - * @todo 加入ID冲突的检查机制,如果发现注册的ID冲突,进入IDcrash_Handler()的死循环中 - * * @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数 * 推荐传参时像标准库一样构造initStructure然后传入此函数. * recommend: type xxxinitStructure = { @@ -55,8 +56,8 @@ typedef struct * 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz), * 并减小DJIMotorControl()任务的运行频率. * - * @attention 当前并没有对电机的ID冲突进行检查,请保证在注册电机的时候,他们的反馈ID不会产生冲突! - * M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突. + * @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突. + * 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突. * * @param config 电机can设置,对于DJI电机仅需要将tx_id设置为电调闪动次数(c620,615,610)或拨码开关的值(GM6020) * 你不需要自己查表计算发送和接收id,我们会帮你处理好! @@ -87,8 +88,18 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref); /** - * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率 + * @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用) * + * @param motor 要切换反馈数据来源的电机 + * @param loop 要切换反馈数据来源的控制闭环 + * @param type 目标反馈模式 + */ +void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type); + + +/** + * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率 + * @todo 增加前馈功能 */ void DJIMotorControl(); diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 10ed417..78ff10f 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -1,27 +1,31 @@ #ifndef MOTOR_DEF_H #define MOTOR_DEF_H -typedef enum +typedef enum { - CURRENT=0, - SPEED=1, - ANGLE=2 + CURRENT_LOOP = 0b0001, + SPEED_LOOP = 0b0010, + ANGLE_LOOP = 0b0100, + + // only for check + _ = 0b0011, + __ = 0b0110, + ___ = 0b0111 } Closeloop_Type_e; typedef enum { - MOTOR=0, - OTHER=1 + MOTOR_FEED = 0, + OTHER_FEED = 1 } Feedback_Source_e; typedef enum { - CLOCKWISE=0, - COUNTER_CLOCKWISE=1 + MOTOR_DIRECTION_NORMAL = 0, + MOTOR_DIRECTION_REVERSE = 1 } Reverse_Flag_e; - -typedef struct +typedef struct { Closeloop_Type_e close_loop_type; Reverse_Flag_e reverse_flag; @@ -30,46 +34,37 @@ typedef struct } Motor_Control_Setting_s; +typedef struct +{ + float *other_angle_feedback_ptr; + float *other_speed_feedback_ptr; + + PID_t current_PID; + PID_t speed_PID; + PID_t angle_PID; + + // 将会作为每个环的输入和输出 + float pid_ref; +} Motor_Controller_s; + +typedef enum +{ + GM6020 = 0, + M3508 = 1, + M2006 = 2, + LK9025 = 3, + HT04 = 4 +} Motor_Type_e; typedef struct { - float* other_angle_feedback_ptr; - float* other_speed_feedback_ptr; + float *other_angle_feedback_ptr; + float *other_speed_feedback_ptr; - PID_t* current_PID; - PID_t* speed_PID; - PID_t* angle_PID; + PID_Init_config_s current_PID; + PID_Init_config_s speed_PID; + PID_Init_config_s angle_PID; - float pid_ref; - float pid_output; - -} Motor_Controller_s; - -typedef enum -{ - GM6020=0, - M3508=1, - M2006=2, - LK9025=3, - HT04=4 -} Motor_Type_e; - -typedef struct -{ - /* data */ -} PID_config_s; - -typedef struct -{ - float* other_angle_feedback_ptr; - float* other_speed_feedback_ptr; - - PID_config_s current_PID; - PID_config_s speed_PID; - PID_config_s angle_PID; } Motor_Controller_Init_s; - - - #endif // !MOTOR_DEF_H diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 2625dc3..0af4456 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -1,32 +1,11 @@ #include "motor_task.h" -static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT]; -static joint_instance* joint_motor_info[HT_MOTOR_CNT]; -static driven_instance* driven_motor_info[LK_MOTOR_CNT]; - - -void RegisterMotor(Motor_Type_e motor_type,void* motor_instance) -{ - static uint8_t dji_idx,joint_idx,driven_idx; - switch (motor_type) - { - case GM6020: - case M3508: - case M2006: - dji_motor_info[dji_idx++]=(dji_motor_instance*)motor_instance; - break; - case LK9025: - driven_motor_info[driven_idx++]=(driven_instance*)motor_instance; - break; - case HT04: - joint_motor_info[joint_idx++]=(joint_instance*)motor_instance; - break; - } -} - - void MotorControlTask() { - + DJIMotorControl(); + + //LKMotorControl(); + + //HTMotorControl(); } diff --git a/modules/motor/motor_task.h b/modules/motor/motor_task.h index eddf921..1f6f81b 100644 --- a/modules/motor/motor_task.h +++ b/modules/motor/motor_task.h @@ -4,13 +4,8 @@ #include "LK9025.h" #include "HT04.h" #include "dji_motor.h" -#include "motor_def.h" - - void MotorControlTask(); -void RegisterMotor(Motor_Type_e motor_type,void* motor_instance); - #endif // !MOTOR_TASK_H diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index 254e827..d7ba9b6 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -51,14 +51,14 @@ #define KEY_PRESSED_OFFSET_V ((uint16_t)1 << 14) #define KEY_PRESSED_OFFSET_B ((uint16_t)1 << 15) /* ----------------------- Data Struct ------------------------------------- */ -typedef __packed struct +typedef struct { - __packed struct + struct { int16_t ch[5]; char s[2]; } rc; - __packed struct + struct { int16_t x; int16_t y; @@ -66,7 +66,7 @@ typedef __packed struct uint8_t press_l; uint8_t press_r; } mouse; - __packed struct + struct { uint16_t v; } key;