robofish/RMUC2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

infantry

Browse Source
This commit is contained in:
RB
2025-03-11 21:32:41 +08:00
parent 1480a30aa1
commit 740dc38e96
327 changed files with 252743 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
User/bsp/a/bsp_adc.c Normal file
View File

@@ -0,0 +1,14 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_adc.h"
#include "adc.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
float BSP_GetTemprater(void) { return 0.0f; }
float BSP_GetBatteryVolt(void) { return 0.0f; }
uint8_t BSP_GetHardwareVersion(void) { return 1u; }

23
User/bsp/a/bsp_buzzer.c Normal file
View File

@@ -0,0 +1,23 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_buzzer.h"
#include "main.h"
#include "tim.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t BSP_Buzzer_Start(void) { return HAL_TIM_PWM_Start(&htim12, TIM_CHANNEL_4); }
uint8_t BSP_Buzzer_Set(float freq, float duty_cycle) {
uint16_t pulse = freq * PWM_RESOLUTION;
__HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_4, pulse);
pulse = duty_cycle * PWM_RESOLUTION;
__HAL_TIM_PRESCALER(&htim12, pulse);
return 0;
}
uint8_t BSP_Buzzer_Stop(void) { return HAL_TIM_PWM_Stop(&htim12, TIM_CHANNEL_4); }

10
User/bsp/a/bsp_crc32.c Normal file
View File

@@ -0,0 +1,10 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_crc.h"
#include "crc.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

20
User/bsp/a/bsp_delay.c Normal file
View File

@@ -0,0 +1,20 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_delay.h"
#include "cmsis_os.h"
#include "main.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t BSP_Delay(uint32_t ms) {
if (osKernelRunning()) {
osDelay(ms);
} else {
HAL_Delay(ms);
}
return 0;
}

11
User/bsp/a/bsp_flash.c Normal file
View File

@@ -0,0 +1,11 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_flash.h"
#include "main.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

27
User/bsp/a/bsp_fric.c Normal file
View File

@@ -0,0 +1,27 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_fric.h"
#include "bsp_delay.h"
#include "tim.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t BSP_Fric_Start(void) {
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
BSP_Delay(500);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
return 0;
}
uint8_t BSP_Fric_Set(float duty_cycle) {
uint16_t pulse = duty_cycle * PWM_RESOLUTION;
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pulse);
}
uint8_t BSP_Fric_Stop(void) {
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);
}

20
User/bsp/a/bsp_laser.c Normal file
View File

@@ -0,0 +1,20 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_laser.h"
#include "gpio.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t Laser_On(void) {
HAL_GPIO_WritePin(LASER_GPIO_Port, LASER_Pin, GPIO_PIN_SET);
return 0;
}
uint8_t Laser_Off(void) {
HAL_GPIO_WritePin(LASER_GPIO_Port, LASER_Pin, GPIO_PIN_RESET);
return 0;
}

84
User/bsp/a/bsp_led.c Normal file
View File

@@ -0,0 +1,84 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_led.h"
#include "gpio.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t BSP_LED_Set(BSP_LED_Channel_t ch, BSP_LED_Status_t s, int16_t duty_cycle) {
GPIO_TypeDef *gpiox;
uint16_t gpio_pin;
switch (ch) {
case BSP_LED1:
gpio_pin = LED1_Pin;
gpiox = LED1_GPIO_Port;
break;
case BSP_LED2:
gpio_pin = LED2_Pin;
gpiox = LED2_GPIO_Port;
break;
case BSP_LED3:
gpio_pin = LED3_Pin;
gpiox = LED3_GPIO_Port;
break;
case BSP_LED4:
gpio_pin = LED4_Pin;
gpiox = LED4_GPIO_Port;
break;
case BSP_LED5:
gpio_pin = LED5_Pin;
gpiox = LED5_GPIO_Port;
break;
case BSP_LED6:
gpio_pin = LED6_Pin;
gpiox = LED6_GPIO_Port;
break;
case BSP_LED7:
gpio_pin = LED7_Pin;
gpiox = LED7_GPIO_Port;
break;
case BSP_LED8:
gpio_pin = LED8_Pin;
gpiox = LED8_GPIO_Port;
break;
case BSP_LED_RED:
gpiox = LED_R_GPIO_Port;
gpio_pin = LED_R_Pin;
break;
case BSP_LED_GRN:
gpiox = LED_G_GPIO_Port;
gpio_pin = LED_G_Pin;
break;
}
switch (s) {
case BSP_LED_ON:
HAL_GPIO_WritePin(gpiox, gpio_pin, GPIO_PIN_RESET);
break;
case BSP_LED_OFF:
HAL_GPIO_WritePin(gpiox, gpio_pin, GPIO_PIN_SET);
break;
case BSP_LED_TAGGLE:
HAL_GPIO_TogglePin(gpiox, gpio_pin);
break;
}
return 0;
}

52
User/bsp/a/bsp_power.c Normal file
View File

@@ -0,0 +1,52 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_power.h"
#include "gpio.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t Power_On(BSP_Power_Port_t port) {
switch (port) {
case POWER_PORT1:
HAL_GPIO_WritePin(POWER1_CTRL_GPIO_Port, POWER1_CTRL_Pin, GPIO_PIN_RESET);
break;
case POWER_PORT2:
HAL_GPIO_WritePin(POWER2_CTRL_GPIO_Port, POWER2_CTRL_Pin, GPIO_PIN_RESET);
break;
case POWER_PORT3:
HAL_GPIO_WritePin(POWER3_CTRL_GPIO_Port, POWER3_CTRL_Pin, GPIO_PIN_RESET);
break;
case POWER_PORT4:
HAL_GPIO_WritePin(POWER4_CTRL_GPIO_Port, POWER4_CTRL_Pin, GPIO_PIN_RESET);
break;
}
return 0;
}
uint8_t Power_Off(BSP_Power_Port_t port) {
switch (port) {
case POWER_PORT1:
HAL_GPIO_WritePin(POWER1_CTRL_GPIO_Port, POWER1_CTRL_Pin, GPIO_PIN_SET);
break;
case POWER_PORT2:
HAL_GPIO_WritePin(POWER2_CTRL_GPIO_Port, POWER2_CTRL_Pin, GPIO_PIN_SET);
break;
case POWER_PORT3:
HAL_GPIO_WritePin(POWER3_CTRL_GPIO_Port, POWER3_CTRL_Pin, GPIO_PIN_SET);
break;
case POWER_PORT4:
HAL_GPIO_WritePin(POWER4_CTRL_GPIO_Port, POWER4_CTRL_Pin, GPIO_PIN_SET);
break;
}
return 0;
}

37
User/bsp/a/bsp_pwm.c Normal file
View File

@@ -0,0 +1,37 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_pwm.h"
#include "main.h"
#include "tim.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
switch (ch) {
case BSP_PWM_IMU_HEAT:
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
break;
}
return 0;
}
int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) {
if (duty_cycle > 1.0f) return -1;
uint16_t pulse = duty_cycle * PWM_RESOLUTION;
switch (ch) {
case BSP_PWM_IMU_HEAT:
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, pulse);
break;
default:
return -1;
}
return 0;
}
uint8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);

11
User/bsp/a/bsp_rand.c Normal file
View File

@@ -0,0 +1,11 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_rand.h"
#include "rng.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

11
User/bsp/a/bsp_servo.c Normal file
View File

@@ -0,0 +1,11 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_servo.h"
#include "tim.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

295
User/bsp/a/bsp_spi.c Normal file
View File

@@ -0,0 +1,295 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_spi.h"
#include "main.h"
#include "spi.h"
/* Private define ----------------------------------------------------------- */
#define OLED_SPI SPI1
#define IMU_SPI SPI5
/* #define XXX_SPI SPIX */
/* Private macro ------------------------------------------------------------ */
#define IMU_SPI_NSS_Reset() \
HAL_GPIO_WritePin(SPI5_NSS_GPIO_Port, SPI5_NSS_Pin, GPIO_PIN_RESET)
#define IMU_SPI_NSS_Set() \
HAL_GPIO_WritePin(SPI5_NSS_GPIO_Port, SPI5_NSS_Pin, GPIO_PIN_SET)
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static struct {
struct {
void (*TxCpltCallback)(void); /* SPI Tx Completed callback */
void (*RxCpltCallback)(void); /* SPI Rx Completed callback */
void (*TxRxCpltCallback)(void); /* SPI TxRx Completed callback */
void (*TxHalfCpltCallback)(void); /* SPI Tx Half Completed callback */
void (*RxHalfCpltCallback)(void); /* SPI Rx Half Completed callback */
void (*TxRxHalfCpltCallback)(void); /* SPI TxRx Half Completed callback */
void (*ErrorCallback)(void); /* SPI Error callback */
void (*AbortCpltCallback)(void); /* SPI Abort callback */
} oled;
struct {
void (*TxCpltCallback)(void); /* SPI Tx Completed callback */
void (*RxCpltCallback)(void); /* SPI Rx Completed callback */
void (*TxRxCpltCallback)(void); /* SPI TxRx Completed callback */
void (*TxHalfCpltCallback)(void); /* SPI Tx Half Completed callback */
void (*RxHalfCpltCallback)(void); /* SPI Rx Half Completed callback */
void (*TxRxHalfCpltCallback)(void); /* SPI TxRx Half Completed callback */
void (*ErrorCallback)(void); /* SPI Error callback */
void (*AbortCpltCallback)(void); /* SPI Abort callback */
} imu;
} bsp_spi_callback;
/* Private function -------------------------------------------------------- */
void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.TxCpltCallback != NULL) {
bsp_spi_callback.oled.TxCpltCallback();
}
} else if (hspi->Instance == IMU_SPI) {
IMU_SPI_NSS_Set();
if (bsp_spi_callback.imu.TxCpltCallback != NULL) {
bsp_spi_callback.imu.TxCpltCallback();
}
}
}
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.RxCpltCallback != NULL) {
bsp_spi_callback.oled.RxCpltCallback();
}
} else if (hspi->Instance == IMU_SPI) {
IMU_SPI_NSS_Set();
if (bsp_spi_callback.imu.RxCpltCallback != NULL) {
bsp_spi_callback.imu.RxCpltCallback();
}
}
}
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.TxRxCpltCallback != NULL) {
bsp_spi_callback.oled.TxRxCpltCallback();
}
} else if (hspi->Instance == IMU_SPI) {
if (bsp_spi_callback.imu.TxRxCpltCallback != NULL) {
bsp_spi_callback.imu.TxRxCpltCallback();
}
}
}
void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.TxHalfCpltCallback != NULL) {
bsp_spi_callback.oled.TxHalfCpltCallback();
}
} else if (hspi->Instance == IMU_SPI) {
if (bsp_spi_callback.imu.TxHalfCpltCallback != NULL) {
bsp_spi_callback.imu.TxHalfCpltCallback();
}
}
}
void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.RxHalfCpltCallback != NULL) {
bsp_spi_callback.oled.RxHalfCpltCallback();
}
} else if (hspi->Instance == IMU_SPI) {
if (bsp_spi_callback.imu.RxHalfCpltCallback != NULL) {
bsp_spi_callback.imu.RxHalfCpltCallback();
}
}
}
void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.TxRxHalfCpltCallback != NULL) {
bsp_spi_callback.oled.TxRxHalfCpltCallback();
}
} else if (hspi->Instance == IMU_SPI) {
if (bsp_spi_callback.imu.TxRxHalfCpltCallback != NULL) {
bsp_spi_callback.imu.TxRxHalfCpltCallback();
}
}
}
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.ErrorCallback != NULL) {
bsp_spi_callback.oled.ErrorCallback();
}
} else if (hspi->Instance == IMU_SPI) {
if (bsp_spi_callback.imu.ErrorCallback != NULL) {
bsp_spi_callback.imu.ErrorCallback();
}
}
}
void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == OLED_SPI) {
if (bsp_spi_callback.oled.AbortCpltCallback != NULL) {
bsp_spi_callback.oled.AbortCpltCallback();
}
} else if (hspi->Instance == IMU_SPI) {
if (bsp_spi_callback.imu.AbortCpltCallback != NULL) {
bsp_spi_callback.imu.AbortCpltCallback();
}
}
/*
else if (hspi->Instance == XXX_SPI) {
if (bsp_spi_callback.xxx.AbortCpltCallback != NULL) {
bsp_spi_callback.xxx.AbortCpltCallback();
}
}
*/
}
/* Exported functions ------------------------------------------------------- */
int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return -1;
switch (spi) {
case BSP_SPI_IMU:
switch (type) {
case BSP_SPI_TX_COMPLETE_CB:
bsp_spi_callback.imu.TxCpltCallback = callback;
break;
case BSP_SPI_RX_COMPLETE_CB:
bsp_spi_callback.imu.RxCpltCallback = callback;
break;
case BSP_SPI_TX_RX_COMPLETE_CB:
bsp_spi_callback.imu.TxRxCpltCallback = callback;
break;
case BSP_SPI_TX_HALF_COMPLETE_CB:
bsp_spi_callback.imu.TxHalfCpltCallback = callback;
break;
case BSP_SPI_RX_HALF_COMPLETE_CB:
bsp_spi_callback.imu.RxHalfCpltCallback = callback;
break;
case BSP_SPI_TX_RX_HALF_COMPLETE_CB:
bsp_spi_callback.imu.TxRxHalfCpltCallback = callback;
break;
case BSP_SPI_ERROR_CB:
bsp_spi_callback.imu.ErrorCallback = callback;
break;
case BSP_SPI_ABORT_CB:
bsp_spi_callback.imu.AbortCpltCallback = callback;
break;
default:
return -1;
}
break;
case BSP_SPI_OLED:
switch (type) {
case BSP_SPI_TX_COMPLETE_CB:
bsp_spi_callback.oled.TxCpltCallback = callback;
break;
case BSP_SPI_RX_COMPLETE_CB:
bsp_spi_callback.oled.RxCpltCallback = callback;
break;
case BSP_SPI_TX_RX_COMPLETE_CB:
bsp_spi_callback.oled.TxRxCpltCallback = callback;
break;
case BSP_SPI_TX_HALF_COMPLETE_CB:
bsp_spi_callback.oled.TxHalfCpltCallback = callback;
break;
case BSP_SPI_RX_HALF_COMPLETE_CB:
bsp_spi_callback.oled.RxHalfCpltCallback = callback;
break;
case BSP_SPI_TX_RX_HALF_COMPLETE_CB:
bsp_spi_callback.oled.TxRxHalfCpltCallback = callback;
break;
case BSP_SPI_ERROR_CB:
bsp_spi_callback.oled.ErrorCallback = callback;
break;
case BSP_SPI_ABORT_CB:
bsp_spi_callback.oled.AbortCpltCallback = callback;
break;
default:
return -1;
}
break;
/*
case BSP_SPI_XXX:
switch (type) {
case BSP_SPI_TX_COMPLETE_CB:
bsp_spi_callback.xxx.TxCpltCallback = callback;
break;
case BSP_SPI_RX_COMPLETE_CB:
bsp_spi_callback.xxx.RxCpltCallback = callback;
break;
case BSP_SPI_TX_RX_COMPLETE_CB:
bsp_spi_callback.xxx.TxRxCpltCallback = callback;
break;
case BSP_SPI_TX_HALF_COMPLETE_CB:
bsp_spi_callback.xxx.TxHalfCpltCallback =
callback; break; case BSP_SPI_RX_HALF_COMPLETE_CB:
bsp_spi_callback.xxx.RxHalfCpltCallback =
callback; break; case BSP_SPI_TX_RX_HALF_COMPLETE_CB:
bsp_spi_callback.xxx.TxRxHalfCpltCallback =
callback; break; case BSP_SPI_ERROR_CB: bsp_spi_callback.xxx.ErrorCallback
= callback; break; case BSP_SPI_ABORT_CB:
bsp_spi_callback.xxx.AbortCpltCallback = callback;
break;
default:
return -1;
}
break;
*/
}
return 0;
}
int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t len) {
if (data == NULL) return -1;
switch (spi) {
case BSP_SPI_IMU:
/* Do NOT use hardware NSS. It doesn't implement the same logic. */
IMU_SPI_NSS_Reset();
HAL_SPI_Transmit(&hspi5, data, len, 55);
break;
case BSP_SPI_OLED:
HAL_SPI_Transmit(&hspi1, data, len, 55);
// HAL_SPI_Transmit_DMA(&hspi1, data, len);
break;
/*
case BSP_SPI_XXX:
HAL_SPI_Transmit_DMA(&hspix, data, len);
break;
*/
}
return 0;
}
int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t len) {
if (data == NULL) return -1;
switch (spi) {
case BSP_SPI_IMU:
IMU_SPI_NSS_Reset();
if (len > 1u) {
HAL_SPI_Receive_DMA(&hspi5, data, len);
} else {
HAL_SPI_Receive(&hspi5, data, len, 55);
}
break;
case BSP_SPI_OLED:
return -1;
/*
case BSP_SPI_XXX:
HAL_SPI_Receive(&hspix, data, len);
break;
*/
}
return 0;
}

198
User/bsp/a/bsp_uart.c Normal file
View File

@@ -0,0 +1,198 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_uart.h"
#include "usart.h"
/* Private define ----------------------------------------------------------- */
#define DR16_UART USART1
/* #define XXX_UART USARTX */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static struct {
struct {
void (*TxHalfCpltCallback)(void); /* UART Tx Half Complete Callback */
void (*TxCpltCallback)(void); /* UART Tx Complete Callback */
void (*RxHalfCpltCallback)(void); /* UART Rx Half Complete Callback */
void (*RxCpltCallback)(void); /* UART Rx Complete Callback */
void (*ErrorCallback)(void); /* UART Error Callback */
void (*AbortCpltCallback)(void); /* UART Abort Complete Callback */
void (*AbortTransmitCpltCallback)(
void); /* UART Abort Transmit Complete Callback */
void (*AbortReceiveCpltCallback)(
void); /* UART Abort Receive Complete Callback */
} dr16;
/* Add other uart device here. */
} bsp_uart_callback;
/* Private function -------------------------------------------------------- */
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.TxCpltCallback != NULL) {
bsp_uart_callback.dr16.TxCpltCallback();
}
}
}
void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.TxHalfCpltCallback != NULL) {
bsp_uart_callback.dr16.TxHalfCpltCallback();
}
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.RxCpltCallback != NULL) {
bsp_uart_callback.dr16.RxCpltCallback();
}
}
}
void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.RxHalfCpltCallback != NULL) {
bsp_uart_callback.dr16.RxHalfCpltCallback();
}
}
}
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.ErrorCallback != NULL) {
bsp_uart_callback.dr16.ErrorCallback();
}
}
}
void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.AbortCpltCallback != NULL) {
bsp_uart_callback.dr16.AbortCpltCallback();
}
}
}
void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.AbortTransmitCpltCallback != NULL) {
bsp_uart_callback.dr16.AbortTransmitCpltCallback();
}
}
}
void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == DR16_UART) {
if (bsp_uart_callback.dr16.AbortReceiveCpltCallback != NULL) {
bsp_uart_callback.dr16.AbortReceiveCpltCallback();
}
}
/*
else if (hspi->Instance == XXX_UART) {
if (bsp_uart_callback.xxx.AbortReceiveCpltCallback != NULL) {
bsp_uart_callback.xxx.AbortReceiveCpltCallback();
}
}
*/
}
/* Exported functions ------------------------------------------------------- */
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return -1;
switch (uart) {
case BSP_UART_DR16:
switch (type) {
case BSP_UART_TX_HALFCOMPLETE_CB:
bsp_uart_callback.dr16.TxHalfCpltCallback = callback;
break;
case BSP_UART_TX_COMPLETE_CB:
bsp_uart_callback.dr16.TxCpltCallback = callback;
break;
case BSP_UART_RX_HALFCOMPLETE_CB:
bsp_uart_callback.dr16.RxHalfCpltCallback = callback;
break;
case BSP_UART_RX_COMPLETE_CB:
bsp_uart_callback.dr16.RxCpltCallback = callback;
break;
case BSP_UART_ERROR_CB:
bsp_uart_callback.dr16.ErrorCallback = callback;
break;
case BSP_UART_ABORT_COMPLETE_CB:
bsp_uart_callback.dr16.AbortCpltCallback = callback;
break;
case BSP_UART_ABORT_TRANSMIT_COMPLETE_CB:
bsp_uart_callback.dr16.AbortTransmitCpltCallback = callback;
break;
case BSP_UART_ABORT_RECEIVE_COMPLETE_CB:
bsp_uart_callback.dr16.AbortReceiveCpltCallback = callback;
break;
default:
return -1;
}
break;
/*
case BSP_UART_XXX:
switch (type) {
case BSP_UART_TX_HALFCOMPLETE_CB:
bsp_uart_callback.xxx.TxHalfCpltCallback = callback;
break;
case BSP_UART_TX_COMPLETE_CB:
bsp_uart_callback.xxx.TxCpltCallback = callback;
break;
case BSP_UART_RX_HALFCOMPLETE_CB:
bsp_uart_callback.xxx.RxHalfCpltCallback = callback;
break;
case BSP_UART_RX_COMPLETE_CB:
bsp_uart_callback.xxx.RxCpltCallback = callback;
break;
case BSP_UART_ERROR_CB:
bsp_uart_callback.xxx.ErrorCallback = callback;
break;
case BSP_UART_ABORT_COMPLETE_CB:
bsp_uart_callback.xxx.AbortCpltCallback = callback;
break;
case BSP_UART_ABORT_TRANSMIT_COMPLETE_CB:
bsp_uart_callback.xxx.AbortTransmitCpltCallback =
callback; break; case BSP_UART_ABORT_RECEIVE_COMPLETE_CB:
bsp_uart_callback.xxx.AbortReceiveCpltCallback =
callback; break; default: return -1;
}
break;
*/
default:
return -1;
}
return 0;
}
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t len) {
if (data == NULL) return -1;
switch (uart) {
case BSP_UART_DR16:
return -1;
/*
case BSP_UART_XXX:
return -1;
*/
}
return 0;
}
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t len) {
if (data == NULL) return -1;
switch (uart) {
case BSP_UART_DR16:
HAL_UART_Receive_DMA(&huart1, data, len);
break;
/*
case BSP_UART_XXX:
return -1;
*/
}
return 0;
}

30
User/bsp/a/bsp_usb.c Normal file
View File

@@ -0,0 +1,30 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp_usb.h"
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
#include "usbd_cdc_if.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
uint8_t usb_tx_buf[512];
uint8_t usb_rx_buf[512];
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t BSP_USB_Printf(const char *fmt, ...) {
static va_list ap;
uint16_t len = 0;
va_start(ap, fmt);
len = vsprintf((char *)usb_tx_buf, fmt, ap);
va_end(ap);
CDC_Transmit_FS(usb_tx_buf, len);
return 0;
}

22
User/bsp/adc.h Normal file
View File

@@ -0,0 +1,22 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
float BSP_GetTemperature(void);
float BSP_GetBatteryVolt(void);
uint8_t BSP_GetHardwareVersion(void);
#ifdef __cplusplus
}
#endif

17
User/bsp/bsp.h Normal file
View File

@@ -0,0 +1,17 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#define BSP_OK (0)
#define BSP_ERR (-1)
#define BSP_ERR_NULL (-2)
#define BSP_ERR_INITED (-3)
#define BSP_ERR_NO_DEV (-4)
#define SIGNAL_BSP_USB_BUF_RECV (1u << 0)
#ifdef __cplusplus
}
#endif

22
User/bsp/buzzer.h Normal file
View File

@@ -0,0 +1,22 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_Buzzer_Start(void);
int8_t BSP_Buzzer_Set(float freq, float duty_cycle);
int8_t BSP_Buzzer_Stop(void);
#ifdef __cplusplus
}
#endif

50
User/bsp/c/adc.c Normal file
View File

@@ -0,0 +1,50 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\adc.h"
#include <adc.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile float volt_vrefint_proportion =
8.0586080586080586080586080586081e-4f;
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
float BSP_GetTemperature(void) {
ADC_ChannelConfTypeDef sConfig;
sConfig.Channel = ADC_CHANNEL_TEMPSENSOR;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) Error_Handler();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 5u);
uint32_t raw = HAL_ADC_GetValue(&hadc1);
float temp = (float)raw * volt_vrefint_proportion;
return (temp - 0.76f) * 400.0f + 25.0f;
}
float BSP_GetBatteryVolt(void) {
ADC_ChannelConfTypeDef sConfig;
sConfig.Channel = ADC_CHANNEL_8;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) Error_Handler();
HAL_ADC_Start(&hadc3);
HAL_ADC_PollForConversion(&hadc3, 5u);
uint32_t raw = HAL_ADC_GetValue(&hadc3);
float volt =
(float)raw * volt_vrefint_proportion * 10.090909090909090909090909090909f;
return volt;
}
uint8_t BSP_GetHardwareVersion(void) { return 1u; }

30
User/bsp/c/buzzer.c Normal file
View File

@@ -0,0 +1,30 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\buzzer.h"
#include <main.h>
#include <tim.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_Buzzer_Start(void) {
if (HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3) == HAL_OK) return BSP_OK;
return BSP_ERR;
}
int8_t BSP_Buzzer_Set(float freq, float duty_cycle) {
uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, pulse);
pulse = (uint16_t)freq;
__HAL_TIM_PRESCALER(&htim4, pulse);
return BSP_OK;
}
int8_t BSP_Buzzer_Stop(void) {
if (HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3) == HAL_OK) return BSP_OK;
return BSP_ERR;
}

141
User/bsp/c/can.c Normal file
View File

@@ -0,0 +1,141 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\can.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
if (hcan->Instance == CAN2)
return BSP_CAN_2;
else if (hcan->Instance == CAN1)
return BSP_CAN_1;
else
return BSP_CAN_ERR;
}
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB]();
}
}
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB]();
}
}
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB]();
}
}
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB]();
}
}
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB]();
}
}
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB]();
}
}
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB]();
}
}
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB]();
}
}
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB]();
}
}
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB]();
}
}
void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB])
CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB]();
}
}
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB])
CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB]();
}
}
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_ERROR_CB])
CAN_Callback[bsp_can][HAL_CAN_ERROR_CB]();
}
}
/* Exported functions ------------------------------------------------------- */
CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
switch (can) {
case BSP_CAN_2:
return &hcan2;
case BSP_CAN_1:
return &hcan1;
default:
return NULL;
}
}
int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
CAN_Callback[can][type] = callback;
return BSP_OK;
}

26
User/bsp/c/crc32.c Normal file
View File

@@ -0,0 +1,26 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\crc32.h"
#include <crc.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint32_t BSP_CRC32_Calc(uint8_t *buf, size_t len) {
return HAL_CRC_Calculate(&hcrc, (uint32_t *)buf, len / sizeof(uint32_t));
}
bool BSP_CRC32_Verify(uint8_t *buf, size_t len) {
if (len < 2) return false;
uint32_t expected = BSP_CRC32_Calc(buf, len / sizeof(uint32_t) - 1);
return expected == ((uint32_t *)buf)[len / sizeof(uint32_t) - 1];
}
bool BSP_CRC32_Append(uint8_t *buf, size_t len) {
return HAL_CRC_Accumulate(&hcrc, (uint32_t *)buf, len / sizeof(uint32_t));
}

34
User/bsp/c/delay.c Normal file
View File

@@ -0,0 +1,34 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\delay.h"
#include <cmsis_os2.h>
#include <main.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_Delay(uint32_t ms) {
uint32_t tick_period = 1000u / osKernelGetTickFreq();
uint32_t ticks = ms / tick_period;
switch (osKernelGetState()) {
case osKernelError:
case osKernelReserved:
case osKernelLocked:
case osKernelSuspended:
return BSP_ERR;
case osKernelRunning:
osDelay(ticks ? ticks : 1);
break;
case osKernelInactive:
case osKernelReady:
HAL_Delay(ms);
break;
}
return BSP_OK;
}

51
User/bsp/c/flash.c Normal file
View File

@@ -0,0 +1,51 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\flash.h"
#include <main.h>
#include <string.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void BSP_Flash_EraseSector(uint32_t sector) {
FLASH_EraseInitTypeDef flash_erase;
uint32_t sector_error;
#ifdef DEV_BOARD_C
if (sector > 0 && sector < 12) {
#elif
if (sector > 0 && sector < 24) {
#endif
flash_erase.Sector = sector;
flash_erase.TypeErase = FLASH_TYPEERASE_SECTORS;
flash_erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;
flash_erase.NbSectors = 1;
HAL_FLASH_Unlock();
while (FLASH_WaitForLastOperation(50) != HAL_OK)
;
HAL_FLASHEx_Erase(&flash_erase, &sector_error);
HAL_FLASH_Lock();
}
}
void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) {
HAL_FLASH_Unlock();
while (len > 0) {
while (FLASH_WaitForLastOperation(50) != HAL_OK)
;
HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, address, *buf);
address++;
buf++;
len--;
}
HAL_FLASH_Lock();
}
void BSP_Flash_ReadBytes(uint32_t address, void *buf, size_t len) {
memcpy(buf, (void *)address, len);
}

31
User/bsp/c/fric.c Normal file
View File

@@ -0,0 +1,31 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\fric.h"
#include <tim.h>
#include "bsp\delay.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_Fric_Start(void) {
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
BSP_Delay(500);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
return BSP_OK;
}
int8_t BSP_Fric_Set(float duty_cycle) {
uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pulse);
return BSP_OK;
}
int8_t BSP_Fric_Stop(void) {
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);
return BSP_OK;
}

93
User/bsp/c/gpio.c Normal file
View File

@@ -0,0 +1,93 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\gpio.h"
#include <gpio.h>
#include <main.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static void (*GPIO_Callback[16])(void);
/* Private function -------------------------------------------------------- */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
for (uint8_t i = 0; i < 16; i++) {
if (GPIO_Pin & (1 << i)) {
if (GPIO_Callback[i]) {
GPIO_Callback[i]();
}
}
}
}
/* Exported functions ------------------------------------------------------- */
int8_t BSP_GPIO_RegisterCallback(uint16_t pin, void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
for (uint8_t i = 0; i < 16; i++) {
if (pin & (1 << i)) {
GPIO_Callback[i] = callback;
break;
}
}
return BSP_OK;
}
int8_t BSP_GPIO_EnableIRQ(uint16_t pin) {
switch (pin) {
case ACCL_INT_Pin:
HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn);
break;
case GYRO_INT_Pin:
HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn);
break;
case USER_KEY_Pin:
HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn);
break;
case CMPS_INT_Pin:
HAL_NVIC_EnableIRQ(CMPS_INT_EXTI_IRQn);
break;
/*
case XXX_Pin:
HAL_NVIC_EnableIRQ(XXX_IRQn);
break;
*/
default:
return BSP_ERR;
}
return BSP_OK;
}
int8_t BSP_GPIO_DisableIRQ(uint16_t pin) {
switch (pin) {
case ACCL_INT_Pin:
HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn);
break;
case GYRO_INT_Pin:
HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn);
break;
case USER_KEY_Pin:
HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn);
break;
case CMPS_INT_Pin:
HAL_NVIC_DisableIRQ(CMPS_INT_EXTI_IRQn);
break;
/*
case XXX_Pin:
HAL_NVIC_EnableIRQ(XXX_IRQn);
break;
*/
default:
return BSP_ERR;
}
return BSP_OK;
}

112
User/bsp/c/i2c.c Normal file
View File

@@ -0,0 +1,112 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\i2c.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static void (*I2C_Callback[BSP_I2C_NUM][BSP_I2C_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_I2C_t I2C_Get(I2C_HandleTypeDef *hi2c) {
if (hi2c->Instance == I2C3) return BSP_I2C_COMP;
/*
else if (hi2c->Instance == I2CX)
return BSP_I2C_XXX;
*/
else
return BSP_I2C_ERR;
}
void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_MASTER_TX_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_MASTER_TX_CPLT_CB]();
}
}
void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_MASTER_RX_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_MASTER_RX_CPLT_CB]();
}
}
void HAL_I2C_SlaveTxCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_TX_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_TX_CPLT_CB]();
}
}
void HAL_I2C_SlaveRxCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_RX_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_RX_CPLT_CB]();
}
}
void HAL_I2C_ListenCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_LISTEN_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_LISTEN_CPLT_CB]();
}
}
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_MEM_TX_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_MEM_TX_CPLT_CB]();
}
}
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_MEM_RX_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_MEM_RX_CPLT_CB]();
}
}
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_ERROR_CB])
I2C_Callback[bsp_i2c][HAL_I2C_ERROR_CB]();
}
}
void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c) {
BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
if (bsp_i2c != BSP_I2C_ERR) {
if (I2C_Callback[bsp_i2c][HAL_I2C_ABORT_CPLT_CB])
I2C_Callback[bsp_i2c][HAL_I2C_ABORT_CPLT_CB]();
}
}
/* Exported functions ------------------------------------------------------- */
I2C_HandleTypeDef *BSP_I2C_GetHandle(BSP_I2C_t i2c) {
switch (i2c) {
case BSP_I2C_COMP:
return &hi2c3;
/*
case BSP_I2C_XXX:
return &hi2cX;
*/
default:
return NULL;
}
}
int8_t BSP_I2C_RegisterCallback(BSP_I2C_t i2c, BSP_I2C_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
I2C_Callback[i2c][type] = callback;
return BSP_OK;
}

30
User/bsp/c/laser.c Normal file
View File

@@ -0,0 +1,30 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\laser.h"
#include <tim.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_Laser_Start(void) {
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
return BSP_OK;
}
int8_t BSP_Laser_Set(float duty_cycle) {
if (duty_cycle > 1.0f) return BSP_ERR;
uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, pulse);
return BSP_OK;
}
int8_t BSP_Laser_Stop(void) {
HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_3);
return BSP_OK;
}

61
User/bsp/c/led.c Normal file
View File

@@ -0,0 +1,61 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\led.h"
#include <tim.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static uint32_t led_stats;
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_LED_Set(BSP_LED_Channel_t ch, BSP_LED_Status_t s, float duty_cycle) {
if (duty_cycle > 1.0f) return BSP_ERR;
uint32_t tim_ch;
uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
switch (ch) {
case BSP_LED_RED:
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, pulse);
tim_ch = TIM_CHANNEL_3;
break;
case BSP_LED_GRN:
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, pulse);
tim_ch = TIM_CHANNEL_2;
break;
case BSP_LED_BLU:
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, pulse);
tim_ch = TIM_CHANNEL_1;
break;
}
switch (s) {
case BSP_LED_ON:
HAL_TIM_PWM_Start(&htim5, tim_ch);
led_stats |= tim_ch;
break;
case BSP_LED_OFF:
HAL_TIM_PWM_Stop(&htim5, tim_ch);
led_stats &= ~tim_ch;
break;
case BSP_LED_TAGGLE:
if (led_stats & tim_ch) {
HAL_TIM_PWM_Stop(&htim5, tim_ch);
led_stats &= ~tim_ch;
} else {
HAL_TIM_PWM_Start(&htim5, tim_ch);
led_stats |= tim_ch;
}
break;
}
return BSP_OK;
}

14
User/bsp/c/mm.c Normal file
View File

@@ -0,0 +1,14 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\mm.h"
#include "FreeRTOS.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); }
inline void BSP_Free(void *pv) { vPortFree(pv); }

67
User/bsp/c/pwm.c Normal file
View File

@@ -0,0 +1,67 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\pwm.h"
#include <main.h>
#include <tim.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
switch (ch) {
case BSP_PWM_IMU_HEAT:
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
break;
case BSP_PWM_SHOOT_SERVO:
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
break;
}
return BSP_OK;
}
int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) {
if (duty_cycle > 1.0f) return BSP_ERR;
if (duty_cycle < 0.0f) duty_cycle = 0.f;
uint16_t pulse;
/* 通过PWM通道对应定时器重载值和给定占空比，计算PWM周期值 */
switch (ch) {
case BSP_PWM_IMU_HEAT:
pulse = (uint16_t)(duty_cycle * (float)__HAL_TIM_GET_AUTORELOAD(&htim10));
break;
case BSP_PWM_SHOOT_SERVO:
pulse = (uint16_t)(duty_cycle * (float)__HAL_TIM_GET_AUTORELOAD(&htim1));
break;
}
if (pulse > 0) {
switch (ch) {
case BSP_PWM_IMU_HEAT:
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, pulse);
break;
case BSP_PWM_SHOOT_SERVO:
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse);
break;
}
} else {
BSP_PWM_Stop(ch);
}
return BSP_OK;
}
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) {
switch (ch) {
case BSP_PWM_IMU_HEAT:
HAL_TIM_PWM_Stop(&htim10, TIM_CHANNEL_1);
break;
case BSP_PWM_SHOOT_SERVO:
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
break;
}
HAL_GPIO_WritePin(IMU_HEAT_PWM_GPIO_Port, IMU_HEAT_PWM_Pin, GPIO_PIN_RESET);
return BSP_OK;
}

11
User/bsp/c/rand.c Normal file
View File

@@ -0,0 +1,11 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\rand.h"
#include "rng.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

128
User/bsp/c/servo.c Normal file
View File

@@ -0,0 +1,128 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\servo.h"
#include <main.h>
#include <tim.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static float range[BSP_SERVO_NUM];
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_Servo_Init(BSP_Servo_Channel_t ch, float max_angle) {
range[ch] = max_angle;
return BSP_OK;
}
int8_t BSP_Servo_Start(BSP_Servo_Channel_t ch) {
switch (ch) {
case BSP_SERVO_A:
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
break;
case BSP_SERVO_B:
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
break;
case BSP_SERVO_C:
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
break;
case BSP_SERVO_D:
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
break;
case BSP_SERVO_E:
HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
break;
case BSP_SERVO_F:
HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
break;
case BSP_SERVO_G:
HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
case BSP_SERVO_NUM:
break;
}
return BSP_OK;
}
int8_t BSP_Servo_Set(BSP_Servo_Channel_t ch, uint8_t angle) {
if (angle > 1.0f) return BSP_ERR;
uint16_t pulse = angle * UINT16_MAX;
switch (ch) {
case BSP_SERVO_A:
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse);
break;
case BSP_SERVO_B:
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pulse);
break;
case BSP_SERVO_C:
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pulse);
break;
case BSP_SERVO_D:
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pulse);
break;
case BSP_SERVO_E:
__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, pulse);
break;
case BSP_SERVO_F:
__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, pulse);
break;
case BSP_SERVO_G:
__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, pulse);
case BSP_SERVO_NUM:
break;
}
return BSP_OK;
}
int8_t BSP_Servo_Stop(BSP_Servo_Channel_t ch) {
switch (ch) {
case BSP_SERVO_A:
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
break;
case BSP_SERVO_B:
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
break;
case BSP_SERVO_C:
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
break;
case BSP_SERVO_D:
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);
break;
case BSP_SERVO_E:
HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_1);
break;
case BSP_SERVO_F:
HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_2);
break;
case BSP_SERVO_G:
HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_3);
case BSP_SERVO_NUM:
break;
}
return BSP_OK;
}

110
User/bsp/c/spi.c Normal file
View File

@@ -0,0 +1,110 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\spi.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == SPI1)
return BSP_SPI_IMU;
else if (hspi->Instance == SPI2)
return BSP_SPI_OLED;
/*
else if (hspi->Instance == SPIX)
return BSP_SPI_XXX;
*/
else
return BSP_SPI_ERR;
}
void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) {
SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]();
}
}
}
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]();
}
}
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]();
}
}
void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]();
}
}
void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]();
}
}
void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]();
}
}
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]();
}
}
void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]();
}
}
/* Exported functions ------------------------------------------------------- */
SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) {
switch (spi) {
case BSP_SPI_OLED:
return &hspi2;
case BSP_SPI_IMU:
return &hspi1;
/*
case BSP_SPI_XXX:
return &hspiX;
*/
default:
return NULL;
}
}
int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
SPI_Callback[spi][type] = callback;
return BSP_OK;
}

130
User/bsp/c/uart.c Normal file
View File

@@ -0,0 +1,130 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\uart.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
if (huart->Instance == USART3)
return BSP_UART_DR16;
else if (huart->Instance == USART1)
return BSP_UART_REF;
else if (huart->Instance == USART6)
return BSP_UART_AI;
/*
else if (huart->Instance == USARTX)
return BSP_UART_XXX;
*/
else
return BSP_UART_ERR;
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]();
}
}
}
void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]();
}
}
}
void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) {
UART_Callback[bsp_uart][BSP_UART_ERROR_CB]();
}
}
}
void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]();
}
}
}
void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]();
}
}
}
void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]();
}
}
}
/* Exported functions ------------------------------------------------------- */
void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) {
if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) {
__HAL_UART_CLEAR_IDLEFLAG(huart);
if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) {
UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]();
}
}
}
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) {
case BSP_UART_DR16:
return &huart3;
case BSP_UART_REF:
return &huart1;
case BSP_UART_AI:
return &huart6;
/*
case BSP_UART_XXX:
return &huartX;
*/
default:
return NULL;
}
}
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
UART_Callback[uart][type] = callback;
return BSP_OK;
}

61
User/bsp/c/usb.c Normal file
View File

@@ -0,0 +1,61 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\usb.h"
#include <stdarg.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include "bsp\delay.h"
#include "usbd_cdc_if.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Must set to NULL explicitly. */
osThreadId_t gbsp_usb_alert = NULL;
uint8_t usb_rx_buf[BSP_USB_MAX_RX_LEN];
uint8_t usb_tx_buf[BSP_USB_MAX_TX_LEN];
/* Private function -------------------------------------------------------- */
static int8_t BSP_USB_Transmit(uint8_t *buffer, uint16_t len) {
uint8_t retry = 0;
do {
if (CDC_Transmit_FS(buffer, len) != USBD_OK) {
retry++;
BSP_Delay(10);
} else {
break;
}
} while (retry < 3);
return BSP_OK;
}
/* Exported functions ------------------------------------------------------- */
int8_t BSP_USB_ReadyReceive(osThreadId_t alert) {
if (alert == NULL) return BSP_ERR_NULL;
gbsp_usb_alert = alert;
CDC_ReadyReceive();
return BSP_OK;
}
char BSP_USB_ReadChar(void) { return usb_rx_buf[0]; }
int8_t BSP_USB_Printf(const char *fmt, ...) {
va_list ap;
uint16_t len = 0;
va_start(ap, fmt);
len =
(uint16_t)vsnprintf((char *)usb_tx_buf, BSP_USB_MAX_TX_LEN - 1, fmt, ap);
va_end(ap);
if (len > 0) {
BSP_USB_Transmit(usb_tx_buf, len);
return BSP_OK;
} else {
return BSP_ERR_NULL;
}
}

46
User/bsp/can.h Normal file
View File

@@ -0,0 +1,46 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <can.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
BSP_CAN_1,
BSP_CAN_2,
BSP_CAN_NUM,
BSP_CAN_ERR,
} BSP_CAN_t;
typedef enum {
HAL_CAN_TX_MAILBOX0_CPLT_CB,
HAL_CAN_TX_MAILBOX1_CPLT_CB,
HAL_CAN_TX_MAILBOX2_CPLT_CB,
HAL_CAN_TX_MAILBOX0_ABORT_CB,
HAL_CAN_TX_MAILBOX1_ABORT_CB,
HAL_CAN_TX_MAILBOX2_ABORT_CB,
HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
HAL_CAN_RX_FIFO0_FULL_CB,
HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
HAL_CAN_RX_FIFO1_FULL_CB,
HAL_CAN_SLEEP_CB,
HAL_CAN_WAKEUP_FROM_RX_MSG_CB,
HAL_CAN_ERROR_CB,
BSP_CAN_CB_NUM
} BSP_CAN_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can);
int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
void (*callback)(void));
#ifdef __cplusplus
}
#endif

24
User/bsp/crc32.h Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
uint32_t BSP_CRC32_Calc(uint8_t *buf, size_t len);
bool BSP_CRC32_Verify(uint8_t *buf, size_t len);
bool BSP_CRC32_Append(uint8_t *buf, size_t len);
#ifdef __cplusplus
}
#endif

20
User/bsp/delay.h Normal file
View File

@@ -0,0 +1,20 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_Delay(uint32_t ms);
#ifdef __cplusplus
}
#endif

78
User/bsp/flash.h Normal file
View File

@@ -0,0 +1,78 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------ */
#include <main.h>
#include "bsp/bsp.h"
/* Exported constants -------------------------------------------------------- */
/* Base address of the Flash sectors */
#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000)
/* Base address of Sector 0, 16 Kbytes */
#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08004000)
/* Base address of Sector 1, 16 Kbytes */
#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08008000)
/* Base address of Sector 2, 16 Kbytes */
#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x0800C000)
/* Base address of Sector 3, 16 Kbytes */
#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08010000)
/* Base address of Sector 4, 64 Kbytes */
#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08020000)
/* Base address of Sector 5, 128 Kbytes */
#define ADDR_FLASH_SECTOR_6 ((uint32_t)0x08040000)
/* Base address of Sector 6, 128 Kbytes */
#define ADDR_FLASH_SECTOR_7 ((uint32_t)0x08060000)
/* Base address of Sector 7, 128 Kbytes */
#define ADDR_FLASH_SECTOR_8 ((uint32_t)0x08080000)
/* Base address of Sector 8, 128 Kbytes */
#define ADDR_FLASH_SECTOR_9 ((uint32_t)0x080A0000)
/* Base address of Sector 9, 128 Kbytes */
#define ADDR_FLASH_SECTOR_10 ((uint32_t)0x080C0000)
/* Base address of Sector 10, 128 Kbytes */
#define ADDR_FLASH_SECTOR_11 ((uint32_t)0x080E0000)
/* Base address of Sector 11, 128 Kbytes */
#ifdef DEV_BOARD_C
#define ADDR_FLASH_END ((uint32_t)0x08100000) /* End address for flash */
#else
#define ADDR_FLASH_SECTOR_12 ((uint32_t)0x08100000)
/* Base address of Sector 12, 16 Kbytes */
#define ADDR_FLASH_SECTOR_13 ((uint32_t)0x08104000)
/* Base address of Sector 13, 16 Kbytes */
#define ADDR_FLASH_SECTOR_14 ((uint32_t)0x08108000)
/* Base address of Sector 14, 16 Kbytes */
#define ADDR_FLASH_SECTOR_15 ((uint32_t)0x0810C000)
/* Base address of Sector 15, 16 Kbytes */
#define ADDR_FLASH_SECTOR_16 ((uint32_t)0x08110000)
/* Base address of Sector 16, 64 Kbytes */
#define ADDR_FLASH_SECTOR_17 ((uint32_t)0x08120000)
/* Base address of Sector 17, 128 Kbytes */
#define ADDR_FLASH_SECTOR_18 ((uint32_t)0x08140000)
/* Base address of Sector 18, 128 Kbytes */
#define ADDR_FLASH_SECTOR_19 ((uint32_t)0x08160000)
/* Base address of Sector 19, 128 Kbytes */
#define ADDR_FLASH_SECTOR_20 ((uint32_t)0x08180000)
/* Base address of Sector 20, 128 Kbytes */
#define ADDR_FLASH_SECTOR_21 ((uint32_t)0x081A0000)
/* Base address of Sector 21, 128 Kbytes */
#define ADDR_FLASH_SECTOR_22 ((uint32_t)0x081C0000)
/* Base address of Sector 22, 128 Kbytes */
#define ADDR_FLASH_SECTOR_23 ((uint32_t)0x081E0000)
/* Base address of Sector 23, 128 Kbytes */
#define ADDR_FLASH_END ((uint32_t)0x08200000) /* End address for flash */
#endif
/* Exported macro ------------------------------------------------------------ */
/* Exported types ------------------------------------------------------------ */
/* Exported functions prototypes --------------------------------------------- */
void BSP_Flash_EraseSector(uint32_t sector);
void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len);
void BSP_Flash_ReadBytes(uint32_t address, void *buf, size_t len);
#ifdef __cplusplus
}
#endif

22
User/bsp/fric.h Normal file
View File

@@ -0,0 +1,22 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_Fric_Start(void);
int8_t BSP_Fric_Set(float duty_cycle);
int8_t BSP_Fric_Stop(void);
#ifdef __cplusplus
}
#endif

23
User/bsp/gpio.h Normal file
View File

@@ -0,0 +1,23 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_GPIO_RegisterCallback(uint16_t pin, void (*callback)(void));
int8_t BSP_GPIO_EnableIRQ(uint16_t pin);
int8_t BSP_GPIO_DisableIRQ(uint16_t pin);
#ifdef __cplusplus
}
#endif

47
User/bsp/i2c.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <i2c.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用I2C的新设备，需要先在此添加对应的枚举值 */
/* I2C实体枚举，与设备对应 */
typedef enum {
BSP_I2C_COMP,
/* BSP_I2C_XXX,*/
BSP_I2C_NUM,
BSP_I2C_ERR,
} BSP_I2C_t;
/* I2C支持的中断回调函数类型，具体参考HAL中定义 */
typedef enum {
HAL_I2C_MASTER_TX_CPLT_CB,
HAL_I2C_MASTER_RX_CPLT_CB,
HAL_I2C_SLAVE_TX_CPLT_CB,
HAL_I2C_SLAVE_RX_CPLT_CB,
HAL_I2C_LISTEN_CPLT_CB,
HAL_I2C_MEM_TX_CPLT_CB,
HAL_I2C_MEM_RX_CPLT_CB,
HAL_I2C_ERROR_CB,
HAL_I2C_ABORT_CPLT_CB,
BSP_I2C_CB_NUM,
} BSP_I2C_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
I2C_HandleTypeDef *BSP_I2C_GetHandle(BSP_I2C_t i2c);
int8_t BSP_I2C_RegisterCallback(BSP_I2C_t i2c, BSP_I2C_Callback_t type,
void (*callback)(void));
#ifdef __cplusplus
}
#endif

24
User/bsp/laser.h Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
/* duty_cycle大于零时，A板为全开，C板为pwm调光*/
int8_t BSP_Laser_Start(void);
int8_t BSP_Laser_Set(float duty_cycle);
int8_t BSP_Laser_Stop(void);
#ifdef __cplusplus
}
#endif

48
User/bsp/led.h Normal file
View File

@@ -0,0 +1,48 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* LED灯状态，设置用 */
typedef enum {
BSP_LED_ON,
BSP_LED_OFF,
BSP_LED_TAGGLE,
} BSP_LED_Status_t;
/* LED通道 */
typedef enum {
#ifdef DEV_BOARD_A
BSP_LED1,
BSP_LED2,
BSP_LED3,
BSP_LED4,
BSP_LED5,
BSP_LED6,
BSP_LED7,
BSP_LED8,
#elif defined DEV_BOARD_C
BSP_LED_BLU,
#endif
BSP_LED_RED,
BSP_LED_GRN,
} BSP_LED_Channel_t;
/* Exported functions prototypes -------------------------------------------- */
/* 用于A板时，会无视duty_cycle的值。使用B板时，duty_cycle才有效*/
int8_t BSP_LED_Set(BSP_LED_Channel_t ch, BSP_LED_Status_t s, float duty_cycle);
#ifdef __cplusplus
}
#endif

20
User/bsp/mm.h Normal file
View File

@@ -0,0 +1,20 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stddef.h>
#include <stdint.h>
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
void *BSP_Malloc(size_t size);
void BSP_Free(void *pv);
#ifdef __cplusplus
}
#endif

28
User/bsp/power.h Normal file
View File

@@ -0,0 +1,28 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdbool.h>
#include <stdint.h>
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 电源输出接口 */
typedef enum {
POWER_PORT1,
POWER_PORT2,
POWER_PORT3,
POWER_PORT4,
} BSP_Power_Port_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_Power_Set(BSP_Power_Port_t port, bool s);
#ifdef __cplusplus
}
#endif

29
User/bsp/pwm.h Normal file
View File

@@ -0,0 +1,29 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* PWM通道 */
typedef enum {
BSP_PWM_IMU_HEAT,
BSP_PWM_SHOOT_SERVO,
} BSP_PWM_Channel_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle);
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
#ifdef __cplusplus
}
#endif

21
User/bsp/rand.h Normal file
View File

@@ -0,0 +1,21 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
uint32_t BSP_GetRandomNum(void);
int32_t BSP_GetRandomRangle(int32_t min, int32_t max);
#ifdef __cplusplus
}
#endif

47
User/bsp/servo.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 舵机通道 */
typedef enum {
BSP_SERVO_A = 0,
BSP_SERVO_B,
BSP_SERVO_C,
BSP_SERVO_D,
BSP_SERVO_E,
BSP_SERVO_F,
BSP_SERVO_G,
#ifdef DEV_BOARD_A
BSP_SERVO_H,
BSP_SERVO_S,
BSP_SERVO_T,
BSP_SERVO_U,
BSP_SERVO_V,
BSP_SERVO_W,
BSP_SERVO_X,
BSP_SERVO_Y,
BSP_SERVO_Z,
#endif
BSP_SERVO_NUM,
} BSP_Servo_Channel_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_Servo_Init(BSP_Servo_Channel_t ch, float max_angle);
int8_t BSP_Servo_Start(BSP_Servo_Channel_t ch);
int8_t BSP_Servo_Set(BSP_Servo_Channel_t ch, uint8_t angle);
int8_t BSP_Servo_Stop(BSP_Servo_Channel_t ch);
#ifdef __cplusplus
}
#endif

47
User/bsp/spi.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <spi.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */
/* SPI实体枚举，与设备对应 */
typedef enum {
BSP_SPI_OLED,
BSP_SPI_IMU,
/* BSP_SPI_XXX,*/
BSP_SPI_NUM,
BSP_SPI_ERR,
} BSP_SPI_t;
/* SPI支持的中断回调函数类型，具体参考HAL中定义 */
typedef enum {
BSP_SPI_TX_CPLT_CB,
BSP_SPI_RX_CPLT_CB,
BSP_SPI_TX_RX_CPLT_CB,
BSP_SPI_TX_HALF_CPLT_CB,
BSP_SPI_RX_HALF_CPLT_CB,
BSP_SPI_TX_RX_HALF_CPLT_CB,
BSP_SPI_ERROR_CB,
BSP_SPI_ABORT_CPLT_CB,
BSP_SPI_CB_NUM,
} BSP_SPI_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi);
int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
void (*callback)(void));
#ifdef __cplusplus
}
#endif

50
User/bsp/uart.h Normal file
View File

@@ -0,0 +1,50 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */
/* UART实体枚举，与设备对应 */
typedef enum {
BSP_UART_DR16,
BSP_UART_REF,
BSP_UART_AI,
/* BSP_UART_XXX, */
BSP_UART_NUM,
BSP_UART_ERR,
} BSP_UART_t;
/* UART支持的中断回调函数类型，具体参考HAL中定义 */
typedef enum {
BSP_UART_TX_HALF_CPLT_CB,
BSP_UART_TX_CPLT_CB,
BSP_UART_RX_HALF_CPLT_CB,
BSP_UART_RX_CPLT_CB,
BSP_UART_ERROR_CB,
BSP_UART_ABORT_CPLT_CB,
BSP_UART_ABORT_TX_CPLT_CB,
BSP_UART_ABORT_RX_CPLT_CB,
BSP_UART_IDLE_LINE_CB,
BSP_UART_CB_NUM,
} BSP_UART_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart);
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void));
#ifdef __cplusplus
}
#endif

32
User/bsp/usb.h Normal file
View File

@@ -0,0 +1,32 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdint.h>
#include <string.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
#define BSP_USB_MAX_RX_LEN 128 /* USB接收数据缓冲区长度 */
#define BSP_USB_MAX_TX_LEN 1024 /* USB发送数据缓冲区长度 */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
extern osThreadId_t gbsp_usb_alert; /* 暴露给USB库用以提示任务收到新内容 */
extern uint8_t usb_rx_buf[BSP_USB_MAX_RX_LEN]; /* 暴露给USB库用以存储接收数据 */
extern uint8_t usb_tx_buf[BSP_USB_MAX_TX_LEN]; /* 暴露给USB库用以存储发送数据 */
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_USB_ReadyReceive(osThreadId_t alert); /* 指示USB库准备好接收数据 */
char BSP_USB_ReadChar(void); /* 读取缓存第一个字符 */
int8_t BSP_USB_Printf(const char *fmt, ...); /* 打印至虚拟串口 */
#ifdef __cplusplus
}
#endif

352
User/component/FreeRTOS_CLI.c Normal file
View File

@@ -0,0 +1,352 @@
/*
* FreeRTOS+CLI V1.0.4
* Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
/* Standard includes. */
#include <string.h>
#include <stdint.h>
/* FreeRTOS includes. */
#include "FreeRTOS.h"
#include "task.h"
/* Utils includes. */
#include "FreeRTOS_CLI.h"
/* If the application writer needs to place the buffer used by the CLI at a
fixed address then set configAPPLICATION_PROVIDES_cOutputBuffer to 1 in
FreeRTOSConfig.h, then declare an array with the following name and size in
one of the application files:
char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
*/
#ifndef configAPPLICATION_PROVIDES_cOutputBuffer
#define configAPPLICATION_PROVIDES_cOutputBuffer 0
#endif
typedef struct xCOMMAND_INPUT_LIST
{
const CLI_Command_Definition_t *pxCommandLineDefinition;
struct xCOMMAND_INPUT_LIST *pxNext;
} CLI_Definition_List_Item_t;
/*
* The callback function that is executed when "help" is entered. This is the
* only default command that is always present.
*/
static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
/*
* Return the number of parameters that follow the command name.
*/
static int8_t prvGetNumberOfParameters( const char *pcCommandString );
/* The definition of the "help" command. This command is always at the front
of the list of registered commands. */
static const CLI_Command_Definition_t xHelpCommand =
{
"help",
"\r\nhelp:\r\n Lists all the registered commands\r\n\r\n",
prvHelpCommand,
0
};
/* The definition of the list of commands. Commands that are registered are
added to this list. */
static CLI_Definition_List_Item_t xRegisteredCommands =
{
&xHelpCommand, /* The first command in the list is always the help command, defined in this file. */
NULL /* The next pointer is initialised to NULL, as there are no other registered commands yet. */
};
/* A buffer into which command outputs can be written is declared here, rather
than in the command console implementation, to allow multiple command consoles
to share the same buffer. For example, an application may allow access to the
command interpreter by UART and by Ethernet. Sharing a buffer is done purely
to save RAM. Note, however, that the command console itself is not re-entrant,
so only one command interpreter interface can be used at any one time. For that
reason, no attempt at providing mutual exclusion to the cOutputBuffer array is
attempted.
configAPPLICATION_PROVIDES_cOutputBuffer is provided to allow the application
writer to provide their own cOutputBuffer declaration in cases where the
buffer needs to be placed at a fixed address (rather than by the linker). */
#if( configAPPLICATION_PROVIDES_cOutputBuffer == 0 )
static char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
#else
extern char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
#endif
/*---------------------------------------------------------- */
BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister )
{
static CLI_Definition_List_Item_t *pxLastCommandInList = &xRegisteredCommands;
CLI_Definition_List_Item_t *pxNewListItem;
BaseType_t xReturn = pdFAIL;
/* Check the parameter is not NULL. */
configASSERT( pxCommandToRegister );
/* Create a new list item that will reference the command being registered. */
pxNewListItem = ( CLI_Definition_List_Item_t * ) pvPortMalloc( sizeof( CLI_Definition_List_Item_t ) );
configASSERT( pxNewListItem );
if( pxNewListItem != NULL )
{
taskENTER_CRITICAL();
{
/* Reference the command being registered from the newly created
list item. */
pxNewListItem->pxCommandLineDefinition = pxCommandToRegister;
/* The new list item will get added to the end of the list, so
pxNext has nowhere to point. */
pxNewListItem->pxNext = NULL;
/* Add the newly created list item to the end of the already existing
list. */
pxLastCommandInList->pxNext = pxNewListItem;
/* Set the end of list marker to the new list item. */
pxLastCommandInList = pxNewListItem;
}
taskEXIT_CRITICAL();
xReturn = pdPASS;
}
return xReturn;
}
/*---------------------------------------------------------- */
BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen )
{
static const CLI_Definition_List_Item_t *pxCommand = NULL;
BaseType_t xReturn = pdTRUE;
const char *pcRegisteredCommandString;
size_t xCommandStringLength;
/* Note: This function is not re-entrant. It must not be called from more
thank one task. */
if( pxCommand == NULL )
{
/* Search for the command string in the list of registered commands. */
for( pxCommand = &xRegisteredCommands; pxCommand != NULL; pxCommand = pxCommand->pxNext )
{
pcRegisteredCommandString = pxCommand->pxCommandLineDefinition->pcCommand;
xCommandStringLength = strlen( pcRegisteredCommandString );
/* To ensure the string lengths match exactly, so as not to pick up
a sub-string of a longer command, check the byte after the expected
end of the string is either the end of the string or a space before
a parameter. */
if( ( pcCommandInput[ xCommandStringLength ] == ' ' ) || ( pcCommandInput[ xCommandStringLength ] == 0x00 ) )
{
if( strncmp( pcCommandInput, pcRegisteredCommandString, xCommandStringLength ) == 0 )
{
/* The command has been found. Check it has the expected
number of parameters. If cExpectedNumberOfParameters is -1,
then there could be a variable number of parameters and no
check is made. */
if( pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters >= 0 )
{
if( prvGetNumberOfParameters( pcCommandInput ) != pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters )
{
xReturn = pdFALSE;
}
}
break;
}
}
}
}
if( ( pxCommand != NULL ) && ( xReturn == pdFALSE ) )
{
/* The command was found, but the number of parameters with the command
was incorrect. */
strncpy( pcWriteBuffer, "Incorrect command parameter(s). Enter \"help\" to view a list of available commands.\r\n\r\n", xWriteBufferLen );
pxCommand = NULL;
}
else if( pxCommand != NULL )
{
/* Call the callback function that is registered to this command. */
xReturn = pxCommand->pxCommandLineDefinition->pxCommandInterpreter( pcWriteBuffer, xWriteBufferLen, pcCommandInput );
/* If xReturn is pdFALSE, then no further strings will be returned
after this one, and pxCommand can be reset to NULL ready to search
for the next entered command. */
if( xReturn == pdFALSE )
{
pxCommand = NULL;
}
}
else
{
/* pxCommand was NULL, the command was not found. */
strncpy( pcWriteBuffer, "Command not recognised. Enter 'help' to view a list of available commands.\r\n\r\n", xWriteBufferLen );
xReturn = pdFALSE;
}
return xReturn;
}
/*---------------------------------------------------------- */
char *FreeRTOS_CLIGetOutputBuffer( void )
{
return cOutputBuffer;
}
/*---------------------------------------------------------- */
const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength )
{
UBaseType_t uxParametersFound = 0;
const char *pcReturn = NULL;
*pxParameterStringLength = 0;
while( uxParametersFound < uxWantedParameter )
{
/* Index the character pointer past the current word. If this is the start
of the command string then the first word is the command itself. */
while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
{
pcCommandString++;
}
/* Find the start of the next string. */
while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) == ' ' ) )
{
pcCommandString++;
}
/* Was a string found? */
if( *pcCommandString != 0x00 )
{
/* Is this the start of the required parameter? */
uxParametersFound++;
if( uxParametersFound == uxWantedParameter )
{
/* How long is the parameter? */
pcReturn = pcCommandString;
while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
{
( *pxParameterStringLength )++;
pcCommandString++;
}
if( *pxParameterStringLength == 0 )
{
pcReturn = NULL;
}
break;
}
}
else
{
break;
}
}
return pcReturn;
}
/*---------------------------------------------------------- */
static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
static const CLI_Definition_List_Item_t * pxCommand = NULL;
BaseType_t xReturn;
( void ) pcCommandString;
if( pxCommand == NULL )
{
/* Reset the pxCommand pointer back to the start of the list. */
pxCommand = &xRegisteredCommands;
}
/* Return the next command help string, before moving the pointer on to
the next command in the list. */
strncpy( pcWriteBuffer, pxCommand->pxCommandLineDefinition->pcHelpString, xWriteBufferLen );
pxCommand = pxCommand->pxNext;
if( pxCommand == NULL )
{
/* There are no more commands in the list, so there will be no more
strings to return after this one and pdFALSE should be returned. */
xReturn = pdFALSE;
}
else
{
xReturn = pdTRUE;
}
return xReturn;
}
/*---------------------------------------------------------- */
static int8_t prvGetNumberOfParameters( const char *pcCommandString )
{
int8_t cParameters = 0;
BaseType_t xLastCharacterWasSpace = pdFALSE;
/* Count the number of space delimited words in pcCommandString. */
while( *pcCommandString != 0x00 )
{
if( ( *pcCommandString ) == ' ' )
{
if( xLastCharacterWasSpace != pdTRUE )
{
cParameters++;
xLastCharacterWasSpace = pdTRUE;
}
}
else
{
xLastCharacterWasSpace = pdFALSE;
}
pcCommandString++;
}
/* If the command string ended with spaces, then there will have been too
many parameters counted. */
if( xLastCharacterWasSpace == pdTRUE )
{
cParameters--;
}
/* The value returned is one less than the number of space delimited words,
as the first word should be the command itself. */
return cParameters;
}

108
User/component/FreeRTOS_CLI.h Normal file
View File

@@ -0,0 +1,108 @@
/*
* FreeRTOS+CLI V1.0.4
* Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
#ifndef COMMAND_INTERPRETER_H
#define COMMAND_INTERPRETER_H
/* This config should be defined in FreeRTOSConfig.h. But due to the limition of CubeMX I put it here. */
#define configCOMMAND_INT_MAX_OUTPUT_SIZE 512
/* The prototype to which callback functions used to process command line
commands must comply. pcWriteBuffer is a buffer into which the output from
executing the command can be written, xWriteBufferLen is the length, in bytes of
the pcWriteBuffer buffer, and pcCommandString is the entire string as input by
the user (from which parameters can be extracted).*/
typedef BaseType_t (*pdCOMMAND_LINE_CALLBACK)( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
/* The structure that defines command line commands. A command line command
should be defined by declaring a const structure of this type. */
typedef struct xCOMMAND_LINE_INPUT
{
const char * const pcCommand; /* The command that causes pxCommandInterpreter to be executed. For example "help". Must be all lower case. */
const char * const pcHelpString; /* String that describes how to use the command. Should start with the command itself, and end with "\r\n". For example "help: Returns a list of all the commands\r\n". */
const pdCOMMAND_LINE_CALLBACK pxCommandInterpreter; /* A pointer to the callback function that will return the output generated by the command. */
int8_t cExpectedNumberOfParameters; /* Commands expect a fixed number of parameters, which may be zero. */
} CLI_Command_Definition_t;
/* For backward compatibility. */
#define xCommandLineInput CLI_Command_Definition_t
/*
* Register the command passed in using the pxCommandToRegister parameter.
* Registering a command adds the command to the list of commands that are
* handled by the command interpreter. Once a command has been registered it
* can be executed from the command line.
*/
BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister );
/*
* Runs the command interpreter for the command string "pcCommandInput". Any
* output generated by running the command will be placed into pcWriteBuffer.
* xWriteBufferLen must indicate the size, in bytes, of the buffer pointed to
* by pcWriteBuffer.
*
* FreeRTOS_CLIProcessCommand should be called repeatedly until it returns pdFALSE.
*
* pcCmdIntProcessCommand is not reentrant. It must not be called from more
* than one task - or at least - by more than one task at a time.
*/
BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen );
/*---------------------------------------------------------- */
/*
* A buffer into which command outputs can be written is declared in the
* main command interpreter, rather than in the command console implementation,
* to allow application that provide access to the command console via multiple
* interfaces to share a buffer, and therefore save RAM. Note, however, that
* the command interpreter itself is not re-entrant, so only one command
* console interface can be used at any one time. For that reason, no attempt
* is made to provide any mutual exclusion mechanism on the output buffer.
*
* FreeRTOS_CLIGetOutputBuffer() returns the address of the output buffer.
*/
char *FreeRTOS_CLIGetOutputBuffer( void );
/*
* Return a pointer to the xParameterNumber'th word in pcCommandString.
*/
const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength );
#endif /* COMMAND_INTERPRETER_H */

405
User/component/ahrs.c Normal file
View File

@@ -0,0 +1,405 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#include "ahrs.h"
#include <string.h>
#include "user_math.h"
#define BETA_IMU (0.033f)
#define BETA_AHRS (0.041f)
/* 2 * proportional gain (Kp) */
static float beta = BETA_IMU;
/**
* @brief 不使用磁力计计算姿态
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @return int8_t 0对应没有错误
*/
static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_IMU;
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
q3q3;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_4q0 = 4.0f * ahrs->quat.q0;
_4q1 = 4.0f * ahrs->quat.q1;
_4q2 = 4.0f * ahrs->quat.q2;
_8q1 = 8.0f * ahrs->quat.q1;
_8q2 = 8.0f * ahrs->quat.q2;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Gradient decent algorithm corrective step */
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 -
_2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 -
_2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax +
4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay;
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) {
if (ahrs == NULL) return -1;
ahrs->inv_sample_freq = 1.0f / sample_freq;
ahrs->quat.q0 = 1.0f;
ahrs->quat.q1 = 0.0f;
ahrs->quat.q2 = 0.0f;
ahrs->quat.q3 = 0.0f;
if (magn) {
float yaw = -atan2(magn->y, magn->x);
if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.997458339f;
ahrs->quat.q1 = 0.000336312107f;
ahrs->quat.q2 = -0.0057230792f;
ahrs->quat.q3 = 0.0740156546;
} else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > M_PI)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
}
}
return 0;
}
/**
* @brief 姿态运算更新一次
* @note 输入数据必须是NED(North East Down) 参考坐标系
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_AHRS;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
_2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
q2q2, q2q3, q3q3;
if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro);
float mx = magn->x;
float my = magn->y;
float mz = magn->z;
/* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */
/* magnetometer normalisation) */
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
return AHRS_UpdateIMU(ahrs, accl, gyro);
}
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Normalise magnetometer measurement */
recip_norm = InvSqrt(mx * mx + my * my + mz * mz);
mx *= recip_norm;
my *= recip_norm;
mz *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0mx = 2.0f * ahrs->quat.q0 * mx;
_2q0my = 2.0f * ahrs->quat.q0 * my;
_2q0mz = 2.0f * ahrs->quat.q0 * mz;
_2q1mx = 2.0f * ahrs->quat.q1 * mx;
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2;
_2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Reference direction of Earth's magnetic field */
hx = mx * q0q0 - _2q0my * ahrs->quat.q3 +
_2q0mz * ahrs->quat.q2 + mx * q1q1 +
_2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 -
mx * q2q2 - mx * q3q3;
hy = _2q0mx * ahrs->quat.q3 + my * q0q0 -
_2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 -
my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3;
// _2bx = sqrtf(hx * hx + hy * hy);
// 改为invsqrt
_2bx = 1.f / InvSqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 +
mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 +
_2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
/* Gradient decent algorithm corrective step */
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
_2bz * ahrs->quat.q2 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q2 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
_2bz * ahrs->quat.q3 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
(-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
(-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q1 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
if (eulr == NULL) return -1;
if (ahrs == NULL) return -1;
const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q3);
const float cosr_cosp =
1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2);
eulr->pit = atan2f(sinr_cosp, cosr_cosp);
const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 -
ahrs->quat.q3 * ahrs->quat.q1);
if (fabsf(sinp) >= 1.0f)
eulr->rol = copysignf(M_PI / 2.0f, sinp);
else
eulr->rol = asinf(sinp);
const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 +
ahrs->quat.q1 * ahrs->quat.q2);
const float cosy_cosp =
1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
eulr->yaw = atan2f(siny_cosp, cosy_cosp);
#if 0
eulr->yaw *= M_RAD2DEG_MULT;
eulr->rol *= M_RAD2DEG_MULT;
eulr->pit *= M_RAD2DEG_MULT;
#endif
return 0;
}
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }

98
User/component/ahrs.h Normal file
View File

@@ -0,0 +1,98 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* 欧拉角（Euler angle） */
typedef struct {
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} AHRS_Eulr_t;
/* 加速度计 Accelerometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Accl_t;
/* 陀螺仪 Gyroscope */
typedef struct {
float x;
float y;
float z;
} AHRS_Gyro_t;
/* 磁力计 Magnetometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Magn_t;
/* 四元数 */
typedef struct {
float q0;
float q1;
float q2;
float q3;
} AHRS_Quaternion_t;
/* 姿态解算算法主结构体 */
typedef struct {
/* 四元数 */
AHRS_Quaternion_t quat;
float inv_sample_freq; /* 采样频率的的倒数 */
} AHRS_t;
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
/**
* @brief 姿态运算更新一次
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
#ifdef __cplusplus
}
#endif

12
User/component/ballistics.c Normal file
View File

@@ -0,0 +1,12 @@
/*
弹道补偿算法。
*/
#include "ballistics.h"
void Ballistics_Init(Ballistics_t *b) { (void)b; }
void Ballistics_Apply(Ballistics_t *b, float bullet_speed) {
(void)b;
(void)bullet_speed;
}
void Ballistics_Reset(Ballistics_t *b) { (void)b; }

23
User/component/ballistics.h Normal file
View File

@@ -0,0 +1,23 @@
/*
弹道补偿算法。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "ahrs.h"
typedef struct {
AHRS_Eulr_t *eulr;
} Ballistics_t;
void Ballistics_Init(Ballistics_t *b);
void Ballistics_Apply(Ballistics_t *b, float bullet_speed);
void Ballistics_Reset(Ballistics_t *b);
#ifdef __cplusplus
}
#endif

59
User/component/capacity.c Normal file
View File

@@ -0,0 +1,59 @@
/*
剩余电量算法。
通过电压值计算剩余电量。
*/
#include "capacity.h"
/**
* @brief 通过电压计算电池剩余电量
*
* @param volt 电压值
* @return float 剩余电量比例
*/
float Capacity_GetBatteryRemain(float volt) {
float percentage;
float volt_2 = volt * volt;
float volt_3 = volt_2 * volt;
if (volt < 19.5f)
percentage = 0.0f;
else if (volt < 21.9f)
percentage = 0.005664f * volt_3 - 0.3386f * volt_2 + 6.765f * volt - 45.17f;
else if (volt < 25.5f)
percentage = 0.02269f * volt_3 - 1.654f * volt_2 + 40.34f * volt - 328.4f;
else
percentage = 1.0f;
if (percentage < 0.0f)
percentage = 0.0f;
else if (percentage > 1.0f)
percentage = 1.0f;
return percentage;
}
/**
* @brief
*
* @param vcap 电容电压
* @param vbat 电池电压
* @param v_cutoff 截止电压
* @return float 电容剩余电量比例
*/
float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff) {
float percentage = (vcap - v_cutoff) / (vbat - v_cutoff);
if (percentage < 0.0f)
percentage = 0.0f;
else if (percentage > 1.0f)
percentage = 1.0f;
return percentage;
}

35
User/component/capacity.h Normal file
View File

@@ -0,0 +1,35 @@
/*
剩余电量算法。
通过电压值计算剩余电量。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/**
* @brief 通过电压计算电池剩余电量
*
* @param volt 电压值
* @return float 剩余电量比例
*/
float Capacity_GetBatteryRemain(float volt);
/**
* @brief
*
* @param vcap 电容电压
* @param vbat 电池电压
* @param v_cutoff 截止电压
* @return float 电容剩余电量比例
*/
float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff);
#ifdef __cplusplus
}
#endif

375
User/component/cmd.c Normal file
View File

@@ -0,0 +1,375 @@
/*
控制命令
*/
#include "cmd.h"
#include <string.h>
/**
* @brief 行为转换为对应按键
*
* @param cmd 主结构体
* @param behavior 行为
* @return uint16_t 行为对应的按键
*/
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
CMD_Behavior_t behavior) {
return cmd->param->map.key_map[behavior].key;
}
static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
CMD_Behavior_t behavior) {
return cmd->param->map.key_map[behavior].active;
}
/**
* @brief 检查按键是否按下
*
* @param rc 遥控器数据
* @param key 按键名称
* @param stateful 是否为状态切换按键
* @return true 按下
* @return false 未按下
*/
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK) {
return rc->mouse.l_click;
}
if (key == CMD_R_CLICK) {
return rc->mouse.r_click;
}
return rc->key & (1u << key);
}
static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
CMD_Behavior_t behavior) {
CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
bool now_key_pressed, last_key_pressed;
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK) {
now_key_pressed = rc->mouse.l_click;
last_key_pressed = cmd->mouse_last.l_click;
} else if (key == CMD_R_CLICK) {
now_key_pressed = rc->mouse.r_click;
last_key_pressed = cmd->mouse_last.r_click;
} else {
now_key_pressed = rc->key & (1u << key);
last_key_pressed = cmd->key_last & (1u << key);
}
switch (active) {
case CMD_ACTIVE_PRESSING:
return now_key_pressed && !last_key_pressed;
case CMD_ACTIVE_RASING:
return !now_key_pressed && last_key_pressed;
case CMD_ACTIVE_PRESSED:
return now_key_pressed;
}
}
/**
* @brief 解析pc行为逻辑
*
* @param rc 遥控器数据
* @param cmd 主结构体
* @param dt_sec 两次解析的间隔
*/
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
/* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
cmd->gimbal.delta_eulr.yaw =
(float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
cmd->gimbal.delta_eulr.pit =
(float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
cmd->shoot.reverse_trig = false;
/* 按键行为映射相关逻辑 */
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
/* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
} else {
/* 切换至准备模式，停止射击 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = false;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
/* 每按一次依次切换开火下一个模式 */
cmd->shoot.fire_mode++;
cmd->shoot.fire_mode %= FIRE_MODE_NUM;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
/* 切换到小陀螺模式 */
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
/* 每按一次开、关弹舱盖 */
cmd->shoot.cover_open = !cmd->shoot.cover_open;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
if (cmd->ai_status == AI_STATUS_HITSWITCH) {
/* 停止ai的打符模式，停用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
} else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
/* 自瞄模式中切换失败提醒 */
} else {
/* ai切换至打符模式，启用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
cmd->ai_status = AI_STATUS_HITSWITCH;
cmd->host_overwrite = true;
}
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
if (cmd->ai_status == AI_STATUS_AUTOAIM) {
/* 停止ai的自瞄模式，停用host控制 */
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
} else {
/* ai切换至自瞄模式，启用host控制 */
cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
}
} else {
cmd->host_overwrite = false;
// TODO: 修复逻辑
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
/* 按下拨弹反转 */
cmd->shoot.reverse_trig = true;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
}
/* 保存当前按下的键位状态 */
cmd->key_last = rc->key;
memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
}
/**
* @brief 解析rc行为逻辑
*
* @param rc 遥控器数据
* @param cmd 主结构体
* @param dt_sec 两次解析的间隔
*/
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
switch (rc->sw_l) {
/* 左拨杆相应行为选择和解析 */
case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_BREAK;
break;
case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
case CMD_SW_DOWN:
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
break;
case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX;
break;
}
switch (rc->sw_r) {
/* 右拨杆相应行为选择和解析*/
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
break;
/*
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
break;
*/
case CMD_SW_ERR:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
}
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
}
/**
* @brief rc失控时机器人恢复放松模式
*
* @param cmd 主结构体
*/
static void CMD_RcLostLogic(CMD_t *cmd) {
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */
cmd->chassis.mode = CHASSIS_MODE_RELAX;
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
}
/**
* @brief 初始化命令解析
*
* @param cmd 主结构体
* @param param 参数
* @return int8_t 0对应没有错误
*/
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
/* 指针检测 */
if (cmd == NULL) return -1;
if (param == NULL) return -1;
/* 设置机器人的命令参数，初始化控制方式为rc控制 */
cmd->pc_ctrl = false;
cmd->param = param;
return 0;
}
/**
* @brief 检查是否启用上位机控制指令覆盖
*
* @param cmd 主结构体
* @return true 启用
* @return false 不启用
*/
inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
/**
* @brief 解析命令
*
* @param rc 遥控器数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
/* 指针检测 */
if (rc == NULL) return -1;
if (cmd == NULL) return -1;
/* 在pc控制和rc控制间切换 */
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
cmd->pc_ctrl = true;
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
cmd->pc_ctrl = false;
/*c当rc丢控时，恢复机器人至默认状态 */
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd);
} else {
if (cmd->pc_ctrl) {
CMD_PcLogic(rc, cmd, dt_sec);
} else {
CMD_RcLogic(rc, cmd, dt_sec);
}
}
return 0;
}
/**
* @brief 解析上位机命令
*
* @param host host数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
(void)dt_sec; /* 未使用dt_sec，消除警告 */
/* 指针检测 */
if (host == NULL) return -1;
if (cmd == NULL) return -1;
/* 云台欧拉角设置为host相应的变化的欧拉角 */
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
/* host射击命令，设置不同的射击频率和弹丸初速度 */
if (host->fire) {
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
} else {
cmd->shoot.mode = SHOOT_MODE_SAFE;
}
return 0;
}
/**
* @brief 添加向Referee发送的命令
*
* @param ref 命令队列
* @param cmd 要添加的命令
* @return int8_t 0对应没有错误
*/
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
/* 指针检测 */
if (ref == NULL) return -1;
/* 越界检测 */
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
/* 添加机器人当前行为状态到画图的命令队列中 */
ref->cmd[ref->counter] = cmd;
ref->counter++;
return 0;
}

306
User/component/cmd.h Normal file
View File

@@ -0,0 +1,306 @@
/*
控制命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#include "ahrs.h"
#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */
/* 机器人型号 */
typedef enum {
ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */
ROBOT_MODEL_HERO, /* 英雄机器人 */
ROBOT_MODEL_ENGINEER, /* 工程机器人 */
ROBOT_MODEL_DRONE, /* 空中机器人 */
ROBOT_MODEL_SENTRY, /* 哨兵机器人 */
ROBOT_MODEL_NUM, /* 型号数量 */
} CMD_RobotModel_t;
/* 底盘运行模式 */
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
} CMD_ChassisMode_t;
/* 云台运行模式 */
typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
} CMD_GimbalMode_t;
/* 射击运行模式 */
typedef enum {
SHOOT_MODE_RELAX, /* 放松模式，电机不输出 */
SHOOT_MODE_SAFE, /* 保险模式，电机闭环控制保持静止 */
SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */
} CMD_ShootMode_t;
typedef enum {
FIRE_MODE_SINGLE, /* 单发开火模式 */
FIRE_MODE_BURST, /* N连发开火模式 */
FIRE_MODE_CONT, /* 持续开火模式 */
FIRE_MODE_NUM,
} CMD_FireMode_t;
/* 小陀螺转动模式 */
typedef enum {
ROTOR_MODE_CW, /* 顺时针转动 */
ROTOR_MODE_CCW, /* 逆时针转动 */
ROTOR_MODE_RAND, /* 随机转动 */
} CMD_RotorMode_t;
/* 底盘控制命令 */
typedef struct {
CMD_ChassisMode_t mode; /* 底盘运行模式 */
CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
MoveVector_t ctrl_vec; /* 底盘控制向量 */
} CMD_ChassisCmd_t;
/* 云台控制命令 */
typedef struct {
CMD_GimbalMode_t mode; /* 云台运行模式 */
AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */
} CMD_GimbalCmd_t;
/* 射击控制命令 */
typedef struct {
CMD_ShootMode_t mode; /* 射击运行模式 */
CMD_FireMode_t fire_mode; /* 开火模式 */
bool fire; /*开火*/
bool cover_open; /* 弹舱盖开关 */
bool reverse_trig; /* 拨弹电机状态 */
} CMD_ShootCmd_t;
/* 拨杆位置 */
typedef enum {
CMD_SW_ERR = 0,
CMD_SW_UP = 1,
CMD_SW_MID = 3,
CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;
/* 键盘按键值 */
typedef enum {
CMD_KEY_W = 0,
CMD_KEY_S,
CMD_KEY_A,
CMD_KEY_D,
CMD_KEY_SHIFT,
CMD_KEY_CTRL,
CMD_KEY_Q,
CMD_KEY_E,
CMD_KEY_R,
CMD_KEY_F,
CMD_KEY_G,
CMD_KEY_Z,
CMD_KEY_X,
CMD_KEY_C,
CMD_KEY_V,
CMD_KEY_B,
CMD_L_CLICK,
CMD_R_CLICK,
CMD_KEY_NUM,
} CMD_KeyValue_t;
/* 行为值序列 */
typedef enum {
CMD_BEHAVIOR_FORE = 0, /* 向前 */
CMD_BEHAVIOR_BACK, /* 向后 */
CMD_BEHAVIOR_LEFT, /* 向左 */
CMD_BEHAVIOR_RIGHT, /* 向右 */
CMD_BEHAVIOR_ACCELERATE, /* 加速 */
CMD_BEHAVIOR_DECELEBRATE, /* 减速 */
CMD_BEHAVIOR_FIRE, /* 开火 */
CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */
CMD_BEHAVIOR_BUFF, /* 打符模式 */
CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */
CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */
CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */
CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */
CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
CMD_BEHAVIOR_NUM,
} CMD_Behavior_t;
typedef enum {
CMD_ACTIVE_PRESSING, /* 按下时触发 */
CMD_ACTIVE_RASING, /* 抬起时触发 */
CMD_ACTIVE_PRESSED, /* 按住时触发 */
} CMD_ActiveType_t;
typedef struct {
CMD_ActiveType_t active;
CMD_KeyValue_t key;
} CMD_KeyMapItem_t;
/* 行为映射的对应按键数组 */
typedef struct {
CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
} CMD_KeyMap_Params_t;
/* 位移灵敏度参数 */
typedef struct {
float move_sense; /* 移动灵敏度 */
float move_fast_sense; /* 加速灵敏度 */
float move_slow_sense; /* 减速灵敏度 */
} CMD_Move_Params_t;
typedef struct {
uint16_t width;
uint16_t height;
} CMD_Screen_t;
/* 命令参数 */
typedef struct {
float sens_mouse; /* 鼠标灵敏度 */
float sens_rc; /* 遥控器摇杆灵敏度 */
CMD_KeyMap_Params_t map; /* 按键映射行为命令 */
CMD_Move_Params_t move; /* 位移灵敏度参数 */
CMD_Screen_t screen; /* 屏幕分辨率参数 */
} CMD_Params_t;
/* AI行为状态 */
typedef enum {
AI_STATUS_STOP, /* 停止状态 */
AI_STATUS_AUTOAIM, /* 自瞄状态 */
AI_STATUS_HITSWITCH, /* 打符状态 */
AI_STATUS_AUTOMATIC /* 自动状态 */
} CMD_AI_Status_t;
/* UI所用行为状态 */
typedef enum {
CMD_UI_NOTHING, /* 当前无状态 */
CMD_UI_AUTO_AIM_START, /* 自瞄状态开启 */
CMD_UI_AUTO_AIM_STOP, /* 自瞄状态关闭 */
CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */
CMD_UI_HIT_SWITCH_STOP /* 打符状态关闭 */
} CMD_UI_t;
/*裁判系统发送的命令*/
typedef struct {
CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */
uint8_t counter; /* 命令计数 */
} CMD_RefereeCmd_t;
typedef struct {
bool pc_ctrl; /* 是否使用键鼠控制 */
bool host_overwrite; /* 是否Host控制 */
uint16_t key_last; /* 上次按键键值 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse_last; /* 鼠标值 */
CMD_AI_Status_t ai_status; /* AI状态 */
const CMD_Params_t *param; /* 命令参数 */
CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
CMD_GimbalCmd_t gimbal; /* 云台控制命令 */
CMD_ShootCmd_t shoot; /* 射击控制命令 */
CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */
} CMD_t;
typedef struct {
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
float ch_res; /* 第五通道值 */
CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse; /* 鼠标值 */
uint16_t key; /* 按键值 */
uint16_t res; /* 保留，未启用 */
} CMD_RC_t;
typedef struct {
AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */
struct {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
bool fire; /* 开火状态 */
} CMD_Host_t;
/**
* @brief 解析行为命令
*
* @param rc 遥控器数据
* @param cmd 主结构体
*/
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param);
/**
* @brief 检查是否启用上位机控制指令覆盖
*
* @param cmd 主结构体
* @return true 启用
* @return false 不启用
*/
bool CMD_CheckHostOverwrite(CMD_t *cmd);
/**
* @brief 解析命令
*
* @param rc 遥控器数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec);
/**
* @brief 解析上位机命令
*
* @param host host数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec);
/**
* @brief 添加向Referee发送的命令
*
* @param ref 命令队列
* @param cmd 要添加的命令
* @return int8_t 0对应没有错误
*/
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd);
#ifdef __cplusplus
}
#endif

50
User/component/crc16.c Normal file
View File

@@ -0,0 +1,50 @@
#include "crc16.h"
static const uint16_t crc16_tab[256] = {
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) {
return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
}
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
while (len--) crc = CRC16_Byte(crc, *buf++);
return crc;
}
bool CRC16_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
return expected ==
((const uint16_t *)((const uint8_t *)buf +
(len % 2)))[len / sizeof(uint16_t) - 1];
}

18
User/component/crc16.h Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include "user_math.h"
#define CRC16_INIT 0XFFFF
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
bool CRC16_Verify(const uint8_t *buf, size_t len);
#ifdef __cplusplus
}
#endif

44
User/component/crc8.c Normal file
View File

@@ -0,0 +1,44 @@
/*
参考了Linux
*/
#include "crc8.h"
static const uint8_t crc8_tab[256] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
0xd7, 0x89, 0x6b, 0x35,
};
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
/* loop over the buffer data */
while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
return crc;
}
bool CRC8_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
return expected == buf[len - sizeof(uint8_t)];
}

22
User/component/crc8.h Normal file
View File

@@ -0,0 +1,22 @@
/*
参考了Linux
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#define CRC8_INIT 0xFF
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
bool CRC8_Verify(const uint8_t *buf, size_t len);
#ifdef __cplusplus
}
#endif

67
User/component/error_detect.c Normal file
View File

@@ -0,0 +1,67 @@
/*
错误检测。
*/
#include "error_detect.h"
#include <stddef.h>
#include <string.h>
#include "bsp/mm.h"
static ErrorDetect_t ged;
static bool inited = false;
int8_t ErrorDetect_Init(void) {
if (inited) return -1;
memset(&ged, 0x00, sizeof(ged));
for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
ged.error[i].enable = true;
ged.error[i].priority = i;
ged.error[i].patient_lost = 500;
ged.error[i].patient_work = 500;
}
return 0;
}
void ErrorDetect_Processing(uint32_t sys_time) {
for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
if (!ged.error[i].enable) continue;
if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
ged.error[i].is_lost = true;
ged.error[i].found_lost = sys_time;
} else if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
} else {
ged.error[i].cycle_time = ged.error[i].showup - ged.error[i].showup_last;
}
}
}
bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit) {
if (unit == ERROR_DETECT_UNIT_NO_DEV) {
for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
if (ged.error[i].error_exist) return true;
}
return false;
} else {
return ged.error[unit].error_exist;
}
}
ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void) {
for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
if (ged.error[i].error_exist) return i;
}
return ERROR_DETECT_UNIT_NO_DEV;
}
const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit) {
return &ged.error[unit];
}
void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current) {
ged.error[unit].showup = time_current;
}

66
User/component/error_detect.h Normal file
View File

@@ -0,0 +1,66 @@
/*
错误检测。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
typedef enum {
/* Low priority */
ERROR_DETECT_UNIT_NO_DEV = 0,
ERROR_DETECT_UNIT_REFEREE,
ERROR_DETECT_UNIT_CHASSIS_M1,
ERROR_DETECT_UNIT_CHASSIS_M2,
ERROR_DETECT_UNIT_CHASSIS_M3,
ERROR_DETECT_UNIT_CHASSIS_M4,
ERROR_DETECT_UNIT_TRIGGER,
ERROR_DETECT_UNIT_FEED,
ERROR_DETECT_UNIT_GIMBAL_YAW,
ERROR_DETECT_UNIT_GIMBAL_PIT,
ERROR_DETECT_UNIT_GYRO,
ERROR_DETECT_UNIT_ACCL,
ERROR_DETECT_UNIT_MAGN,
ERROR_DETECT_UNIT_DBUS,
ERROR_DETECT_UNIT_NUM,
/* High priority */
} ErrorDetect_Unit_t;
typedef struct {
bool enable;
uint8_t priority;
uint32_t patient_lost;
uint32_t patient_work;
uint32_t showup;
uint32_t showup_last;
uint32_t cycle_time;
uint32_t duration_lost;
uint32_t duration_work;
uint32_t found_lost;
bool error_exist;
bool is_lost;
uint8_t data_is_error;
} ErrorDetect_Error_t;
typedef struct {
ErrorDetect_Error_t error[ERROR_DETECT_UNIT_NUM];
} ErrorDetect_t;
int8_t ErrorDetect_Init(void);
void ErrorDetect_Processing(uint32_t sys_time);
bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit);
ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void);
const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit);
void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current);
#ifdef __cplusplus
}
#endif

185
User/component/filter.c Normal file
View File

@@ -0,0 +1,185 @@
/*
各类滤波器。
*/
#include "filter.h"
#include "user_math.h"
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq) {
if (f == NULL) return;
f->cutoff_freq = cutoff_freq;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (f->cutoff_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float fr = sample_freq / f->cutoff_freq;
const float ohm = tanf(M_PI / fr);
const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
f->b0 = ohm * ohm / c;
f->b1 = 2.0f * f->b0;
f->b2 = f->b0;
f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
/* do the filtering */
float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
if (isinf(delay_element_0)) {
/* don't allow bad values to propagate via the filter */
delay_element_0 = sample;
}
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
/* return the value. Should be no need to check limits */
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
const float dval = sample / (f->b0 + f->b1 + f->b2);
if (isfinite(dval)) {
f->delay_element_1 = dval;
f->delay_element_2 = dval;
} else {
f->delay_element_1 = sample;
f->delay_element_2 = sample;
}
return LowPassFilter2p_Apply(f, sample);
}
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth) {
if (f == NULL) return;
f->notch_freq = notch_freq;
f->bandwidth = bandwidth;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (notch_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float alpha = tanf(M_PI * bandwidth / sample_freq);
const float beta = -cosf(M_2PI * notch_freq / sample_freq);
const float a0_inv = 1.0f / (alpha + 1.0f);
f->b0 = a0_inv;
f->b1 = 2.0f * beta * a0_inv;
f->b2 = a0_inv;
f->a1 = f->b1;
f->a2 = (1.0f - alpha) * a0_inv;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
/* Direct Form II implementation */
const float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
float dval = sample;
if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
dval = dval / (f->b0 + f->b1 + f->b2);
}
f->delay_element_1 = dval;
f->delay_element_2 = dval;
return NotchFilter_Apply(f, sample);
}

104
User/component/filter.h Normal file
View File

@@ -0,0 +1,104 @@
/*
各类滤波器。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* 二阶低通滤波器 */
typedef struct {
float cutoff_freq; /* 截止频率 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} LowPassFilter2p_t;
/* 带阻滤波器 */
typedef struct {
float notch_freq; /* 阻止频率 */
float bandwidth; /* 带宽 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} NotchFilter_t;
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Apply(NotchFilter_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample);
#ifdef __cplusplus
}
#endif

107
User/component/limiter.c Normal file
View File

@@ -0,0 +1,107 @@
/*
限制器
*/
#include "limiter.h"
#include <math.h>
#include <stddef.h>
#define POWER_BUFF_THRESHOLD 20
#define CHASSIS_POWER_CHECK_FREQ 10
#define CHASSIS_POWER_FACTOR_PASS 0.9f
#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len) {
/* power_limit小于0时不进行限制 */
if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
float sum_motor_out = 0.0f;
for (uint32_t i = 0; i < len; i++) {
/* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
sum_motor_out +=
fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
}
/* 保持每个电机输出值缩小时比例不变 */
if (sum_motor_out > power_limit) {
for (uint32_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / sum_motor_out;
}
}
return 0;
}
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer) {
float target_power = 0.0f;
/* 计算下一个检测周期的剩余缓冲能量 */
float heat_buff = power_buffer - (float)(power_in - power_limit) /
(float)CHASSIS_POWER_CHECK_FREQ;
if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
} else {
target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
}
return target_power;
}
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer) {
float target_power = 0.0f;
/* 根据剩余缓冲能量计算输出功率 */
target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
if (target_power < 0.0f) target_power = 0.0f;
return target_power;
}
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big) {
float heat_percent = heat / heat_limit;
float stable_freq = cooling_rate / heat_increase;
if (is_big)
return stable_freq;
else
return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
}

55
User/component/limiter.h Normal file
View File

@@ -0,0 +1,55 @@
/*
限制器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len);
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer);
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer);
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big);

94
User/component/mixer.c Normal file
View File

@@ -0,0 +1,94 @@
/*
混合器
*/
#include "mixer.h"
#include "math.h"
/**
* @brief 初始化混合器
*
* @param mixer 混合器
* @param mode 混合器模式
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode) {
if (mixer == NULL) return -1;
mixer->mode = mode;
return 0;
}
/**
* @brief 计算输出
*
* @param mixer 混合器
* @param move_vec 运动向量
* @param out 输出数组
* @param len 输出数组长短
* @param scale 输出放大因子
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
int8_t len, float scale) {
if (mixer == NULL) return -1;
switch (mixer->mode) {
case MIXER_MECANUM:
if (len == 4) {
out[0] = move_vec->vx - move_vec->vy + move_vec->wz;
out[1] = move_vec->vx + move_vec->vy + move_vec->wz;
out[2] = -move_vec->vx + move_vec->vy + move_vec->wz;
out[3] = -move_vec->vx - move_vec->vy + move_vec->wz;
} else {
goto error;
}
break;
case MIXER_PARLFIX4:
if (len == 4) {
out[0] = -move_vec->vx;
out[1] = move_vec->vx;
out[2] = move_vec->vx;
out[3] = -move_vec->vx;
} else {
goto error;
}
case MIXER_PARLFIX2:
if (len == 2) {
out[0] = -move_vec->vx;
out[1] = move_vec->vx;
} else {
goto error;
}
case MIXER_SINGLE:
if (len == 1) {
out[0] = move_vec->vx;
} else {
goto error;
}
case MIXER_OMNICROSS:
case MIXER_OMNIPLUS:
goto error;
}
float abs_max = 0.f;
for (int8_t i = 0; i < len; i++) {
const float abs_val = fabsf(out[i]);
abs_max = (abs_val > abs_max) ? abs_val : abs_max;
}
if (abs_max > 1.f) {
for (int8_t i = 0; i < len; i++) {
out[i] /= abs_max;
}
}
for (int8_t i = 0; i < len; i++) {
out[i] *= scale;
}
return 0;
error:
for (uint8_t i = 0; i < len; i++) out[i] = 0;
return -1;
}

60
User/component/mixer.h Normal file
View File

@@ -0,0 +1,60 @@
/*
混合器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/** 四轮布局 */
/* 前 */
/* 2 1 */
/* 3 4 */
/* 两轮布局 */
/* 前 */
/* 2 1 */
/* 混合器模式 */
typedef enum {
MIXER_MECANUM, /* 麦克纳姆轮 */
MIXER_PARLFIX4, /* 平行四驱动轮 */
MIXER_PARLFIX2, /* 平行对侧两驱动轮 */
MIXER_OMNICROSS, /* 叉形全向轮 */
MIXER_OMNIPLUS, /* 十字全向轮 */
MIXER_SINGLE, /* 单个摩擦轮 */
} Mixer_Mode_t;
typedef struct {
Mixer_Mode_t mode;
} Mixer_t; /* 混合器主结构体 */
/**
* @brief 初始化混合器
*
* @param mixer 混合器
* @param mode 混合器模式
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode);
/**
* @brief 计算输出
*
* @param mixer 混合器
* @param move_vec 运动向量
* @param out 输出数组
* @param len 输出数组长短
* @param scale 输出放大因子
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
int8_t len, float scale);
#ifdef __cplusplus
}
#endif

158
User/component/pid.c Normal file
View File

@@ -0,0 +1,158 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
参考资料：
https://github.com/PX4/Firmware/issues/12362
https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
https://en.wikipedia.org/wiki/PID_controller
http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
#include "pid.h"
#define SIGMA 0.000001f
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param) {
if (pid == NULL) return -1;
if (!isfinite(param->p)) return -1;
if (!isfinite(param->i)) return -1;
if (!isfinite(param->d)) return -1;
if (!isfinite(param->i_limit)) return -1;
if (!isfinite(param->out_limit)) return -1;
pid->param = param;
float dt_min = 1.0f / sample_freq;
if (isfinite(dt_min))
pid->dt_min = dt_min;
else
return -1;
LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
pid->mode = mode;
PID_Reset(pid);
return 0;
}
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
return pid->last.out;
}
/* 计算误差值 */
const float err = CircleError(sp, fb, pid->param->range);
/* 计算P项 */
const float k_err = err * pid->param->k;
/* 计算D项 */
const float k_fb = pid->param->k * fb;
const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
float d;
switch (pid->mode) {
case KPID_MODE_CALC_D:
/* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
/* 当sp不变时，err的微分等于负的fb的微分 */
d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
break;
case KPID_MODE_SET_D:
d = fb_dot;
break;
case KPID_MODE_NO_D:
d = 0.0f;
break;
}
pid->last.err = err;
pid->last.k_fb = filtered_k_fb;
if (!isfinite(d)) d = 0.0f;
/* 计算PD输出 */
float output = (k_err * pid->param->p) - (d * pid->param->d);
/* 计算I项 */
const float i = pid->i + (k_err * dt);
const float i_out = i * pid->param->i;
if (pid->param->i > SIGMA) {
/* 检查是否饱和 */
if (isfinite(i)) {
if ((fabsf(output + i_out) <= pid->param->out_limit) &&
(fabsf(i) <= pid->param->i_limit)) {
/* 未饱和，使用新积分 */
pid->i = i;
}
}
}
/* 计算PID输出 */
output += i_out;
/* 限制输出 */
if (isfinite(output)) {
if (pid->param->out_limit > SIGMA) {
output = AbsClip(output, pid->param->out_limit);
}
pid->last.out = output;
}
return pid->last.out;
}
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
return 0;
}
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
pid->last.err = 0.0f;
pid->last.k_fb = 0.0f;
pid->last.out = 0.0f;
LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
return 0;
}

95
User/component/pid.h Normal file
View File

@@ -0,0 +1,95 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "filter.h"
#include "user_math.h"
/* PID模式 */
typedef enum {
KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
} KPID_Mode_t;
/* PID参数 */
typedef struct {
float k; /* 控制器增益，设置为1用于并行模式 */
float p; /* 比例项增益，设置为1用于标准形式 */
float i; /* 积分项增益 */
float d; /* 微分项增益 */
float i_limit; /* 积分项上限 */
float out_limit; /* 输出绝对值限制 */
float d_cutoff_freq; /* D项低通截止频率 */
float range; /* 计算循环误差时使用，大于0时启用 */
} KPID_Params_t;
/* PID主结构体 */
typedef struct {
KPID_Mode_t mode;
const KPID_Params_t *param;
float dt_min; /* 最小PID_Calc调用间隔 */
float i; /* 积分 */
struct {
float err; /* 上次误差 */
float k_fb; /* 上次反馈值 */
float out; /* 上次输出 */
} last;
LowPassFilter2p_t dfilter; /* D项低通滤波器 */
} KPID_t;
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param);
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid);
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid);
#ifdef __cplusplus
}
#endif

301
User/component/ui.c Normal file
View File

@@ -0,0 +1,301 @@
/*
UI相关命令
*/
#include "component\ui.h"
#include <stdio.h>
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end) {
if (grapic_line == NULL) return -1;
snprintf((char *)grapic_line->name, 2, "%s", name);
grapic_line->layer = layer;
grapic_line->type_op = type_op;
grapic_line->type_ele = 0;
grapic_line->color = color;
grapic_line->width = width;
grapic_line->x_start = x_start;
grapic_line->y_start = y_start;
grapic_line->x_end = x_end;
grapic_line->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end) {
if (grapic_rectangle == NULL) return -1;
snprintf((char *)grapic_rectangle->name, 2, "%s", name);
grapic_rectangle->type_op = type_op;
grapic_rectangle->type_ele = 1;
grapic_rectangle->layer = layer;
grapic_rectangle->color = color;
grapic_rectangle->width = width;
grapic_rectangle->x_start = x_start;
grapic_rectangle->y_start = y_start;
grapic_rectangle->x_end = x_end;
grapic_rectangle->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius) {
if (grapic_cycle == NULL) return -1;
snprintf((char *)grapic_cycle->name, 2, "%s", name);
grapic_cycle->type_op = type_op;
grapic_cycle->layer = layer;
grapic_cycle->type_ele = 2;
grapic_cycle->color = color;
grapic_cycle->width = width;
grapic_cycle->x_start = x_center;
grapic_cycle->y_start = y_center;
grapic_cycle->radius = radius;
return 0;
}
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis) {
if (grapic_oval == NULL) return -1;
snprintf((char *)grapic_oval->name, 2, "%s", name);
grapic_oval->type_op = type_op;
grapic_oval->type_ele = 3;
grapic_oval->layer = layer;
grapic_oval->color = color;
grapic_oval->width = width;
grapic_oval->x_start = x_center;
grapic_oval->y_start = y_center;
grapic_oval->x_end = x_semiaxis;
grapic_oval->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) {
if (grapic_arc == NULL) return -1;
snprintf((char *)grapic_arc->name, 2, "%s", name);
grapic_arc->type_op = type_op;
grapic_arc->type_ele = 4;
grapic_arc->layer = layer;
grapic_arc->color = color;
grapic_arc->angle_start = angle_start;
grapic_arc->angle_end = angle_end;
grapic_arc->width = width;
grapic_arc->x_start = x_center;
grapic_arc->y_start = y_center;
grapic_arc->x_end = x_semiaxis;
grapic_arc->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low) {
if (grapic_floating == NULL) return -1;
snprintf((char *)grapic_floating->name, 2, "%s", name);
grapic_floating->type_op = type_op;
grapic_floating->type_ele = 5;
grapic_floating->layer = layer;
grapic_floating->color = color;
grapic_floating->angle_start = font_size;
grapic_floating->angle_end = digits;
grapic_floating->width = width;
grapic_floating->x_start = x_start;
grapic_floating->y_start = y_start;
grapic_floating->radius = float_high;
grapic_floating->x_end = float_middle;
grapic_floating->y_end = float_low;
return 0;
}
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low) {
if (grapic_integer == NULL) return -1;
snprintf((char *)grapic_integer->name, 2, "%s", name);
grapic_integer->type_op = type_op;
grapic_integer->type_ele = 6;
grapic_integer->layer = layer;
grapic_integer->color = color;
grapic_integer->angle_start = font_size;
grapic_integer->width = width;
grapic_integer->x_start = x_start;
grapic_integer->y_start = y_start;
grapic_integer->radius = int32_t_high;
grapic_integer->x_end = int32_t_middle;
grapic_integer->y_end = int32_t_low;
return 0;
}
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character) {
if (grapic_character == NULL) return -1;
snprintf((char *)grapic_character->grapic.name, 2, "%s", name);
grapic_character->grapic.type_op = type_op;
grapic_character->grapic.type_ele = 7;
grapic_character->grapic.layer = layer;
grapic_character->grapic.color = color;
grapic_character->grapic.angle_start = font_size;
grapic_character->grapic.angle_end = length;
grapic_character->grapic.width = width;
grapic_character->grapic.x_start = x_start;
grapic_character->grapic.y_start = y_start;
snprintf((char *)grapic_character->character, 29, "%s", character);
return 0;
}
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) {
if (del == NULL) return -1;
del->del_operation = opt;
del->layer = layer;
return 0;
}

276
User/component/ui.h Normal file
View File

@@ -0,0 +1,276 @@
/*
UI相关命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include "component\user_math.h"
#define UI_DEL_OPERATION_NOTHING (0)
#define UI_DEL_OPERATION_DEL (1)
#define UI_DEL_OPERATION_DEL_ALL (2)
#define UI_GRAPIC_OPERATION_NOTHING (0)
#define UI_GRAPIC_OPERATION_ADD (1)
#define UI_GRAPIC_OPERATION_REWRITE (2)
#define UI_GRAPIC_OPERATION_DEL (3)
#define UI_GRAPIC_LAYER_CONST (0)
#define UI_GRAPIC_LAYER_AUTOAIM (1)
#define UI_GRAPIC_LAYER_CHASSIS (2)
#define UI_GRAPIC_LAYER_CAP (3)
#define UI_GRAPIC_LAYER_GIMBAL (4)
#define UI_GRAPIC_LAYER_SHOOT (5)
#define UI_GRAPIC_LAYER_CMD (6)
#define UI_DEFAULT_WIDTH (0x01)
#define UI_CHAR_DEFAULT_WIDTH (0x02)
typedef enum {
RED_BLUE,
YELLOW,
GREEN,
ORANGE,
PURPLISH_RED,
PINK,
CYAN,
BLACK,
WHITE
} UI_Color_t;
typedef struct __packed {
uint8_t op;
uint8_t num_layer;
} UI_InterStudent_UIDel_t;
typedef struct __packed {
uint8_t name[3];
uint8_t type_op : 3;
uint8_t type_ele : 3;
uint8_t layer : 4;
uint8_t color : 4;
uint16_t angle_start : 9;
uint16_t angle_end : 9;
uint16_t width : 10;
uint16_t x_start : 11;
uint16_t y_start : 11;
uint16_t radius : 10;
uint16_t x_end : 11;
uint16_t y_end : 11;
} UI_Ele_t;
typedef struct __packed {
UI_Ele_t grapic;
} UI_Drawgrapic_1_t;
typedef struct __packed {
UI_Ele_t grapic[2];
} UI_Drawgrapic_2_t;
typedef struct __packed {
UI_Ele_t grapic[5];
} UI_Drawgrapic_5_t;
typedef struct __packed {
UI_Ele_t grapic[7];
} UI_Drawgrapic_7_t;
typedef struct __packed {
UI_Ele_t grapic;
uint8_t character[30];
} UI_Drawcharacter_t;
typedef struct __packed {
uint8_t del_operation;
uint8_t layer;
} UI_Del_t;
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end);
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end);
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius);
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis);
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis);
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low);
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low);
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character);
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer);
#ifdef __cplusplus
}
#endif

114
User/component/user_math.c Normal file
View File

@@ -0,0 +1,114 @@
/*
自定义的数学运算。
*/
#include "user_math.h"
#include <string.h>
inline float InvSqrt(float x) {
//#if 0
/* Fast inverse square-root */
/* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
//#else
// return 1.0f / sqrtf(x);
//#endif
}
inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in);
}
inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max;
if (*origin < min) *origin = min;
}
inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
/**
* \brief 将运动向量置零
*
* \param mv 被操作的值
*/
inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 被操作的值
* \param fb 变化量
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
*/
inline float CircleError(float sp, float fb, float range) {
float error = sp - fb;
if (range > 0.0f) {
float half_range = range / 2.0f;
if (error > half_range)
error -= range;
else if (error < -half_range)
error += range;
}
return error;
}
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
*
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
inline void CircleAdd(float *origin, float delta, float range) {
float out = *origin + delta;
if (range > 0.0f) {
if (out >= range)
out -= range;
else if (out < 0.0f)
out += range;
}
*origin = out;
}
/**
* @brief 循环值取反
*
* @param origin 被操作的值
*/
inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
/**
* @brief 根据目标弹丸速度计算摩擦轮转速
*
* @param bullet_speed 弹丸速度
* @param fric_radius 摩擦轮半径
* @param is17mm 是否为17mm
* @return 摩擦轮转速
*/
inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
if (bullet_speed == 0.0f) return 0.f;
if (is17mm) {
if (bullet_speed == 15.0f) return 4670.f;
if (bullet_speed == 18.0f) return 5200.f;
if (bullet_speed == 30.0f) return 7350.f;
} else {
if (bullet_speed == 10.0f) return 4450.f;
if (bullet_speed == 16.0f) return 5800.f;
}
/* 不为裁判系统设定值时,计算转速 */
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
}

106
User/component/user_math.h Normal file
View File

@@ -0,0 +1,106 @@
/*
自定义的数学运算。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
#define ARM_MATH_CM4
#include <float.h>
#include <math.h>
#include <stdbool.h>
#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#ifndef M_2PI
#define M_2PI 6.28318530717958647692f
#endif
#define max(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a > _b ? _a : _b; \
})
#define min(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a < _b ? _a : _b; \
})
/* 移动向量 */
typedef struct {
float vx; /* 前后平移 */
float vy; /* 左右平移 */
float wz; /* 转动 */
} MoveVector_t;
float InvSqrt(float x);
float AbsClip(float in, float limit);
void Clip(float *origin, float min, float max);
float Sign(float in);
/**
* \brief 将运动向量置零
*
* \param mv 被操作的值
*/
void ResetMoveVector(MoveVector_t *mv);
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 被操作的值
* \param fb 变化量
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
*/
float CircleError(float sp, float fb, float range);
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
*
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
void CircleAdd(float *origin, float delta, float range);
/**
* @brief 循环值取反
*
* @param origin 被操作的值
*/
void CircleReverse(float *origin);
/**
* @brief 根据目标弹丸速度计算摩擦轮转速
*
* @param bullet_speed 弹丸速度
* @param fric_radius 摩擦轮半径
* @param is17mm 是否为17mm
* @return 摩擦轮转速
*/
float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
#ifdef __cplusplus
}
#endif

133
User/device/ai.c Normal file
View File

@@ -0,0 +1,133 @@
/*
AI
*/
/* Includes ----------------------------------------------------------------- */
#include "ai.h"
#include <string.h>
#include "bsp\delay.h"
#include "bsp\uart.h"
#include "component\crc16.h"
#include "component\crc8.h"
#include "component\user_math.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) {
if (ai == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
Ai_RxCpltCallback);
inited = true;
return 0;
}
int8_t AI_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
return DEVICE_OK;
}
int8_t AI_StartReceiving(AI_t *ai) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
(uint8_t *)&(ai->form_host),
sizeof(ai->form_host)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool AI_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
SIGNAL_AI_RAW_REDY);
}
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
(void)cmd_host;
if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
goto error;
cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
return DEVICE_OK;
error:
drop_message++;
return DEVICE_ERR;
}
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
if (ai == NULL) return DEVICE_ERR_NULL;
if (cmd_host == NULL) return DEVICE_ERR_NULL;
memset(&(ai->form_host), 0, sizeof(ai->form_host));
memset(cmd_host, 0, sizeof(*cmd_host));
return 0;
}
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
ai->to_host.mcu.id = AI_ID_MCU;
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
sizeof(*quat));
ai->to_host.mcu.package.data.notice = 0;
if (ai->status == AI_STATUS_AUTOAIM)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
else if (ai->status == AI_STATUS_HITSWITCH)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
else if (ai->status == AI_STATUS_AUTOMATIC)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
ai->to_host.mcu.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.mcu.package),
sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
(void)ref;
ai->to_host.ref.id = AI_ID_REF;
ai->to_host.ref.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.ref.package),
sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_StartSend(AI_t *ai, bool ref_update) {
if (ref_update) {
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
(uint8_t *)&(ai->to_host.mcu),
sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}

63
User/device/ai.h Normal file
View File

@@ -0,0 +1,63 @@
/*
AI
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#include "component\ahrs.h"
#include "component\cmd.h"
#include "component\user_math.h"
#include "device\device.h"
#include "device\referee.h"
#include "protocol.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint8_t id;
Protocol_UpPackageReferee_t package;
} AI_UpPackageReferee_t;
typedef struct __packed {
uint8_t id;
Protocol_UpPackageMCU_t package;
} AI_UpPackageMCU_t;
typedef struct __packed {
osThreadId_t thread_alert;
Protocol_DownPackage_t form_host;
struct {
AI_UpPackageReferee_t ref;
AI_UpPackageMCU_t mcu;
} to_host;
CMD_AI_Status_t status;
} AI_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t AI_Init(AI_t *ai);
int8_t AI_Restart(void);
int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void);
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
int8_t AI_StartSend(AI_t *ai, bool option);
#ifdef __cplusplus
}
#endif

369
User/device/bmi088.c Normal file
View File

@@ -0,0 +1,369 @@
/*
BMI088 陀螺仪+加速度计传感器。
*/
/* Includes ----------------------------------------------------------------- */
#include "bmi088.h"
#include <cmsis_os2.h>
#include <gpio.h>
#include <stdbool.h>
#include <string.h>
#include "bsp\delay.h"
#include "bsp\gpio.h"
#include "bsp\spi.h"
#include "component\user_math.h"
/* Private define ----------------------------------------------------------- */
#define BMI088_REG_ACCL_CHIP_ID (0x00)
#define BMI088_REG_ACCL_ERR (0x02)
#define BMI088_REG_ACCL_STATUS (0x03)
#define BMI088_REG_ACCL_X_LSB (0x12)
#define BMI088_REG_ACCL_X_MSB (0x13)
#define BMI088_REG_ACCL_Y_LSB (0x14)
#define BMI088_REG_ACCL_Y_MSB (0x15)
#define BMI088_REG_ACCL_Z_LSB (0x16)
#define BMI088_REG_ACCL_Z_MSB (0x17)
#define BMI088_REG_ACCL_SENSORTIME_0 (0x18)
#define BMI088_REG_ACCL_SENSORTIME_1 (0x19)
#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A)
#define BMI088_REG_ACCL_INT_STAT_1 (0x1D)
#define BMI088_REG_ACCL_TEMP_MSB (0x22)
#define BMI088_REG_ACCL_TEMP_LSB (0x23)
#define BMI088_REG_ACCL_CONF (0x40)
#define BMI088_REG_ACCL_RANGE (0x41)
#define BMI088_REG_ACCL_INT1_IO_CONF (0x53)
#define BMI088_REG_ACCL_INT2_IO_CONF (0x54)
#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58)
#define BMI088_REG_ACCL_SELF_TEST (0x6D)
#define BMI088_REG_ACCL_PWR_CONF (0x7C)
#define BMI088_REG_ACCL_PWR_CTRL (0x7D)
#define BMI088_REG_ACCL_SOFTRESET (0x7E)
#define BMI088_REG_GYRO_CHIP_ID (0x00)
#define BMI088_REG_GYRO_X_LSB (0x02)
#define BMI088_REG_GYRO_X_MSB (0x03)
#define BMI088_REG_GYRO_Y_LSB (0x04)
#define BMI088_REG_GYRO_Y_MSB (0x05)
#define BMI088_REG_GYRO_Z_LSB (0x06)
#define BMI088_REG_GYRO_Z_MSB (0x07)
#define BMI088_REG_GYRO_INT_STAT_1 (0x0A)
#define BMI088_REG_GYRO_RANGE (0x0F)
#define BMI088_REG_GYRO_BANDWIDTH (0x10)
#define BMI088_REG_GYRO_LPM1 (0x11)
#define BMI088_REG_GYRO_SOFTRESET (0x14)
#define BMI088_REG_GYRO_INT_CTRL (0x15)
#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16)
#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18)
#define BMI088_REG_GYRO_SELF_TEST (0x3C)
#define BMI088_CHIP_ID_ACCL (0x1E)
#define BMI088_CHIP_ID_GYRO (0x0F)
#define BMI088_LEN_RX_BUFF (19)
/* Private macro ------------------------------------------------------------ */
#define BMI088_ACCL_NSS_SET() \
HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET)
#define BMI088_ACCL_NSS_RESET() \
HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_RESET)
#define BMI088_GYRO_NSS_SET() \
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET)
#define BMI088_GYRO_NSS_RESET() \
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_RESET)
/* Private typedef ---------------------------------------------------------- */
typedef enum {
BMI_ACCL,
BMI_GYRO,
} BMI_Device_t;
/* Private variables -------------------------------------------------------- */
static uint8_t buffer[2];
static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF];
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) {
buffer[0] = (reg & 0x7f);
buffer[1] = data;
BSP_Delay(1);
switch (dv) {
case BMI_ACCL:
BMI088_ACCL_NSS_RESET();
break;
case BMI_GYRO:
BMI088_GYRO_NSS_RESET();
break;
}
HAL_SPI_Transmit(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 2u, 20u);
switch (dv) {
case BMI_ACCL:
BMI088_ACCL_NSS_SET();
break;
case BMI_GYRO:
BMI088_GYRO_NSS_SET();
break;
}
}
static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) {
BSP_Delay(1);
switch (dv) {
case BMI_ACCL:
BMI088_ACCL_NSS_RESET();
break;
case BMI_GYRO:
BMI088_GYRO_NSS_RESET();
break;
}
buffer[0] = (uint8_t)(reg | 0x80);
HAL_SPI_Transmit(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 1u, 20u);
HAL_SPI_Receive(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 2u, 20u);
switch (dv) {
case BMI_ACCL:
BMI088_ACCL_NSS_SET();
return buffer[1];
case BMI_GYRO:
BMI088_GYRO_NSS_SET();
return buffer[0];
}
}
static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) {
if (data == NULL) return;
switch (dv) {
case BMI_ACCL:
BMI088_ACCL_NSS_RESET();
break;
case BMI_GYRO:
BMI088_GYRO_NSS_RESET();
break;
}
buffer[0] = (uint8_t)(reg | 0x80);
HAL_SPI_Transmit(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 1u, 20u);
HAL_SPI_Receive_DMA(BSP_SPI_GetHandle(BSP_SPI_IMU), data, len);
}
static void BMI088_RxCpltCallback(void) {
if (HAL_GPIO_ReadPin(ACCL_CS_GPIO_Port, ACCL_CS_Pin) == GPIO_PIN_RESET) {
BMI088_ACCL_NSS_SET();
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY);
}
if (HAL_GPIO_ReadPin(GYRO_CS_GPIO_Port, GYRO_CS_Pin) == GPIO_PIN_RESET) {
BMI088_GYRO_NSS_SET();
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY);
}
}
static void BMI088_AcclIntCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA);
}
static void BMI088_GyroIntCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA);
}
/* Exported functions ------------------------------------------------------- */
int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) {
if (bmi088 == NULL) return DEVICE_ERR_NULL;
if (cali == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
bmi088->cali = cali;
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6);
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6);
BSP_Delay(30);
/* Switch accl to SPI mode. */
BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL);
if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL)
return DEVICE_ERR_NO_DEV;
if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO)
return DEVICE_ERR_NO_DEV;
BSP_GPIO_DisableIRQ(ACCL_INT_Pin);
BSP_GPIO_DisableIRQ(GYRO_INT_Pin);
BSP_SPI_RegisterCallback(BSP_SPI_IMU, BSP_SPI_RX_CPLT_CB,
BMI088_RxCpltCallback);
BSP_GPIO_RegisterCallback(ACCL_INT_Pin, BMI088_AcclIntCallback);
BSP_GPIO_RegisterCallback(GYRO_INT_Pin, BMI088_GyroIntCallback);
/* Accl init. */
/* Filter setting: Normal. */
/* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA);
/* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01);
/* INT1 as output. Push-pull. Active low. Output. */
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08);
/* Map data ready interrupt to INT1. */
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04);
/* Turn on accl. Now we can read data. */
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04);
BSP_Delay(50);
/* Gyro init. */
/* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01);
/* Filter bw: 47Hz. */
/* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03);
/* INT3 and INT4 as output. Push-pull. Active low. */
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00);
/* Map data ready interrupt to INT3. */
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01);
/* Enable new data interrupt. */
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80);
BSP_Delay(10);
inited = true;
BSP_GPIO_EnableIRQ(ACCL_INT_Pin);
BSP_GPIO_EnableIRQ(GYRO_INT_Pin);
return DEVICE_OK;
}
bool BMI088_GyroStable(AHRS_Gyro_t *gyro) {
return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
}
uint32_t BMI088_WaitNew() {
return osThreadFlagsWait(
SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll,
osWaitForever);
}
int8_t BMI088_AcclStartDmaRecv() {
BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF);
return DEVICE_OK;
}
uint32_t BMI088_AcclWaitDmaCplt() {
return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll,
osWaitForever);
}
int8_t BMI088_GyroStartDmaRecv() {
BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u);
return DEVICE_OK;
}
uint32_t BMI088_GyroWaitDmaCplt() {
return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll,
osWaitForever);
}
int8_t BMI088_ParseAccl(BMI088_t *bmi088) {
if (bmi088 == NULL) return DEVICE_ERR_NULL;
#if 1
int16_t raw_x, raw_y, raw_z;
memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x));
memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y));
memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z));
bmi088->accl.x = (float)raw_x;
bmi088->accl.y = (float)raw_y;
bmi088->accl.z = (float)raw_z;
#else
const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1);
const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3);
const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5);
bmi088->accl.x = (float)*praw_x;
bmi088->accl.y = (float)*praw_y;
bmi088->accl.z = (float)*praw_z;
#endif
/* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */
bmi088->accl.x /= 5460.0f;
bmi088->accl.y /= 5460.0f;
bmi088->accl.z /= 5460.0f;
int16_t raw_temp =
(uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5));
if (raw_temp > 1023) raw_temp -= 2048;
bmi088->temp = (float)raw_temp * 0.125f + 23.0f;
return DEVICE_OK;
}
int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
if (bmi088 == NULL) return DEVICE_ERR_NULL;
#if 1
/* Gyroscope imu_raw -> degrees/sec -> radians/sec */
int16_t raw_x, raw_y, raw_z;
memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x));
memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y));
memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z));
bmi088->gyro.x = (float)raw_x;
bmi088->gyro.y = (float)raw_y;
bmi088->gyro.z = (float)raw_z;
#else
/* Gyroscope imu_raw -> degrees/sec -> radians/sec */
const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7);
const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9);
const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11);
bmi088->gyro.x = (float)*raw_x;
bmi088->gyro.y = (float)*raw_y;
bmi088->gyro.z = (float)*raw_z;
#endif
/* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768.
* FS2000: 16.384.*/
bmi088->gyro.x /= 32.768f;
bmi088->gyro.y /= 32.768f;
bmi088->gyro.z /= 32.768f;
bmi088->gyro.x *= M_DEG2RAD_MULT;
bmi088->gyro.y *= M_DEG2RAD_MULT;
bmi088->gyro.z *= M_DEG2RAD_MULT;
bmi088->gyro.x -= bmi088->cali->gyro_offset.x;
bmi088->gyro.y -= bmi088->cali->gyro_offset.y;
bmi088->gyro.z -= bmi088->cali->gyro_offset.z;
return DEVICE_ERR_NULL;
}
float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
(void)bmi088;
return 400.0f;
}

64
User/device/bmi088.h Normal file
View File

@@ -0,0 +1,64 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdbool.h>
#include <stdint.h>
#include "component\ahrs.h"
#include "device\device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
struct {
float x;
float y;
float z;
} gyro_offset; /* 陀螺仪偏置 */
} BMI088_Cali_t; /* BMI088校准数据 */
typedef struct {
AHRS_Accl_t accl;
AHRS_Gyro_t gyro;
float temp; /* 温度 */
const BMI088_Cali_t *cali;
} BMI088_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali);
int8_t BMI088_Restart(void);
bool BMI088_GyroStable(AHRS_Gyro_t *gyro);
/* Sensor use right-handed coordinate system. */
/*
x < R(logo)
y
UP is z
All implementation should follow this rule.
*/
uint32_t BMI088_WaitNew();
/*
BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。
即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能
出现 BMI088_GyroStartDmaRecv()。
*/
int8_t BMI088_AcclStartDmaRecv();
uint32_t BMI088_AcclWaitDmaCplt();
int8_t BMI088_GyroStartDmaRecv();
uint32_t BMI088_GyroWaitDmaCplt();
int8_t BMI088_ParseAccl(BMI088_t *bmi088);
int8_t BMI088_ParseGyro(BMI088_t *bmi088);
float BMI088_GetUpdateFreq(BMI088_t *bmi088);
#ifdef __cplusplus
}
#endif

326
User/device/can.c Normal file
View File

@@ -0,0 +1,326 @@
/*
CAN总线上的设
将所有CAN总线上挂载的设抽象成一设进行配和控制
*/
/* Includes ----------------------------------------------------------------- */
#include "can.h"
#include <stdbool.h>
#include <string.h>
#include "bsp\can.h"
#include "bsp\mm.h"
#include "component\user_math.h"
#include "device\referee.h"
/* Private define ----------------------------------------------------------- */
/* Motor id */
/* id feedback id control id */
/* 1-4 0x205 to 0x208 0x1ff */
/* 5-6 0x209 to 0x20B 0x2ff */
#define CAN_GM6020_FB_ID_BASE (0x205)
#define CAN_GM6020_CTRL_ID_BASE (0x1ff)
#define CAN_GM6020_CTRL_ID_EXTAND (0x2ff)
/* id feedback id control id */
/* 1-4 0x201 to 0x204 0x200 */
/* 5-6 0x205 to 0x208 0x1ff */
#define CAN_M3508_M2006_FB_ID_BASE (0x201)
#define CAN_M3508_M2006_CTRL_ID_BASE (0x200)
#define CAN_M3508_M2006_CTRL_ID_EXTAND (0x1ff)
#define CAN_M3508_M2006_ID_SETTING_ID (0x700)
/* 电机最大控制输出绝对值 */
#define CAN_GM6020_MAX_ABS_LSB (30000)
#define CAN_M3508_MAX_ABS_LSB (16384)
#define CAN_M2006_MAX_ABS_LSB (10000)
#define CAN_MOTOR_TX_BUF_SIZE (8)
#define CAN_MOTOR_RX_BUF_SIZE (8)
#define CAN_MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */
#define CAN_MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */
#define CAN_MOTOR_RX_FIFO CAN_RX_FIFO0
/* Super capacitor */
#define CAN_CAP_FB_ID_BASE (0x211)
#define CAN_CAP_CTRL_ID_BASE (0x210)
#define CAN_CAP_RES (100) /* 电容数据分辨率 */
#define CAN_CAP_RX_FIFO CAN_RX_FIFO1
/* TOF */
#define CAN_TOF_ID_BASE (0x280)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static CAN_RawRx_t raw_rx1;
static CAN_RawRx_t raw_rx2;
static CAN_RawTx_t raw_tx;
static osThreadId_t thread_alert;
static CAN_t *gcan;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void CAN_Motor_Decode(CAN_MotorFeedback_t *feedback,
const uint8_t *raw) {
uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]);
int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);
feedback->rotor_angle = raw_angle / (float)CAN_MOTOR_ENC_RES * M_2PI;
feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]);
feedback->torque_current =
raw_current * CAN_M3508_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
feedback->temp = raw[6];
}
void CAN_Cap_Decode(CAN_CapFeedback_t *feedback, const uint8_t *raw) {
feedback->input_volt = (float)((raw[1] << 8) | raw[0]) / (float)CAN_CAP_RES;
feedback->cap_volt = (float)((raw[3] << 8) | raw[2]) / (float)CAN_CAP_RES;
feedback->input_curr = (float)((raw[5] << 8) | raw[4]) / (float)CAN_CAP_RES;
feedback->target_power = (float)((raw[7] << 8) | raw[6]) / (float)CAN_CAP_RES;
}
void CAN_Tof_Decode(CAN_Tof_t *tof, const uint8_t *raw) {
tof->dist = (float)((raw[2] << 16) | (raw[1] << 8) | raw[0]) / 1000.0f;
tof->status = raw[3];
tof->signal_strength = (raw[5] << 8) | raw[4];
}
static void CAN_CAN1RxFifoMsgPendingCallback(void) {
HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_MOTOR_RX_FIFO,
&raw_rx1.rx_header, raw_rx1.rx_data);
osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0);
}
static void CAN_CAN2RxFifoMsgPendingCallback(void) {
HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_2), CAN_CAP_RX_FIFO,
&raw_rx2.rx_header, raw_rx2.rx_data);
osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
}
/* Exported functions ------------------------------------------------------- */
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
if (can == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL);
can->param = param;
CAN_FilterTypeDef can_filter = {0};
can_filter.FilterBank = 0;
can_filter.FilterIdHigh = 0;
can_filter.FilterIdLow = 0;
can_filter.FilterMode = CAN_FILTERMODE_IDMASK;
can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
can_filter.FilterMaskIdHigh = 0;
can_filter.FilterMaskIdLow = 0;
can_filter.FilterActivation = ENABLE;
can_filter.SlaveStartFilterBank = 14;
can_filter.FilterFIFOAssignment = CAN_MOTOR_RX_FIFO;
HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_1), &can_filter);
HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_1));
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
CAN_CAN1RxFifoMsgPendingCallback);
HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_1),
CAN_IT_RX_FIFO0_MSG_PENDING);
can_filter.FilterBank = 14;
can_filter.FilterFIFOAssignment = CAN_CAP_RX_FIFO;
HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_2), &can_filter);
HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_2));
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
CAN_CAN2RxFifoMsgPendingCallback);
HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_2),
CAN_IT_RX_FIFO1_MSG_PENDING);
gcan = can;
inited = true;
return DEVICE_OK;
}
int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
CAN_t *can) {
if (output == NULL) return DEVICE_ERR_NULL;
int16_t motor1, motor2, motor3, motor4;
int16_t yaw_motor, pit_motor;
int16_t fric1_motor, fric2_motor, trig_motor;
switch (group) {
case CAN_MOTOR_GROUT_CHASSIS:
motor1 =
(int16_t)(output->chassis.named.m1 * (float)CAN_M3508_MAX_ABS_LSB);
motor2 =
(int16_t)(output->chassis.named.m2 * (float)CAN_M3508_MAX_ABS_LSB);
motor3 =
(int16_t)(output->chassis.named.m3 * (float)CAN_M3508_MAX_ABS_LSB);
motor4 =
(int16_t)(output->chassis.named.m4 * (float)CAN_M3508_MAX_ABS_LSB);
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_BASE;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis),
&raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.chassis));
break;
case CAN_MOTOR_GROUT_GIMBAL1:
case CAN_MOTOR_GROUT_GIMBAL2:
yaw_motor =
(int16_t)(output->gimbal.named.yaw * (float)CAN_GM6020_MAX_ABS_LSB);
pit_motor =
(int16_t)(output->gimbal.named.pit * (float)CAN_GM6020_MAX_ABS_LSB);
raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_EXTAND;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (uint8_t)((yaw_motor >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(yaw_motor & 0xFF);
raw_tx.tx_data[2] = (uint8_t)((pit_motor >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(pit_motor & 0xFF);
raw_tx.tx_data[4] = 0;
raw_tx.tx_data[5] = 0;
raw_tx.tx_data[6] = 0;
raw_tx.tx_data[7] = 0;
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->gimbal),
&raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.gimbal));
// HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
// &raw_tx.tx_header, raw_tx.tx_data,
// &(can->mailbox.gimbal));
break;
case CAN_MOTOR_GROUT_SHOOT1:
case CAN_MOTOR_GROUT_SHOOT2:
fric1_motor =
(int16_t)(output->shoot.named.fric1 * (float)CAN_M3508_MAX_ABS_LSB);
fric2_motor =
(int16_t)(output->shoot.named.fric2 * (float)CAN_M3508_MAX_ABS_LSB);
trig_motor =
(int16_t)(output->shoot.named.trig * (float)CAN_M2006_MAX_ABS_LSB);
raw_tx.tx_header.StdId = CAN_M3508_M2006_CTRL_ID_EXTAND;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (uint8_t)((fric1_motor >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(fric1_motor & 0xFF);
raw_tx.tx_data[2] = (uint8_t)((fric2_motor >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(fric2_motor & 0xFF);
raw_tx.tx_data[4] = (uint8_t)((trig_motor >> 8) & 0xFF);
raw_tx.tx_data[5] = (uint8_t)(trig_motor & 0xFF);
raw_tx.tx_data[6] = 0;
raw_tx.tx_data[7] = 0;
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->shoot),
&raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.shoot));
break;
default:
break;
}
return DEVICE_OK;
}
int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
if (can == NULL) return DEVICE_ERR_NULL;
if (can_rx == NULL) return DEVICE_ERR_NULL;
uint32_t index;
switch (can_rx->rx_header.StdId) {
case CAN_M3508_M1_ID:
case CAN_M3508_M2_ID:
case CAN_M3508_M3_ID:
case CAN_M3508_M4_ID:
index = can_rx->rx_header.StdId - CAN_M3508_M1_ID;
CAN_Motor_Decode(&(can->motor.chassis.as_array[index]), can_rx->rx_data);
can->recive_flag |= 1 << index;
break;
case CAN_M3508_FRIC1_ID:
case CAN_M3508_FRIC2_ID:
case CAN_M2006_TRIG_ID:
index = can_rx->rx_header.StdId - CAN_M3508_FRIC1_ID;
can->recive_flag |= 1 << (index + 6);
CAN_Motor_Decode(&(can->motor.shoot.as_array[index]), can_rx->rx_data);
break;
case CAN_GM6020_YAW_ID:
case CAN_GM6020_PIT_ID:
index = can_rx->rx_header.StdId - CAN_GM6020_YAW_ID;
can->recive_flag |= 1 << (index + 4);
CAN_Motor_Decode(&(can->motor.gimbal.as_array[index]), can_rx->rx_data);
break;
case CAN_CAP_FB_ID_BASE:
can->recive_flag |= 1 << 9;
CAN_Cap_Decode(&(can->cap.cap_feedback), can_rx->rx_data);
break;
case CAN_TOF_ID_BASE:
can->recive_flag |= 1 << 10;
CAN_Tof_Decode(&(can->tof), can_rx->rx_data);
break;
default:
break;
}
return DEVICE_OK;
}
bool CAN_CheckFlag(CAN_t *can, uint32_t flag) {
if (can == NULL) return false;
return (can->recive_flag & flag) == flag;
}
int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag) {
if (can == NULL) return DEVICE_ERR_NULL;
can->recive_flag &= ~flag;
return DEVICE_OK;
}
int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can) {
float power_limit = output->power_limit;
uint16_t cap = (uint16_t)(power_limit * CAN_CAP_RES);
raw_tx.tx_header.StdId = CAN_CAP_CTRL_ID_BASE;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = (cap >> 8) & 0xFF;
raw_tx.tx_data[1] = cap & 0xFF;
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->cap), &raw_tx.tx_header,
raw_tx.tx_data, &(can->mailbox.cap));
return DEVICE_OK;
}
void CAN_CAP_HandleOffline(CAN_Capacitor_t *cap, CAN_CapOutput_t *cap_out,
float power_chassis) {
cap->cap_status = CAN_CAP_STATUS_OFFLINE;
cap_out->power_limit = power_chassis;
}

277
User/device/can.h Normal file
View File

@@ -0,0 +1,277 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include "bsp\can.h"
#include "component\ahrs.h"
#include "component\user_math.h"
#include "device\device.h"
/* Exported constants ------------------------------------------------------- */
#define CAN_MOTOR_CHASSIS_1_RECV (1 << 0)
#define CAN_MOTOR_CHASSIS_2_RECV (1 << 1)
#define CAN_MOTOR_CHASSIS_3_RECV (1 << 2)
#define CAN_MOTOR_CHASSIS_4_RECV (1 << 3)
#define CAN_MOTOR_GIMBAL_YAW_RECV (1 << 4)
#define CAN_MOTOR_GIMBAL_PIT_RECV (1 << 5)
#define CAN_MOTOR_SHOOT_FRIC1_RECV (1 << 6)
#define CAN_MOTOR_SHOOT_FRIC2_RECV (1 << 7)
#define CAN_MOTOR_SHOOT_TRIG_RECV (1 << 8)
#define CAN_MOTOR_CAP_RECV (1 << 9)
#define CAN_TOF_RECV (1 << 10)
#define CAN_REC_CHASSIS_FINISHED \
(CAN_MOTOR_CHASSIS_1_RECV | CAN_MOTOR_CHASSIS_2_RECV | \
CAN_MOTOR_CHASSIS_3_RECV | CAN_MOTOR_CHASSIS_4_RECV)
#define CAN_REC_GIMBAL_FINISHED \
(CAN_MOTOR_GIMBAL_YAW_RECV | CAN_MOTOR_GIMBAL_PIT_RECV)
#define CAN_REC_SHOOT_FINISHED \
(CAN_MOTOR_SHOOT_FRIC1_RECV | CAN_MOTOR_SHOOT_FRIC2_RECV | \
CAN_MOTOR_SHOOT_TRIG_RECV)
#define CAN_REC_CAP_FINISHED CAN_MOTOR_CAP_RECV
#define CAN_REC_TOF_FINISHED CAN_TOF_RECV
#define CAN_MOTOR_TX_BUF_SIZE (8)
#define CAN_MOTOR_RX_BUF_SIZE (8)
#define CAN_CAP_TX_BUF_SIZE (8)
#define CAN_CAP_RX_BUF_SIZE (8)
#define CAN_TX_BUF_SIZE_MAX \
((CAN_MOTOR_TX_BUF_SIZE > CAN_CAP_TX_BUF_SIZE) ? CAN_MOTOR_TX_BUF_SIZE \
: CAN_CAP_TX_BUF_SIZE)
#define CAN_RX_BUF_SIZE_MAX \
((CAN_MOTOR_RX_BUF_SIZE > CAN_CAP_RX_BUF_SIZE) ? CAN_MOTOR_RX_BUF_SIZE \
: CAN_CAP_RX_BUF_SIZE)
/* 电机最大电流绝对值 */
#define CAN_GM6020_MAX_ABS_CUR (1)
#define CAN_M3508_MAX_ABS_CUR (20)
#define CAN_M2006_MAX_ABS_CUR (10)
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 电机型号 */
typedef enum {
CAN_MOTOR_M2006 = 0,
CAN_MOTOR_M3508,
CAN_MOTOR_GM6020,
} CAN_MotorModel_t;
typedef enum {
CAN_M3508_M1_ID = 0x201, /* 1 */
CAN_M3508_M2_ID = 0x202, /* 2 */
CAN_M3508_M3_ID = 0x203, /* 3 */
CAN_M3508_M4_ID = 0x204, /* 4 */
CAN_M3508_FRIC1_ID = 0x205, /* 5 */
CAN_M3508_FRIC2_ID = 0x206, /* 6 */
CAN_M2006_TRIG_ID = 0x207, /* 7 */
CAN_GM6020_YAW_ID = 0x209, /* 5 */
CAN_GM6020_PIT_ID = 0x20A, /* 6 */
} CAN_MotorId_t;
typedef struct {
BSP_CAN_t chassis;
BSP_CAN_t gimbal;
BSP_CAN_t shoot;
BSP_CAN_t cap;
} CAN_Params_t;
/* 电机反馈信息 */
typedef struct {
float rotor_angle;
float rotor_speed;
float torque_current;
float temp;
} CAN_MotorFeedback_t;
enum CAN_MotorChassis_e {
CAN_MOTOR_CHASSIS_M1 = 0,
CAN_MOTOR_CHASSIS_M2,
CAN_MOTOR_CHASSIS_M3,
CAN_MOTOR_CHASSIS_M4,
CAN_MOTOR_CHASSIS_NUM,
};
enum CAN_MotorGimbal_e {
CAN_MOTOR_GIMBAL_YAW = 0,
CAN_MOTOR_GIMBAL_PIT,
CAN_MOTOR_GIMBAL_NUM,
};
enum CAN_MotorShoot_e {
CAN_MOTOR_SHOOT_FRIC1 = 0,
CAN_MOTOR_SHOOT_FRIC2,
CAN_MOTOR_SHOOT_TRIG,
CAN_MOTORSHOOT_NUM,
};
typedef struct {
uint8_t num;
uint32_t id_start;
} CAN_MotorGroupInit_t;
typedef struct {
CAN_MotorGroupInit_t chassis;
CAN_MotorGroupInit_t gimbal1;
CAN_MotorGroupInit_t gimbal2;
CAN_MotorGroupInit_t shoot1;
CAN_MotorGroupInit_t shoot2;
} CAN_MotorInit_t;
typedef union {
float as_array[4];
struct {
float m1;
float m2;
float m3;
float m4;
} named;
} CAN_ChassisOutput_t;
typedef union {
float as_array[3];
AHRS_Eulr_t named;
} CAN_GimbalOutput_t;
typedef union {
float as_array[3];
struct {
float fric1;
float fric2;
float trig;
} named;
} CAN_ShootOutput_t;
typedef struct {
float power_limit;
} CAN_CapOutput_t;
typedef struct {
CAN_ChassisOutput_t chassis;
CAN_GimbalOutput_t gimbal;
CAN_ShootOutput_t shoot;
CAN_CapOutput_t cap;
} CAN_Output_t;
typedef enum {
CAN_MOTOR_GROUT_CHASSIS = 0,
CAN_MOTOR_GROUT_GIMBAL1,
CAN_MOTOR_GROUT_GIMBAL2,
CAN_MOTOR_GROUT_SHOOT1,
CAN_MOTOR_GROUT_SHOOT2,
CAN_MOTOR_GROUT_CAP,
CAN_MOTOR_GROUT_NUM,
} CAN_MotorGroup_t;
typedef enum {
CAN_CAP_STATUS_OFFLINE,
CAN_CAP_STATUS_RUNNING,
} CAN_CapStatus_t;
typedef union {
CAN_MotorFeedback_t as_array[4];
struct {
CAN_MotorFeedback_t m1;
CAN_MotorFeedback_t m2;
CAN_MotorFeedback_t m3;
CAN_MotorFeedback_t m4;
} named;
} CAN_ChassisMotor_t;
typedef union {
CAN_MotorFeedback_t as_array[2];
struct {
CAN_MotorFeedback_t yaw;
CAN_MotorFeedback_t pit;
} named;
} CAN_GimbalMotor_t;
typedef union {
CAN_MotorFeedback_t as_array[3];
struct {
CAN_MotorFeedback_t fric1;
CAN_MotorFeedback_t fric2;
CAN_MotorFeedback_t trig;
} named;
} CAN_ShootMotor_t;
typedef struct {
float input_volt;
float cap_volt;
float input_curr;
float target_power;
} CAN_CapFeedback_t;
typedef struct {
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[CAN_RX_BUF_SIZE_MAX];
} CAN_RawRx_t;
typedef struct {
CAN_TxHeaderTypeDef tx_header;
uint8_t tx_data[CAN_TX_BUF_SIZE_MAX];
} CAN_RawTx_t;
typedef struct {
float percentage;
CAN_CapStatus_t cap_status;
CAN_CapFeedback_t cap_feedback;
} CAN_Capacitor_t;
typedef struct {
CAN_ChassisMotor_t chassis;
CAN_GimbalMotor_t gimbal;
CAN_ShootMotor_t shoot;
} CAN_Motor_t;
typedef struct {
float dist;
uint8_t status;
uint16_t signal_strength;
} CAN_Tof_t;
typedef struct {
uint32_t recive_flag;
CAN_Motor_t motor;
CAN_Capacitor_t cap;
CAN_Tof_t tof;
const CAN_Params_t *param;
struct {
uint32_t chassis;
uint32_t gimbal;
uint32_t shoot;
uint32_t cap;
} mailbox;
osMessageQueueId_t msgq_raw;
} CAN_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param);
int8_t CAN_Motor_Control(CAN_MotorGroup_t group, CAN_Output_t *output,
CAN_t *can);
int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx);
bool CAN_CheckFlag(CAN_t *can, uint32_t flag);
int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag);
int8_t CAN_Cap_Control(CAN_CapOutput_t *output, CAN_t *can);
void CAN_ResetCapOut(CAN_CapOutput_t *cap_out);
void CAN_Cap_Decode(CAN_CapFeedback_t *feedback, const uint8_t *raw);
void CAN_CAP_HandleOffline(CAN_Capacitor_t *cap, CAN_CapOutput_t *cap_out,
float power_chassis);
void CAN_Tof_Decode(CAN_Tof_t *tof, const uint8_t *raw);
#ifdef __cplusplus
}
#endif

38
User/device/device.h Normal file
View File

@@ -0,0 +1,38 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#define DEVICE_OK (0)
#define DEVICE_ERR (-1)
#define DEVICE_ERR_NULL (-2)
#define DEVICE_ERR_INITED (-3)
#define DEVICE_ERR_NO_DEV (-4)
/*
SIGNAL是有bit位置区分的，一个SIGNAL只能包含一个高bit位。
不同的SIGNAL不能有相同的高bit位。
*/
#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 0)
#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 1)
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 3)
#define SIGNAL_CAN_MOTOR_RECV (1u << 4)
#define SIGNAL_CAN_UWB_RECV (1u << 5)
#define SIGNAL_CAN_CAP_RECV (1u << 6)
#define SIGNAL_DR16_RAW_REDY (1u << 7)
#define SIGNAL_IST8310_MAGN_NEW_DATA (1u << 8)
#define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
#define SIGNAL_REFEREE_RAW_REDY (1u << 10)
#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 11)
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12)
#define SIGNAL_AI_RAW_REDY (1u << 14)
#ifdef __cplusplus
}
#endif

127
User/device/dr16.c Normal file
View File

@@ -0,0 +1,127 @@
/*
DR16接收机
*/
/* Includes ----------------------------------------------------------------- */
#include "dr16.h"
#include <string.h>
#include "bsp\uart.h"
/* Private define ----------------------------------------------------------- */
#define DR16_CH_VALUE_MIN (364u)
#define DR16_CH_VALUE_MID (1024u)
#define DR16_CH_VALUE_MAX (1684u)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void DR16_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
}
static bool DR16_DataCorrupted(const DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_r_x > DR16_CH_VALUE_MAX))
return true;
if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_r_y > DR16_CH_VALUE_MAX))
return true;
if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_l_x > DR16_CH_VALUE_MAX))
return true;
if ((dr16->data.ch_l_y < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_l_y > DR16_CH_VALUE_MAX))
return true;
if (dr16->data.sw_l == 0) return true;
if (dr16->data.sw_r == 0) return true;
return false;
}
/* Exported functions ------------------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB,
DR16_RxCpltCallback);
inited = true;
return DEVICE_OK;
}
int8_t DR16_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16));
return DEVICE_OK;
}
int8_t DR16_StartDmaRecv(DR16_t *dr16) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16),
(uint8_t *)&(dr16->data),
sizeof(dr16->data)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool DR16_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_DR16_RAW_REDY);
}
int8_t DR16_ParseRC(const DR16_t *dr16, CMD_RC_t *rc) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if (DR16_DataCorrupted(dr16)) {
return DEVICE_ERR;
} else {
memset(rc, 0, sizeof(*rc));
}
float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
rc->ch_r_x = 2 * ((float)dr16->data.ch_r_x - DR16_CH_VALUE_MID) / full_range;
rc->ch_r_y = 2 * ((float)dr16->data.ch_r_y - DR16_CH_VALUE_MID) / full_range;
rc->ch_l_x = 2 * ((float)dr16->data.ch_l_x - DR16_CH_VALUE_MID) / full_range;
rc->ch_l_y = 2 * ((float)dr16->data.ch_l_y - DR16_CH_VALUE_MID) / full_range;
rc->sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
rc->sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
rc->mouse.x = dr16->data.x;
rc->mouse.y = dr16->data.y;
rc->mouse.z = dr16->data.z;
rc->mouse.l_click = dr16->data.press_l;
rc->mouse.r_click = dr16->data.press_r;
rc->key = dr16->data.key;
rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
return DEVICE_OK;
}
int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if (rc == NULL) return DEVICE_ERR_NULL;
(void)dr16;
memset(rc, 0, sizeof(*rc));
return 0;
}

48
User/device/dr16.h Normal file
View File

@@ -0,0 +1,48 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "component\cmd.h"
#include "component\user_math.h"
#include "device\device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint16_t ch_r_x : 11;
uint16_t ch_r_y : 11;
uint16_t ch_l_x : 11;
uint16_t ch_l_y : 11;
uint8_t sw_r : 2;
uint8_t sw_l : 2;
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
uint16_t key;
uint16_t res;
} DR16_Data_t;
typedef struct {
DR16_Data_t data;
} DR16_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16);
int8_t DR16_Restart(void);
int8_t DR16_StartDmaRecv(DR16_t *dr16);
bool DR16_WaitDmaCplt(uint32_t timeout);
int8_t DR16_ParseRC(const DR16_t *dr16, CMD_RC_t *rc);
int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc);
#ifdef __cplusplus
}
#endif

167
User/device/ist8310.c Normal file
View File

@@ -0,0 +1,167 @@
/*
IST8310 地磁传感器。
*/
/* Includes ----------------------------------------------------------------- */
#include "ist8310.h"
#include <gpio.h>
#include <stdbool.h>
#include <string.h>
#include "bsp\delay.h"
#include "bsp\gpio.h"
#include "bsp\i2c.h"
/* Private define ----------------------------------------------------------- */
#define IST8310_WAI (0x00)
#define IST8310_STAT1 (0x02)
#define IST8310_DATAXL (0x03)
#define IST8310_STAT2 (0x09)
#define IST8310_CNTL1 (0x0A)
#define IST8310_CNTL2 (0x0B)
#define IST8310_STR (0x0C)
#define IST8310_TEMPL (0x1C)
#define IST8310_TEMPH (0x1D)
#define IST8310_AVGCNTL (0x41)
#define IST8310_PDCNTL (0x42)
#define IST8310_CHIP_ID (0x10)
#define IST8310_IIC_ADDRESS (0x0E << 1)
#define IST8310_LEN_RX_BUFF (6)
/* Private macro ------------------------------------------------------------ */
#define IST8310_SET() \
HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_SET)
#define IST8310_RESET() \
HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_RESET)
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
uint8_t ist8310_rxbuf[IST8310_LEN_RX_BUFF];
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void IST8310_WriteSingle(uint8_t reg, uint8_t data) {
HAL_I2C_Mem_Write(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg,
I2C_MEMADD_SIZE_8BIT, &data, 1, 100);
}
static uint8_t IST8310_ReadSingle(uint8_t reg) {
uint8_t buf = 0;
HAL_I2C_Mem_Read(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg,
I2C_MEMADD_SIZE_8BIT, &buf, 1, 100);
return buf;
}
static void IST8310_Read(uint8_t reg, uint8_t *data, uint8_t len) {
if (data == NULL) return;
HAL_I2C_Mem_Read_DMA(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS,
reg, I2C_MEMADD_SIZE_8BIT, data, len);
}
static void IST8310_MemRxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_RAW_REDY);
}
static void IST8310_IntCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_NEW_DATA);
}
/* Exported functions ------------------------------------------------------- */
int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali) {
if (ist8310 == NULL) return DEVICE_ERR_NULL;
if (cali == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
ist8310->cali = cali;
IST8310_RESET();
BSP_Delay(50);
IST8310_SET();
BSP_Delay(50);
if (IST8310_ReadSingle(IST8310_WAI) != IST8310_CHIP_ID)
return DEVICE_ERR_NO_DEV;
BSP_GPIO_DisableIRQ(CMPS_INT_Pin);
BSP_I2C_RegisterCallback(BSP_I2C_COMP, HAL_I2C_MEM_RX_CPLT_CB,
IST8310_MemRxCpltCallback);
BSP_GPIO_RegisterCallback(CMPS_INT_Pin, IST8310_IntCallback);
/* Init. */
/* 0x00: Stand-By mode. 0x01: Single measurement mode. */
/* 0x08: Data ready function enable. DRDY signal active low*/
IST8310_WriteSingle(IST8310_CNTL2, 0x08);
IST8310_WriteSingle(IST8310_AVGCNTL, 0x09);
IST8310_WriteSingle(IST8310_PDCNTL, 0xC0);
IST8310_WriteSingle(IST8310_CNTL1, 0x0B);
BSP_Delay(10);
inited = true;
BSP_GPIO_EnableIRQ(CMPS_INT_Pin);
return DEVICE_OK;
}
bool IST8310_WaitNew(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_IST8310_MAGN_NEW_DATA, osFlagsWaitAll,
timeout) == SIGNAL_IST8310_MAGN_NEW_DATA);
}
int8_t IST8310_StartDmaRecv() {
IST8310_Read(IST8310_DATAXL, ist8310_rxbuf, IST8310_LEN_RX_BUFF);
return DEVICE_OK;
}
uint32_t IST8310_WaitDmaCplt() {
return osThreadFlagsWait(SIGNAL_IST8310_MAGN_RAW_REDY, osFlagsWaitAll,
osWaitForever);
}
int8_t IST8310_Parse(IST8310_t *ist8310) {
if (ist8310 == NULL) return DEVICE_ERR_NULL;
#if 1
/* Magn -> T */
int16_t raw_x, raw_y, raw_z;
memcpy(&raw_x, ist8310_rxbuf + 0, sizeof(raw_x));
memcpy(&raw_y, ist8310_rxbuf + 2, sizeof(raw_y));
memcpy(&raw_z, ist8310_rxbuf + 4, sizeof(raw_z));
ist8310->magn.x = (float)raw_x;
ist8310->magn.y = (float)raw_y;
ist8310->magn.z = (float)-raw_z;
#else
const int16_t *raw_x = (int16_t *)(ist8310_rxbuf + 0);
const int16_t *raw_y = (int16_t *)(ist8310_rxbuf + 2);
const int16_t *raw_z = (int16_t *)(ist8310_rxbuf + 4);
ist8310->magn.x = (float)*raw_x;
ist8310->magn.y = (float)*raw_y;
ist8310->magn.z = -(float)*raw_z;
#endif
ist8310->magn.x *= 3.0f / 20.0f;
ist8310->magn.y *= 3.0f / 20.0f;
ist8310->magn.z *= 3.0f / 20.0f;
ist8310->magn.x = (ist8310->magn.x - ist8310->cali->magn_offset.x) *
ist8310->cali->magn_scale.x;
ist8310->magn.y = (ist8310->magn.y - ist8310->cali->magn_offset.y) *
ist8310->cali->magn_scale.y;
ist8310->magn.z = (ist8310->magn.z - ist8310->cali->magn_offset.y) *
ist8310->cali->magn_scale.z;
return DEVICE_OK;
}

48
User/device/ist8310.h Normal file
View File

@@ -0,0 +1,48 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#include "component\ahrs.h"
#include "device\device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
struct {
float x;
float y;
float z;
} magn_offset; /* 磁力计偏置 */
struct {
float x;
float y;
float z;
} magn_scale; /* 磁力计缩放 */
} IST8310_Cali_t; /* IST8310校准数据 */
typedef struct {
AHRS_Magn_t magn;
const IST8310_Cali_t *cali;
} IST8310_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali);
int8_t IST8310_Restart(void);
bool IST8310_WaitNew(uint32_t timeout);
int8_t IST8310_StartDmaRecv();
uint32_t IST8310_WaitDmaCplt();
int8_t IST8310_Parse(IST8310_t *ist8310);
#ifdef __cplusplus
}
#endif

61
User/device/joystick.c Normal file
View File

@@ -0,0 +1,61 @@
/*
OLED模块<E6A8A1>?带的摇杆<E69187>?
*/
/* Includes ----------------------------------------------------------------- */
#include "joystick.h"
/* Include BSP相关的头文件*/
#include <adc.h>
#include "bsp_delay.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
static uint32_t adc_raw;
static Joystick_Status_t js;
int8_t Joystick_Update(Joystick_Status_t *val) {
if (val == NULL) return -1;
HAL_ADC_Start(&hadc1);
if (HAL_ADC_PollForConversion(&hadc1, 1)) return -1;
adc_raw = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
if (adc_raw < 500)
*val = JOYSTICK_PRESSED;
else if (adc_raw < 1000)
*val = JOYSTICK_LEFT;
else if (adc_raw < 2000)
*val = JOYSTICK_RIGHT;
else if (adc_raw < 3000)
*val = JOYSTICK_UP;
else if (adc_raw < 4000)
*val = JOYSTICK_DOWN;
else
*val = JOYSTICK_MID;
return 0;
}
uint8_t Joystick_WaitInput(void) {
do {
BSP_Delay(20);
Joystick_Update(&js);
} while (js == JOYSTICK_MID);
return 0;
}
uint8_t Joystick_WaitNoInput(void) {
do {
BSP_Delay(20);
Joystick_Update(&js);
} while (js != JOYSTICK_MID);
return 0;
}

27
User/device/joystick.h Normal file
View File

@@ -0,0 +1,27 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
JOYSTICK_UP,
JOYSTICK_DOWN,
JOYSTICK_LEFT,
JOYSTICK_RIGHT,
JOYSTICK_PRESSED,
JOYSTICK_MID,
} Joystick_Status_t;
/* Exported functions prototypes -------------------------------------------- */
uint8_t Joystick_Update(Joystick_Status_t *val);
uint8_t Joystick_WaitInput(void);
uint8_t Joystick_WaitNoInput(void);
#ifdef __cplusplus
}
#endif

274
User/device/mpu6500.c Normal file
View File

@@ -0,0 +1,274 @@
/*
MPU6500 + IST8310 组合惯性测量单元。
*/
/* Includes ----------------------------------------------------------------- */
#include "imu.h"
/* Include 标准库 */
#include <stdbool.h>
#include <string.h>
/* Include BSP相关的头文件 */
#include "bsp_delay.h"
#include "bsp_spi.h"
/* Include Component相关的头文件 */
#include "user_math.h"
/* Private define ----------------------------------------------------------- */
#define MPU6500_SELF_TEST_XG (0x00)
#define MPU6500_SELF_TEST_YG (0x01)
#define MPU6500_SELF_TEST_ZG (0x02)
#define MPU6500_SELF_TEST_XA (0x0D)
#define MPU6500_SELF_TEST_YA (0x0E)
#define MPU6500_SEFLF_TEST_ZA (0x0F)
#define MPU6500_XG_OFFSET_H (0x13)
#define MPU6500_XG_OFFSET_L (0x14)
#define MPU6500_YG_OFFSET_H (0x15)
#define MPU6500_YG_OFFSET_L (0x16)
#define MPU6500_ZG_OFFSET_H (0x17)
#define MPU6500_ZG_OFFSET_L (0x18)
#define MPU6500_SMPLRT_DIV (0x19)
#define MPU6500_CONFIG (0x1A)
#define MPU6500_GYRO_CONFIG (0x1B)
#define MPU6500_ACCEL_CONFIG (0x1C)
#define MPU6500_ACCEL_CONFIG_2 (0x1D)
#define MPU6500_LP_ACCEL_ODR (0x1E)
#define MPU6500_MOT_THR (0x1F)
#define MPU6500_FIFO_EN (0x23)
#define MPU6500_I2C_MST_CTRL (0x24)
#define MPU6500_I2C_SLV0_ADDR (0x25)
#define MPU6500_I2C_SLV0_REG (0x26)
#define MPU6500_I2C_SLV0_CTRL (0x27)
#define MPU6500_I2C_SLV1_ADDR (0x28)
#define MPU6500_I2C_SLV1_REG (0x29)
#define MPU6500_I2C_SLV1_CTRL (0x2A)
#define MPU6500_I2C_SLV2_ADDR (0x2B)
#define MPU6500_I2C_SLV2_REG (0x2C)
#define MPU6500_I2C_SLV2_CTRL (0x2D)
#define MPU6500_I2C_SLV3_ADDR (0x2E)
#define MPU6500_I2C_SLV3_REG (0x2F)
#define MPU6500_I2C_SLV3_CTRL (0x30)
#define MPU6500_I2C_SLV4_ADDR (0x31)
#define MPU6500_I2C_SLV4_REG (0x32)
#define MPU6500_I2C_SLV4_DO (0x33)
#define MPU6500_I2C_SLV4_CTRL (0x34)
#define MPU6500_I2C_SLV4_DI (0x35)
#define MPU6500_I2C_MST_STATUS (0x36)
#define MPU6500_INT_PIN_CFG (0x37)
#define MPU6500_INT_ENABLE (0x38)
#define MPU6500_INT_STATUS (0x3A)
#define MPU6500_ACCEL_XOUT_H (0x3B)
#define MPU6500_ACCEL_XOUT_L (0x3C)
#define MPU6500_ACCEL_YOUT_H (0x3D)
#define MPU6500_ACCEL_YOUT_L (0x3E)
#define MPU6500_ACCEL_ZOUT_H (0x3F)
#define MPU6500_ACCEL_ZOUT_L (0x40)
#define MPU6500_TEMP_OUT_H (0x41)
#define MPU6500_TEMP_OUT_L (0x42)
#define MPU6500_GYRO_XOUT_H (0x43)
#define MPU6500_GYRO_XOUT_L (0x44)
#define MPU6500_GYRO_YOUT_H (0x45)
#define MPU6500_GYRO_YOUT_L (0x46)
#define MPU6500_GYRO_ZOUT_H (0x47)
#define MPU6500_GYRO_ZOUT_L (0x48)
#define MPU6500_EXT_SENS_DATA_00 (0x49)
#define MPU6500_EXT_SENS_DATA_01 (0x4A)
#define MPU6500_EXT_SENS_DATA_02 (0x4B)
#define MPU6500_EXT_SENS_DATA_03 (0x4C)
#define MPU6500_EXT_SENS_DATA_04 (0x4D)
#define MPU6500_EXT_SENS_DATA_05 (0x4E)
#define MPU6500_EXT_SENS_DATA_06 (0x4F)
#define MPU6500_EXT_SENS_DATA_07 (0x50)
#define MPU6500_EXT_SENS_DATA_08 (0x51)
#define MPU6500_EXT_SENS_DATA_09 (0x52)
#define MPU6500_EXT_SENS_DATA_10 (0x53)
#define MPU6500_EXT_SENS_DATA_11 (0x54)
#define MPU6500_EXT_SENS_DATA_12 (0x55)
#define MPU6500_EXT_SENS_DATA_13 (0x56)
#define MPU6500_EXT_SENS_DATA_14 (0x57)
#define MPU6500_EXT_SENS_DATA_15 (0x58)
#define MPU6500_EXT_SENS_DATA_16 (0x59)
#define MPU6500_EXT_SENS_DATA_17 (0x5A)
#define MPU6500_EXT_SENS_DATA_18 (0x5B)
#define MPU6500_EXT_SENS_DATA_19 (0x5C)
#define MPU6500_EXT_SENS_DATA_20 (0x5D)
#define MPU6500_EXT_SENS_DATA_21 (0x5E)
#define MPU6500_EXT_SENS_DATA_22 (0x5F)
#define MPU6500_EXT_SENS_DATA_23 (0x60)
#define MPU6500_I2C_SLV0_DO (0x63)
#define MPU6500_I2C_SLV1_DO (0x64)
#define MPU6500_I2C_SLV2_DO (0x65)
#define MPU6500_I2C_SLV3_DO (0x66)
#define MPU6500_I2C_MST_DELAY_CTRL (0x67)
#define MPU6500_SIGNAL_PATH_RESET (0x68)
#define MPU6500_MOT_DETECT_CTRL (0x69)
#define MPU6500_USER_CTRL (0x6A)
#define MPU6500_PWR_MGMT_1 (0x6B)
#define MPU6500_PWR_MGMT_2 (0x6C)
#define MPU6500_FIFO_COUNTH (0x72)
#define MPU6500_FIFO_COUNTL (0x73)
#define MPU6500_FIFO_R_W (0x74)
#define MPU6500_WHO_AM_I (0x75)
#define MPU6500_XA_OFFSET_H (0x77)
#define MPU6500_XA_OFFSET_L (0x78)
#define MPU6500_YA_OFFSET_H (0x7A)
#define MPU6500_YA_OFFSET_L (0x7B)
#define MPU6500_ZA_OFFSET_H (0x7D)
#define MPU6500_ZA_OFFSET_L (0x7E)
#define MPU6500_ID (0x70)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
__packed struct {
__packed struct {
int16_t x;
int16_t y;
int16_t z;
} accl;
int16_t temp;
__packed struct {
int16_t x;
int16_t y;
int16_t z;
} gyro;
__packed struct {
int16_t x;
int16_t y;
int16_t z;
} magn;
} packed;
/* Private variables -------------------------------------------------------- */
static uint8_t buffer[2];
static IMU_t *gimu;
static bool inited = false;
/* Private function -------------------------------------------------------- */
void IMU_RxCpltCallback(void) {
osThreadFlagsSet(gimu->received_alert,
IMU_SIGNAL_RAW_ACCL_REDY | IMU_SIGNAL_RAW_GYRO_REDY);
}
void IMU_ExtIntCallback(void) { IMU_StartReceiving(gimu); }
static void MPU_WriteSingle(uint8_t reg, uint8_t data) {
buffer[0] = (reg & 0x7f);
buffer[1] = data;
BSP_SPI_Transmit(BSP_SPI_IMU, buffer, 2);
BSP_Delay(5);
}
static uint8_t MPU_ReadSingle(uint8_t reg) {
buffer[0] = (reg | 0x80);
BSP_SPI_Transmit(BSP_SPI_IMU, buffer, 1);
BSP_SPI_Receive(BSP_SPI_IMU, buffer, 1);
return buffer[0];
}
static void MPU_Read(uint8_t reg, uint8_t *data, uint8_t len) {
if (data == NULL) return;
buffer[0] = (reg | 0x80);
BSP_SPI_Transmit(BSP_SPI_IMU, buffer, 1);
BSP_SPI_Receive(BSP_SPI_IMU, data, len);
}
/* Exported functions ------------------------------------------------------- */
int8_t IMU_Init(IMU_t *imu) {
if (imu == NULL) return -1;
if (inited) return -1;
if (MPU_ReadSingle(MPU6500_WHO_AM_I) != MPU6500_ID) return -1;
gimu = imu;
/* MPU6500 init. */
MPU_WriteSingle(MPU6500_PWR_MGMT_1, 0x80); /* Reset device */
BSP_Delay(100);
MPU_WriteSingle(MPU6500_SIGNAL_PATH_RESET, 0x0f); /* Reset device */
BSP_Delay(100);
MPU_WriteSingle(MPU6500_PWR_MGMT_1, 0x03); /* Clock source -> gyro-z */
MPU_WriteSingle(MPU6500_PWR_MGMT_2, 0x00); /* Enable acc & gyro */
MPU_WriteSingle(MPU6500_CONFIG, 0x04); /* Disable acc LPF */
MPU_WriteSingle(MPU6500_GYRO_CONFIG,
0x18); /* gyro range 0x10:+-1000dps 0x18:+-2000dps */
MPU_WriteSingle(MPU6500_ACCEL_CONFIG, 0x10); /* +-8G */
MPU_WriteSingle(MPU6500_ACCEL_CONFIG_2, 0x10); /* Disable acc LPF */
BSP_Delay(10);
BSP_SPI_RegisterCallback(BSP_SPI_IMU, BSP_SPI_RX_COMPLETE_CB,
IMU_RxCpltCallback);
inited = true;
return 0;
}
IMU_t *IMU_GetDevice(void) {
if (inited) {
return gimu;
}
return NULL;
}
uint8_t IMU_StartReceiving(IMU_t *imu) {
MPU_Read(MPU6500_ACCEL_XOUT_H, (uint8_t *)&(imu->raw), 20);
return 0;
}
/* magn_scale[3] is initially zero. So data from uncalibrated magnentmeter is
* ignored. */
/* Sensor use right-handed coordinate system. */
/*
* x < R (RoboMaster trademark)
* y
* UP is z
*/
int8_t IMU_Parse(IMU_t *imu) {
if (imu == NULL) return -1;
uint8_t raw[20];
memcpy(raw, &(imu->raw), sizeof(imu->raw));
for (uint8_t i = 0; i < 20; i += 2)
((int16_t *)&imu->raw)[i / 2] = (raw[i] << 8) | raw[i + 1];
imu->data.temp = 21.0f + (float)imu->raw.temp / 333.87f;
/* TODO: Strage accl data. Potential sensor damage. */
imu->data.accl.x = (float)imu->raw.accl.x / 4096.0f;
imu->data.accl.y = (float)imu->raw.accl.y / 4096.0f;
imu->data.accl.z = (float)imu->raw.accl.z / 4096.0f;
/* Convert gyroscope imu_raw to degrees/sec, then, to radians/sec */
imu->data.gyro.x = (float)(imu->raw.gyro.x - imu->cali.gyro_offset[0]) /
16.384f / 180.0f * M_PI;
imu->data.gyro.y = (float)(imu->raw.gyro.y - imu->cali.gyro_offset[1]) /
16.384f / 180.0f * M_PI;
imu->data.gyro.z = (float)(imu->raw.gyro.z - imu->cali.gyro_offset[2]) /
16.384f / 180.0f * M_PI;
#if 0
imu->data.magn.x = (float)((imu->raw.magn.x - imu->cali.magn_offset[0]) * imu->cali.magn_scale[0]);
imu->data.magn.y = (float)((imu->raw.magn.y - imu->cali.magn_offset[1]) * imu->cali.magn_scale[1]);
imu->data.magn.z = (float)((imu->raw.magn.z - imu->cali.magn_offset[2]) * imu->cali.magn_scale[2]);
#else
imu->data.magn.x = 0.0f;
imu->data.magn.y = 0.0f;
imu->data.magn.z = 0.0f;
#endif
return 0;
}

381
User/device/oled.c Normal file
View File

@@ -0,0 +1,381 @@
/*
OLED显示模块。
使用SPI通信时，需要#define OLED_USE_SPI
使用I2C通信时，需要#define OLED_USE_I2C
*/
/* Includes ----------------------------------------------------------------- */
/* Include 自身的头文件。*/
#include "oled.h"
/* Include 标准库。*/
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
/* Include HAL相关的头文件。*/
#include "bsp_delay.h"
#include "bsp_spi.h"
#include <gpio.h>
#ifdef OLED_USE_SPI
#include "spi.h"
#
#elif defined OLED_USE_I2C
#include "i2c.h"
#endif
/* Include Component相关的头文件 */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
#ifdef OLED_USE_SPI
#define OLED_CMD_Set() HAL_GPIO_WritePin(OLED_DC_GPIO_Port, OLED_DC_Pin, GPIO_PIN_SET)
#define OLED_CMD_Clr() HAL_GPIO_WritePin(OLED_DC_GPIO_Port, OLED_DC_Pin, GPIO_PIN_RESET)
#define OLED_RST_Set() HAL_GPIO_WritePin(OLED_RST_GPIO_Port, OLED_RST_Pin, GPIO_PIN_SET)
#define OLED_RST_Clr() HAL_GPIO_WritePin(OLED_RST_GPIO_Port, OLED_RST_Pin, GPIO_PIN_RESET)
#elif defined OLED_USE_I2C
#define OLED_I2C_HANDLE hi2c1
#endif
/* Private typedef ---------------------------------------------------------- */
typedef enum {
OLED_WriteCMD = 0,
OLED_WriteData = 1,
}OLED_Write_t;
/* Private variables -------------------------------------------------------- */
static const uint8_t oled_font[95][8] = {
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,}, /* " ", 0 */
{0x00,0x00,0x00,0xcf,0xcf,0x00,0x00,0x00,}, /* "!", 1 */
{0x00,0x0c,0x06,0x00,0x0c,0x06,0x00,0x00,}, /* """, 2 */
{0x24,0xe4,0x3c,0x27,0xe4,0x3c,0x27,0x24,}, /* "#", 3 */
{0x00,0x20,0x46,0xf9,0x9f,0x62,0x04,0x00,}, /* "$", 4 */
{0x06,0x09,0xc6,0x30,0x0c,0x63,0x90,0x60,}, /* "%", 5 */
{0x00,0x00,0x6e,0x91,0xa9,0x46,0xa0,0x00,}, /* "&", 6 */
{0x00,0x00,0x00,0x1c,0x0e,0x00,0x00,0x00,}, /* "'", 7 */
{0x00,0x00,0x3c,0x42,0x81,0x00,0x00,0x00,}, /* "(", 8 */
{0x00,0x00,0x00,0x81,0x42,0x3c,0x00,0x00,}, /* ")", 9 */
{0x00,0x10,0x54,0x38,0x38,0x54,0x10,0x00,}, /* "*", 10 */
{0x00,0x10,0x10,0xfc,0x10,0x10,0x00,0x00,}, /* "+", 11 */
{0x00,0x00,0x00,0xc0,0x60,0x00,0x00,0x00,}, /* ",", 12 */
{0x00,0x00,0x10,0x10,0x10,0x10,0x00,0x00,}, /* "-", 13 */
{0x00,0x00,0x00,0x00,0xc0,0xc0,0x00,0x00,}, /* ".", 14 */
{0x00,0x00,0x00,0xc0,0x38,0x07,0x00,0x00,}, /* "/", 15 */
{0x00,0x00,0x7c,0x92,0x8a,0x7c,0x00,0x00,}, /* "0", 16 */
{0x00,0x00,0x00,0x84,0xfe,0x80,0x00,0x00,}, /* "1", 17 */
{0x00,0x00,0x8c,0xc2,0xa2,0x9c,0x00,0x00,}, /* "2", 18 */
{0x00,0x00,0x44,0x92,0x92,0x6c,0x00,0x00,}, /* "3", 19 */
{0x00,0x20,0x38,0x24,0xfe,0x20,0x00,0x00,}, /* "4", 20 */
{0x00,0x00,0x5e,0x92,0x92,0x62,0x00,0x00,}, /* "5", 21 */
{0x00,0x00,0x78,0x94,0x92,0x62,0x00,0x00,}, /* "6", 22 */
{0x00,0x00,0x82,0x62,0x1a,0x06,0x00,0x00,}, /* "7", 23 */
{0x00,0x00,0x6c,0x92,0x92,0x6c,0x00,0x00,}, /* "8", 24 */
{0x00,0x00,0x8c,0x52,0x32,0x1c,0x00,0x00,}, /* "9", 25 */
{0x00,0x00,0x00,0x6c,0x6c,0x00,0x00,0x00,}, /* ":", 26 */
{0x00,0x00,0x80,0xec,0x6c,0x00,0x00,0x00,}, /* ";", 27 */
{0x00,0x00,0x10,0x28,0x44,0x00,0x00,0x00,}, /* "<", 28 */
{0x00,0x00,0x24,0x24,0x24,0x24,0x00,0x00,}, /* "=", 29 */
{0x00,0x00,0x00,0x44,0x28,0x10,0x00,0x00,}, /* ">", 30 */
{0x00,0x00,0x0c,0xa2,0x92,0x1c,0x00,0x00,}, /* "?", 31 */
{0x00,0x3c,0x42,0x99,0xa5,0xa2,0x3c,0x00,}, /* "@", 32 */
{0x00,0xe0,0x1c,0x12,0x12,0x1c,0xe0,0x00,}, /* "A", 33 */
{0x00,0xfe,0x92,0x92,0x9c,0x90,0x60,0x00,}, /* "B", 34 */
{0x00,0x38,0x44,0x82,0x82,0x82,0x44,0x00,}, /* "C", 35 */
{0x00,0xfe,0x82,0x82,0x82,0x82,0x7c,0x00,}, /* "D", 36 */
{0x00,0xfe,0x92,0x92,0x92,0x92,0x92,0x00,}, /* "E", 37 */
{0x00,0xfe,0x12,0x12,0x12,0x12,0x02,0x00,}, /* "F", 38 */
{0x00,0x7c,0x82,0x92,0x92,0x72,0x00,0x00,}, /* "G", 39 */
{0x00,0xfe,0x10,0x10,0x10,0x10,0xfe,0x00,}, /* "H", 40 */
{0x00,0x82,0x82,0xfe,0x82,0x82,0x00,0x00,}, /* "I", 41 */
{0x00,0x82,0x82,0x7e,0x02,0x02,0x00,0x00,}, /* "J", 42 */
{0x00,0xfe,0x10,0x28,0x44,0x82,0x00,0x00,}, /* "K", 43 */
{0x00,0xfe,0x80,0x80,0x80,0x80,0x00,0x00,}, /* "L", 44 */
{0xfc,0x02,0x04,0xf8,0x04,0x02,0xfc,0x00,}, /* "M", 45 */
{0x00,0xfe,0x04,0x18,0x30,0x40,0xfe,0x00,}, /* "N", 46 */
{0x00,0x7c,0x82,0x82,0x82,0x82,0x7c,0x00,}, /* "O", 47 */
{0x00,0x00,0xfe,0x12,0x12,0x0c,0x00,0x00,}, /* "P", 48 */
{0x00,0x00,0x3c,0x42,0xc2,0xbc,0x00,0x00,}, /* "Q", 49 */
{0x00,0x00,0xfe,0x32,0x52,0x8c,0x00,0x00,}, /* "R", 50 */
{0x00,0x00,0x4c,0x92,0x92,0x64,0x00,0x00,}, /* "S", 51 */
{0x00,0x02,0x02,0xfe,0x02,0x02,0x00,0x00,}, /* "T", 52 */
{0x00,0x7e,0x80,0x80,0x80,0x80,0x7e,0x00,}, /* "U", 53 */
{0x00,0x0c,0x30,0xc0,0x30,0x0c,0x00,0x00,}, /* "V", 54 */
{0x7c,0x80,0x80,0x78,0x80,0x80,0x7c,0x00,}, /* "W", 55 */
{0x00,0x84,0x48,0x30,0x30,0x48,0x84,0x00,}, /* "X", 56 */
{0x00,0x06,0x08,0xf0,0x08,0x06,0x00,0x00,}, /* "Y", 57 */
{0x00,0x00,0xc2,0xa2,0x92,0x8e,0x00,0x00,}, /* "Z", 58 */
{0x00,0x00,0xfe,0x82,0x82,0x82,0x00,0x00,}, /* "[", 59 */
{0x00,0x00,0x06,0x18,0x60,0x80,0x00,0x00,}, /* "\", 60 */
{0x00,0x00,0x82,0x82,0x82,0xfe,0x00,0x00,}, /* "]", 61 */
{0x00,0x30,0x0c,0x02,0x0c,0x30,0x00,0x00,}, /* "^", 62 */
{0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x00,}, /* "_", 63 */
{0x00,0x00,0x04,0x0c,0x18,0x00,0x00,0x00,}, /* "`", 64 */
{0x00,0x00,0x60,0x90,0x90,0xe0,0x00,0x00,}, /* "a", 65 */
{0x00,0x00,0xf8,0xa0,0xe0,0x00,0x00,0x00,}, /* "b", 66 */
{0x00,0x00,0x60,0x90,0x90,0x00,0x00,0x00,}, /* "c", 67 */
{0x00,0x00,0xe0,0xa0,0xf8,0x00,0x00,0x00,}, /* "d", 68 */
{0x00,0x00,0x70,0xa8,0xa8,0x90,0x00,0x00,}, /* "e", 69 */
{0x00,0x00,0x10,0xf8,0x14,0x00,0x00,0x00,}, /* "f", 70 */
{0x00,0x00,0xd8,0xa4,0x7c,0x00,0x00,0x00,}, /* "g", 71 */
{0x00,0x00,0xf8,0x20,0xe0,0x00,0x00,0x00,}, /* "h", 72 */
{0x00,0x00,0x00,0xe8,0x00,0x00,0x00,0x00,}, /* "i", 73 */
{0x00,0x00,0x40,0x90,0x74,0x00,0x00,0x00,}, /* "j", 74 */
{0x00,0x00,0xf8,0x60,0x90,0x00,0x00,0x00,}, /* "k", 75 */
{0x00,0x00,0x78,0x80,0x80,0x00,0x00,0x00,}, /* "l", 76 */
{0x00,0xe0,0x10,0xe0,0x10,0xe0,0x00,0x00,}, /* "m", 77 */
{0x00,0x00,0xf0,0x10,0x10,0xe0,0x00,0x00,}, /* "n", 78 */
{0x00,0x00,0x60,0x90,0x90,0x60,0x00,0x00,}, /* "o", 79 */
{0x00,0x00,0xf0,0x48,0x48,0x30,0x00,0x00,}, /* "p", 80 */
{0x00,0x00,0x30,0x48,0x48,0xf0,0x00,0x00,}, /* "q", 81 */
{0x00,0x00,0x00,0xf0,0x20,0x10,0x00,0x00,}, /* "r", 82 */
{0x00,0x00,0x90,0xa8,0xa8,0x48,0x00,0x00,}, /* "s", 83 */
{0x00,0x10,0x10,0xf8,0x90,0x90,0x00,0x00,}, /* "t", 84 */
{0x00,0x00,0x78,0x80,0x80,0xf8,0x00,0x00,}, /* "u", 85 */
{0x00,0x18,0x60,0x80,0x60,0x18,0x00,0x00,}, /* "v", 86 */
{0x00,0x38,0xc0,0x38,0xc0,0x38,0x00,0x00,}, /* "w", 87 */
{0x00,0x88,0x50,0x20,0x50,0x88,0x00,0x00,}, /* "x", 88 */
{0x00,0x8c,0x50,0x20,0x10,0x0c,0x00,0x00,}, /* "y", 89 */
{0x00,0x88,0xc8,0xa8,0x98,0x88,0x00,0x00,}, /* "z", 90 */
{0x00,0x00,0x10,0x7c,0x82,0x00,0x00,0x00,}, /* "{", 91 */
{0x00,0x00,0x00,0xfe,0x00,0x00,0x00,0x00,}, /* "|", 92 */
{0x00,0x00,0x00,0x82,0x7c,0x10,0x00,0x00,}, /* "}", 93 */
{0x00,0x08,0x04,0x04,0x08,0x10,0x10,0x08,}, /* "~", 94 */
};
static const uint8_t gram_logo[8][128] = {
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x06,0x0E,0X1E,0X3E,0X7E,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFE,0XFC,0XFC,0XF8,0XF8,0XF0,0XE0,0XC0,0X80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x07,0x0F,0X1F,0X3F,0X7F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XF7,0XE7,0XC7,0X87,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x0F,0X1F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFE,0XF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0XC0,0XFC,0XFC,0XFC,0XFC,0XFC,0XFD,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFE,0XFC,0XFC,0XFC,0XFC,0XFC,0XFC,0XFC,0XFC,0XFC,0XFC,0XFE,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X7F,0X3F,0x0F,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0X80,0XF0,0XFE,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X1F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0X1F,0X3F,0X7F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XEF,0XCF,0X87,0x07,0x03,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0XE0,0XFC,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X3F,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0X40,0X40,0X40,0XC0,0XC0,0XC0,0XC0,0XC1,0XC3,0XC7,0XCF,0XDF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFE,0XFC,0XF8,0XF0,0XE0,0XC0,0X80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x08,0x0C,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x03,0x03,0x07,0x07,0x0F,0x0F,0X1F,0X1F,0X3F,0X3F,0X7F,0X7F,0XFF,0XF3,0XE3,0XC3,0X83,0x03,0x03,0x03,0x03,0x03,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x03,0x03,0x06,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x60,0x78,0x7B,0x1B,0x1B,0x1B,0x7B,0x7F,0x6F,0x4E,0x00,0x3E,0x7F,0x63,0x63,0x63,0x7F,0x3E,0x00,0x78,0x7B,0x6B,0x6B,0x6B,0x6B,0x7B,0x7F,0x36,0x00,0x3E,0x7F,0x63,0x63,0x63,0x7F,0x3E,0x40,0x60,0x78,0x3D,0x07,0x1F,0x7C,0x70,0x1D,0x07,0x7F,0x78,0x40,0x00,0x60,0x70,0x38,0x5D,0x6F,0x67,0x6F,0x7C,0x70,0x40,0x00,0x40,0x66,0x6F,0x6B,0x6B,0x6B,0x6B,0x7B,0x31,0x02,0x03,0x03,0x7F,0x7F,0x03,0x03,0x61,0x68,0x6B,0x6B,0x6B,0x6B,0x6B,0x0B,0x03,0x60,0x78,0x7B,0x1B,0x1B,0x1B,0x7B,0x7B,0x6F,0x4E,0x00,0x46,0x6F,0x6F,0x6B,0x6B,0x6B,0x7B,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
};
static OLED_t *goled;
static bool inited = false;
/* Private function ---------------------------------------------- */
static void OLED_WriteByte(uint8_t data, OLED_Write_t type) {
#ifdef OLED_USE_SPI
switch(type) {
case OLED_WriteCMD:
OLED_CMD_Clr();
break;
case OLED_WriteData:
OLED_CMD_Set();
break;
}
BSP_SPI_Transmit(BSP_SPI_OLED, &data, 1);
#elif defined OLED_USE_I2C
uint8_t cmd_data[2];
switch(type) {
case OLED_WriteCMD:
cmd_data[0] = 0x00;
break;
case OLED_WriteData:
cmd_data[0] = 0x40;
break;
}
cmd_data[1] = data;
HAL_I2C_Master_Transmit_DMA(OLED_I2C, OLED_I2C_ADDRESS, cmd_data, 2);
#endif
BSP_Delay(1);
}
static void OLED_WriteSequenceData(uint8_t *data, uint16_t len) {
#ifdef OLED_USE_SPI
OLED_CMD_Set();
HAL_SPI_Transmit_DMA(&hspi1, data, len);
#elif defined OLED_USE_I2C
/* TODO */
uint8_t cmd_data[2];
cmd_data[0] = 0x40;
cmd_data[1] = data;
HAL_I2C_Master_Transmit_DMA(OLED_I2C, OLED_I2C_ADDRESS, cmd_data, 2);
#endif
}
/* Exported functions ------------------------------------------------------- */
int8_t OLED_Init(OLED_t *oled) {
if (oled == NULL)
return -1;
if (inited)
return -1;
oled->cursor.column = 0;
oled->cursor.page = 0;
oled->modified = true;
goled = oled;
OLED_RST_Clr();
BSP_Delay(1);
OLED_RST_Set();
BSP_Delay(5);
OLED_WriteByte(0xae, OLED_WriteCMD); /* Dot martix display off. */
OLED_WriteByte(0x00, OLED_WriteCMD); /* Set lower column address:(00H~0FH). */
OLED_WriteByte(0x10, OLED_WriteCMD); /* Set higher column address:(10H~1FH). */
OLED_WriteByte(0x20, OLED_WriteCMD); /* Set Memory Addressing Mode */
OLED_WriteByte(0x10, OLED_WriteCMD); /* 00,Horizontal Addressing Mode;01,Vertical Addressing Mode;10,Page Addressing Mode (RESET);11,Invalid */
OLED_WriteByte(0x32, OLED_WriteCMD); /* Set pump voltage 8v. */
OLED_WriteByte(0x40, OLED_WriteCMD); /* Set display start line:(40H~7FH). */
OLED_WriteByte(0x81, OLED_WriteCMD); /* Contarst control. */
OLED_WriteByte(0x6f, OLED_WriteCMD); /* Contarst: 0x00~0xff. */
OLED_WriteByte(0xa1, OLED_WriteCMD); /* Set segment re-map. 0xa0 or 0xa1. */
OLED_WriteByte(0xa4, OLED_WriteCMD); /* Entire display off. */
OLED_WriteByte(0xa6, OLED_WriteCMD); /* Set normal display. */
OLED_WriteByte(0xa8, OLED_WriteCMD); /* Set multiplex ratio. */
OLED_WriteByte(0x3f, OLED_WriteCMD);
OLED_WriteByte(0xad, OLED_WriteCMD); /* Set DC/DC booster. */
OLED_WriteByte(0x8b, OLED_WriteCMD);
OLED_WriteByte(0xb0, OLED_WriteCMD); /* Set page address. */
OLED_WriteByte(0xc8, OLED_WriteCMD); /* Com scan COM0->COM63. */
OLED_WriteByte(0xd3, OLED_WriteCMD); /* Set display offset. */
OLED_WriteByte(0x00, OLED_WriteCMD);
OLED_WriteByte(0xd5, OLED_WriteCMD); /* Set frame frequency. */
OLED_WriteByte(0xf0, OLED_WriteCMD);
OLED_WriteByte(0xd9, OLED_WriteCMD); /* Set pre_charge period. */
OLED_WriteByte(0x22, OLED_WriteCMD);
OLED_WriteByte(0xda, OLED_WriteCMD); /* Com pin configuration. */
OLED_WriteByte(0x12, OLED_WriteCMD);
OLED_WriteByte(0xdb, OLED_WriteCMD); /* Set vcom deselect level. */
OLED_WriteByte(0x20, OLED_WriteCMD);
OLED_WriteByte(0x8d, OLED_WriteCMD); /* Set DC-DC enable */
OLED_WriteByte(0x14, OLED_WriteCMD);
OLED_WriteByte(0xaf, OLED_WriteCMD); /* Display on. */
memcmp(oled->gram, gram_logo, sizeof(oled->gram));
OLED_SetAllRam(oled, OLED_PEN_WRITE); /* Clear memory. */
OLED_Refresh(oled); /* Display initial picture. */
return 0;
}
OLED_t *OLED_GetDevice(void) {
if (inited) {
return goled;
}
return NULL;
}
int8_t OLED_PrintRam(OLED_t *oled, const char* str) {
if (str == NULL)
return -1;
oled->modified = true;
for (uint16_t i = 0; str[i] != '\0'; i++) {
uint8_t c = str[i];
if (c < 32) {
switch (c) {
case '\n':
oled->cursor.page ++;
oled->cursor.column = 0;
break;
case '\t':
oled->cursor.column += 16;
break;
}
} else if (c < 126) {
if (oled->cursor.column == 128) {
oled->cursor.page ++;
oled->cursor.column = 0;
}
if (oled->cursor.page == 8) {
OLED_SetAllRam(oled, OLED_PEN_CLEAR);
oled->cursor.page = 0;
}
memcpy(&oled->gram[oled->cursor.page][oled->cursor.column], oled_font[c- 32], sizeof(oled_font[0]));
oled->cursor.column += 8;
}
}
return 0;
}
uint8_t OLED_RewindRam(OLED_t *oled) {
oled->modified = true;
OLED_SetAllRam(oled, OLED_PEN_CLEAR);
oled->cursor.page = 0;
oled->cursor.column = 0;
return 0;
}
uint8_t OLED_SetAllRam(OLED_t *oled, OLED_Pen_t pen) {
switch(pen) {
case OLED_PEN_WRITE:
memset(oled->gram, -1, sizeof(oled->gram));
break;
case OLED_PEN_CLEAR:
memset(oled->gram, 0, sizeof(oled->gram));
break;
case OLED_PEN_INVERSION:
for (uint8_t i = 0; i < 8; i++) {
for (uint8_t n = 0; n < 128; n++) {
oled->gram[i][n] = ~oled->gram[i][n];
}
}
break;
}
return 0;
}
uint8_t OLED_DisplayOn(OLED_t *oled) {
OLED_WriteByte(0x8d, OLED_WriteCMD);
OLED_WriteByte(0x14, OLED_WriteCMD);
OLED_WriteByte(0xaf, OLED_WriteCMD);
return 0;
}
uint8_t OLED_DisplayOff(OLED_t *oled) {
OLED_WriteByte(0x8d, OLED_WriteCMD);
OLED_WriteByte(0x10, OLED_WriteCMD);
OLED_WriteByte(0xae, OLED_WriteCMD);
return 0;
}
void OLED_SetCursor(uint8_t x, uint8_t y)
{
OLED_WriteByte((0xb0 + y), OLED_WriteCMD); /*set page address y */
OLED_WriteByte(((x&0xf0)>>4)|0x10, OLED_WriteCMD); /*set column high address */
OLED_WriteByte((x&0x0f), OLED_WriteCMD); /*set column low address */
}
uint8_t OLED_Refresh(OLED_t *oled) {
for (uint8_t i = 0; i < 8; i++) {
OLED_SetCursor(0, i);
for (uint8_t n = 0; n < 8; n++) {
OLED_WriteByte(oled->gram[i][n], OLED_WriteData);
}
}
oled->modified = false;
return 0;
}

45
User/device/oled.h Normal file
View File

@@ -0,0 +1,45 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdbool.h>
#include <stdint.h>
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
OLED_PEN_CLEAR = 0,
OLED_PEN_WRITE = 1,
OLED_PEN_INVERSION = 2,
} OLED_Pen_t;
typedef struct {
uint8_t column;
s uint8_t page;
} OLED_Cursor_t;
typedef struct {
OLED_Cursor_t cursor;
uint8_t gram[8][128];
bool modified;
} OLED_t;
/* Exported functions prototypes -------------------------------------------- */
uint8_t OLED_Init(OLED_t *oled);
OLED_t *OLED_GetDevice(void);
uint8_t OLED_PrintRam(OLED_t *oled, const char *str);
uint8_t OLED_RewindRam(OLED_t *oled);
uint8_t OLED_SetAllRam(OLED_t *oled, OLED_Pen_t pen);
uint8_t OLED_DisplayOn(OLED_t *oled);
uint8_t OLED_DisplayOff(OLED_t *oled);
uint8_t OLED_Refresh(OLED_t *oled);
#ifdef __cplusplus
}
#endif

789
User/device/referee.c Normal file
View File

@@ -0,0 +1,789 @@
/*
裁判系统抽象。
*/
/* Includes ----------------------------------------------------------------- */
#include "referee.h"
#include <string.h>
#include "bsp\delay.h"
#include "bsp\uart.h"
#include "component\crc16.h"
#include "component\crc8.h"
#include "component\user_math.h"
/* Private define ----------------------------------------------------------- */
#define REF_HEADER_SOF (0xA5)
#define REF_LEN_RX_BUFF (0xFF)
#define REF_UI_FAST_REFRESH_FREQ (50)
#define REF_UI_SLOW_REFRESH_FREQ (0.2f)
#define REF_UI_BOX_UP_OFFSET (4)
#define REF_UI_BOX_BOT_OFFSET (-14)
#define REF_UI_RIGHT_START_POS (0.85f)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[REF_LEN_RX_BUFF];
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void Referee_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY);
}
static void Referee_IdleLineCallback(void) {
HAL_UART_AbortReceive_IT(BSP_UART_GetHandle(BSP_UART_REF));
}
static void Referee_AbortRxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY);
}
static void RefereeFastRefreshTimerCallback(void *arg) {
(void)arg;
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_FAST_REFRESH_UI);
}
static void RefereeSlowRefreshTimerCallback(void *arg) {
(void)arg;
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_SLOW_REFRESH_UI);
}
/* Exported functions ------------------------------------------------------- */
int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui,
const CMD_Screen_t *screen) {
if (ref == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
ui->screen = screen;
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_RX_CPLT_CB,
Referee_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_ABORT_RX_CPLT_CB,
Referee_AbortRxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_IDLE_LINE_CB,
Referee_IdleLineCallback);
uint32_t fast_period_ms = (uint32_t)(1000.0f / REF_UI_FAST_REFRESH_FREQ);
uint32_t slow_period_ms = (uint32_t)(1000.0f / REF_UI_SLOW_REFRESH_FREQ);
ref->ui_fast_timer_id =
osTimerNew(RefereeFastRefreshTimerCallback, osTimerPeriodic, NULL, NULL);
ref->ui_slow_timer_id =
osTimerNew(RefereeSlowRefreshTimerCallback, osTimerPeriodic, NULL, NULL);
osTimerStart(ref->ui_fast_timer_id, fast_period_ms);
osTimerStart(ref->ui_slow_timer_id, slow_period_ms);
__HAL_UART_ENABLE_IT(BSP_UART_GetHandle(BSP_UART_REF), UART_IT_IDLE);
inited = true;
return 0;
}
int8_t Referee_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REF));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REF));
return 0;
}
int8_t Referee_StartReceiving(Referee_t *ref) {
(void)ref;
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_REF), rxbuf,
REF_LEN_RX_BUFF) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool Referee_CheckTXReady() {
return BSP_UART_GetHandle(BSP_UART_REF)->gState == HAL_UART_STATE_READY;
}
void Referee_HandleOffline(Referee_t *referee) {
referee->ref_status = REF_STATUS_OFFLINE;
}
int8_t Referee_Parse(Referee_t *ref) {
REF_SWITCH_STATUS(*ref, REF_STATUS_RUNNING);
uint32_t data_length =
REF_LEN_RX_BUFF -
__HAL_DMA_GET_COUNTER(BSP_UART_GetHandle(BSP_UART_REF)->hdmarx);
uint8_t index = 0;
uint8_t packet_shift;
uint8_t packet_length;
while (index < data_length && rxbuf[index] == REF_HEADER_SOF) {
packet_shift = index;
Referee_Header_t *header = (Referee_Header_t *)(rxbuf + index);
index += sizeof(*header);
if (index - packet_shift >= data_length) goto error;
if (!CRC8_Verify((uint8_t *)header, sizeof(*header))) goto error;
if (header->sof != REF_HEADER_SOF) goto error;
Referee_CMDID_t *cmd_id = (Referee_CMDID_t *)(rxbuf + index);
index += sizeof(*cmd_id);
if (index - packet_shift >= data_length) goto error;
void *target = (rxbuf + index);
void *origin;
size_t size;
switch (*cmd_id) {
case REF_CMD_ID_GAME_STATUS:
origin = &(ref->game_status);
size = sizeof(ref->game_status);
break;
case REF_CMD_ID_GAME_RESULT:
origin = &(ref->game_result);
size = sizeof(ref->game_result);
break;
case REF_CMD_ID_GAME_ROBOT_HP:
origin = &(ref->game_robot_hp);
size = sizeof(ref->game_robot_hp);
break;
case REF_CMD_ID_DART_STATUS:
origin = &(ref->dart_status);
size = sizeof(ref->dart_status);
break;
case REF_CMD_ID_ICRA_ZONE_STATUS:
origin = &(ref->icra_zone);
size = sizeof(ref->icra_zone);
break;
case REF_CMD_ID_FIELD_EVENTS:
origin = &(ref->field_event);
size = sizeof(ref->field_event);
break;
case REF_CMD_ID_SUPPLY_ACTION:
origin = &(ref->supply_action);
size = sizeof(ref->supply_action);
break;
case REF_CMD_ID_REQUEST_SUPPLY:
origin = &(ref->request_supply);
size = sizeof(ref->request_supply);
case REF_CMD_ID_WARNING:
origin = &(ref->warning);
size = sizeof(ref->warning);
break;
case REF_CMD_ID_DART_COUNTDOWN:
origin = &(ref->dart_countdown);
size = sizeof(ref->dart_countdown);
break;
case REF_CMD_ID_ROBOT_STATUS:
origin = &(ref->robot_status);
size = sizeof(ref->robot_status);
break;
case REF_CMD_ID_POWER_HEAT_DATA:
origin = &(ref->power_heat);
size = sizeof(ref->power_heat);
break;
case REF_CMD_ID_ROBOT_POS:
origin = &(ref->robot_pos);
size = sizeof(ref->robot_pos);
break;
case REF_CMD_ID_ROBOT_BUFF:
origin = &(ref->robot_buff);
size = sizeof(ref->robot_buff);
break;
case REF_CMD_ID_DRONE_ENERGY:
origin = &(ref->drone_energy);
size = sizeof(ref->drone_energy);
break;
case REF_CMD_ID_ROBOT_DMG:
origin = &(ref->robot_danage);
size = sizeof(ref->robot_danage);
break;
case REF_CMD_ID_SHOOT_DATA:
origin = &(ref->shoot_data);
size = sizeof(ref->shoot_data);
break;
case REF_CMD_ID_BULLET_REMAINING:
origin = &(ref->bullet_remain);
size = sizeof(ref->bullet_remain);
break;
case REF_CMD_ID_RFID:
origin = &(ref->rfid);
size = sizeof(ref->rfid);
break;
case REF_CMD_ID_DART_CLIENT:
origin = &(ref->dart_client);
size = sizeof(ref->dart_client);
break;
case REF_CMD_ID_INTER_STUDENT_CUSTOM:
origin = &(ref->custom);
size = sizeof(ref->custom);
case REF_CMD_ID_CLIENT_MAP:
origin = &(ref->client_map);
size = sizeof(ref->client_map);
case REF_CMD_ID_KEYBOARD_MOUSE:
origin = &(ref->keyboard_mouse);
size = sizeof(ref->keyboard_mouse);
default:
return DEVICE_ERR;
}
packet_length = sizeof(Referee_Header_t) + sizeof(Referee_CMDID_t) + size +
sizeof(Referee_Tail_t);
index += size;
if (index - packet_shift >= data_length) goto error;
index += sizeof(Referee_Tail_t);
if (index - packet_shift != packet_length) goto error;
if (CRC16_Verify((uint8_t *)(rxbuf + packet_shift), packet_length))
memcpy(origin, target, size);
else
goto error;
}
return DEVICE_OK;
error:
drop_message++;
return DEVICE_ERR;
}
int8_t Referee_StartSend(uint8_t *data, uint32_t len) {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_REF), data,
(size_t)len) == HAL_OK) {
return DEVICE_OK;
} else
return DEVICE_ERR;
}
int8_t Referee_MoveData(void *data, void *tmp, uint32_t len) {
if (len <= 0 || data == NULL || tmp == NULL) return DEVICE_ERR;
memcpy(tmp, (const void *)data, (size_t)len);
memset(data, 0, (size_t)len);
return DEVICE_OK;
}
int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
Referee_StudentCMDID_t cmd_id, uint8_t sender_id) {
header->data_cmd_id = cmd_id;
if (sender_id <= REF_BOT_RED_RADER) switch (sender_id) {
case REF_BOT_RED_HERO:
header->sender_ID = REF_BOT_RED_HERO;
header->receiver_ID = REF_CL_RED_HERO;
break;
case REF_BOT_RED_ENGINEER:
header->sender_ID = REF_BOT_RED_ENGINEER;
header->receiver_ID = REF_CL_RED_ENGINEER;
break;
case REF_BOT_RED_INFANTRY_1:
header->sender_ID = REF_BOT_RED_INFANTRY_1;
header->receiver_ID = REF_CL_RED_INFANTRY_1;
break;
case REF_BOT_RED_INFANTRY_2:
header->sender_ID = REF_BOT_RED_INFANTRY_2;
header->receiver_ID = REF_CL_RED_INFANTRY_2;
break;
case REF_BOT_RED_INFANTRY_3:
header->sender_ID = REF_BOT_RED_INFANTRY_3;
header->receiver_ID = REF_CL_RED_INFANTRY_3;
break;
case REF_BOT_RED_DRONE:
header->sender_ID = REF_BOT_RED_DRONE;
header->receiver_ID = REF_CL_RED_DRONE;
break;
case REF_BOT_RED_SENTRY:
header->sender_ID = REF_BOT_RED_SENTRY;
break;
case REF_BOT_RED_RADER:
header->sender_ID = REF_BOT_RED_RADER;
break;
default:
return -1;
}
else
switch (sender_id) {
case REF_BOT_BLU_HERO:
header->sender_ID = REF_BOT_BLU_HERO;
header->receiver_ID = REF_CL_BLU_HERO;
break;
case REF_BOT_BLU_ENGINEER:
header->sender_ID = REF_BOT_BLU_ENGINEER;
header->receiver_ID = REF_CL_BLU_ENGINEER;
break;
case REF_BOT_BLU_INFANTRY_1:
header->sender_ID = REF_BOT_BLU_INFANTRY_1;
header->receiver_ID = REF_CL_BLU_INFANTRY_1;
break;
case REF_BOT_BLU_INFANTRY_2:
header->sender_ID = REF_BOT_BLU_INFANTRY_2;
header->receiver_ID = REF_CL_BLU_INFANTRY_2;
break;
case REF_BOT_BLU_INFANTRY_3:
header->sender_ID = REF_BOT_BLU_INFANTRY_3;
header->receiver_ID = REF_CL_BLU_INFANTRY_3;
break;
case REF_BOT_BLU_DRONE:
header->sender_ID = REF_BOT_BLU_DRONE;
header->receiver_ID = REF_CL_BLU_DRONE;
break;
case REF_BOT_BLU_SENTRY:
header->sender_ID = REF_BOT_BLU_SENTRY;
break;
case REF_BOT_BLU_RADER:
header->sender_ID = REF_BOT_BLU_RADER;
break;
default:
return -1;
}
return 0;
}
int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref) {
if (!Referee_CheckTXReady()) return 0;
if (ui->character_counter == 0 && ui->grapic_counter == 0 &&
ui->del_counter == 0)
return 0;
static uint8_t send_data[sizeof(Referee_UI_Drawgrapic_7_t)] = {0};
uint16_t size;
if (ui->del_counter != 0) {
if (ui->del_counter < 0 || ui->del_counter > REF_UI_MAX_STRING_NUM)
return DEVICE_ERR;
Referee_UI_Del_t *address = (Referee_UI_Del_t *)send_data;
address->header.sof = REF_HEADER_SOF;
address->header.data_length =
sizeof(UI_Del_t) + sizeof(Referee_Interactive_Header_t);
address->header.crc8 =
CRC8_Calc((const uint8_t *)&(address->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_DEL,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->del[--ui->del_counter]), &(address->data),
sizeof(UI_Del_t));
address->crc16 =
CRC16_Calc((const uint8_t *)address,
sizeof(Referee_UI_Del_t) - sizeof(uint16_t), CRC16_INIT);
size = sizeof(Referee_UI_Del_t);
Referee_StartSend(send_data, size);
return DEVICE_OK;
} else if (ui->grapic_counter != 0) {
switch (ui->grapic_counter) {
case 1:
size = sizeof(Referee_UI_Drawgrapic_1_t);
Referee_UI_Drawgrapic_1_t *address_1 =
(Referee_UI_Drawgrapic_1_t *)send_data;
address_1->header.sof = REF_HEADER_SOF;
address_1->header.data_length =
sizeof(UI_Drawgrapic_1_t) + sizeof(Referee_Interactive_Header_t);
address_1->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_1->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_1->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_1->IA_header), REF_STDNT_CMD_ID_UI_DRAW1,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_1->data.grapic),
sizeof(UI_Drawgrapic_1_t));
address_1->crc16 = CRC16_Calc(
(const uint8_t *)address_1,
sizeof(Referee_UI_Drawgrapic_1_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 2:
size = sizeof(Referee_UI_Drawgrapic_2_t);
Referee_UI_Drawgrapic_2_t *address_2 =
(Referee_UI_Drawgrapic_2_t *)send_data;
address_2->header.sof = REF_HEADER_SOF;
address_2->header.data_length =
sizeof(UI_Drawgrapic_2_t) + sizeof(Referee_Interactive_Header_t);
address_2->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_2->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_2->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_2->IA_header), REF_STDNT_CMD_ID_UI_DRAW2,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_2->data.grapic),
sizeof(UI_Drawgrapic_2_t));
address_2->crc16 = CRC16_Calc(
(const uint8_t *)address_2,
sizeof(Referee_UI_Drawgrapic_2_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 3:
case 4:
case 5:
size = sizeof(Referee_UI_Drawgrapic_5_t);
Referee_UI_Drawgrapic_5_t *address_5 =
(Referee_UI_Drawgrapic_5_t *)send_data;
address_5->header.sof = REF_HEADER_SOF;
address_5->header.data_length =
sizeof(UI_Drawgrapic_5_t) + sizeof(Referee_Interactive_Header_t);
address_5->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_5->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_5->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_5->IA_header), REF_STDNT_CMD_ID_UI_DRAW5,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_5->data.grapic),
sizeof(UI_Drawgrapic_5_t));
address_5->crc16 = CRC16_Calc(
(const uint8_t *)address_5,
sizeof(Referee_UI_Drawgrapic_5_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 6:
case 7:
size = sizeof(Referee_UI_Drawgrapic_7_t);
Referee_UI_Drawgrapic_7_t *address_7 =
(Referee_UI_Drawgrapic_7_t *)send_data;
address_7->header.sof = REF_HEADER_SOF;
address_7->header.data_length =
sizeof(UI_Drawgrapic_7_t) + sizeof(Referee_Interactive_Header_t);
address_7->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_7->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_7->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_7->IA_header), REF_STDNT_CMD_ID_UI_DRAW7,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_7->data.grapic),
sizeof(UI_Drawgrapic_7_t));
address_7->crc16 = CRC16_Calc(
(const uint8_t *)address_7,
sizeof(Referee_UI_Drawgrapic_7_t) - sizeof(uint16_t), CRC16_INIT);
break;
default:
return DEVICE_ERR;
}
if (Referee_StartSend(send_data, size) == HAL_OK) {
ui->grapic_counter = 0;
return DEVICE_OK;
}
} else if (ui->character_counter != 0) {
if (ui->character_counter < 0 ||
ui->character_counter > REF_UI_MAX_STRING_NUM)
return DEVICE_ERR;
Referee_UI_Drawcharacter_t *address =
(Referee_UI_Drawcharacter_t *)send_data;
address->header.sof = REF_HEADER_SOF;
address->header.data_length =
sizeof(UI_Drawcharacter_t) + sizeof(Referee_Interactive_Header_t);
address->header.crc8 =
CRC8_Calc((const uint8_t *)&(address->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_STR,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->character_data[--ui->character_counter]),
&(address->data.grapic), sizeof(UI_Drawcharacter_t));
address->crc16 = CRC16_Calc(
(const uint8_t *)address,
sizeof(Referee_UI_Drawcharacter_t) - sizeof(uint16_t), CRC16_INIT);
size = sizeof(Referee_UI_Drawcharacter_t);
Referee_StartSend(send_data, size);
return DEVICE_OK;
}
return DEVICE_ERR_NULL;
}
UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui) {
if (ref_ui->grapic_counter >= REF_UI_MAX_GRAPIC_NUM ||
ref_ui->grapic_counter < 0)
return NULL;
else
return &(ref_ui->grapic[ref_ui->grapic_counter++]);
}
UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui) {
if (ref_ui->character_counter >= REF_UI_MAX_STRING_NUM ||
ref_ui->character_counter < 0)
return NULL;
else
return &(ref_ui->character_data[ref_ui->character_counter++]);
}
UI_Del_t *Referee_GetDelAdd(Referee_UI_t *ref_ui) {
if (ref_ui->del_counter >= REF_UI_MAX_DEL_NUM || ref_ui->del_counter < 0)
return NULL;
else
return &(ref_ui->del[ref_ui->del_counter++]);
}
uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, CMD_UI_t cmd) {
switch (cmd) {
/* Demo */
case CMD_UI_NOTHING:
/* 字符 */
UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "0",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM,
RED_BLUE, UI_DEFAULT_WIDTH, 100, 100, 200, 200, "Demo");
/* 直线 */
UI_DrawLine(Referee_GetGrapicAdd(ref_ui), "2", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 960, 540,
960, 240);
/* 圆形 */
UI_DrawCycle(Referee_GetGrapicAdd(ref_ui), "1", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 900,
500, 10);
/* 删除 */
UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_AUTOAIM);
break;
case CMD_UI_AUTO_AIM_START:
UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "1",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM,
RED_BLUE, UI_DEFAULT_WIDTH * 10, 50, UI_DEFAULT_WIDTH,
ref_ui->screen->width * 0.8,
ref_ui->screen->height * 0.5, "AUTO");
break;
case CMD_UI_AUTO_AIM_STOP:
UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_AUTOAIM);
default:
return -1;
}
return 0;
}
uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref) {
cap->chassis_power_limit = ref->robot_status.chassis_power_limit;
cap->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff;
cap->chassis_watt = ref->power_heat.chassis_watt;
cap->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackAI(Referee_ForAI_t *ai, const Referee_t *ref) {
ai->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis,
const Referee_t *ref) {
chassis->chassis_power_limit = ref->robot_status.chassis_power_limit;
chassis->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff;
chassis->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackShoot(Referee_ForShoot_t *shoot, Referee_t *ref) {
memcpy(&(shoot->power_heat), &(ref->power_heat), sizeof(shoot->power_heat));
memcpy(&(shoot->robot_status), &(ref->robot_status),
sizeof(shoot->robot_status));
memcpy(&(shoot->shoot_data), &(ref->shoot_data), sizeof(shoot->shoot_data));
shoot->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_UIRefresh(Referee_UI_t *ui) {
static uint8_t fsm = 0;
if (osThreadFlagsGet() & SIGNAL_REFEREE_FAST_REFRESH_UI) {
osThreadFlagsClear(SIGNAL_REFEREE_FAST_REFRESH_UI);
switch (fsm) {
case 0: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CHASSIS);
UI_DrawLine(Referee_GetGrapicAdd(ui), "6", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
ui->screen->width * 0.4, ui->screen->height * 0.2,
ui->screen->width * 0.4 + sin(ui->chassis_ui.angle) * 46,
ui->screen->height * 0.2 + cos(ui->chassis_ui.angle) * 46);
float start_pos_h = 0.0f;
switch (ui->chassis_ui.mode) {
case CHASSIS_MODE_FOLLOW_GIMBAL:
start_pos_h = 0.68f;
break;
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
start_pos_h = 0.66f;
break;
case CHASSIS_MODE_ROTOR:
start_pos_h = 0.64f;
break;
default:
break;
}
if (start_pos_h != 0.0f)
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "8", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS - 6,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 44,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 1:
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CAP);
switch (ui->cap_ui.status) {
case CAN_CAP_STATUS_OFFLINE:
UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CAP, YELLOW, 0, 360,
UI_DEFAULT_WIDTH * 5, ui->screen->width * 0.6,
ui->screen->height * 0.2, 50, 50);
break;
break;
case CAN_CAP_STATUS_RUNNING:
UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CAP, GREEN, 0,
ui->cap_ui.percentage * 360, UI_DEFAULT_WIDTH * 5,
ui->screen->width * 0.6, ui->screen->height * 0.2, 50,
50);
break;
}
break;
case 2: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_GIMBAL);
float start_pos_h = 0.0f;
switch (ui->gimbal_ui.mode) {
case GIMBAL_MODE_RELAX:
start_pos_h = 0.68f;
break;
case GIMBAL_MODE_RELATIVE:
start_pos_h = 0.66f;
break;
case GIMBAL_MODE_ABSOLUTE:
start_pos_h = 0.64f;
break;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "a", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_GIMBAL, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 54,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 102,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 3: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_SHOOT);
float start_pos_h = 0.0f;
switch (ui->shoot_ui.mode) {
case SHOOT_MODE_RELAX:
start_pos_h = 0.68f;
break;
case SHOOT_MODE_SAFE:
start_pos_h = 0.66f;
break;
case SHOOT_MODE_LOADED:
start_pos_h = 0.64f;
break;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "b", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 114,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 162,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
switch (ui->shoot_ui.fire) {
case FIRE_MODE_SINGLE:
start_pos_h = 0.68f;
break;
case FIRE_MODE_BURST:
start_pos_h = 0.66f;
break;
case FIRE_MODE_CONT:
start_pos_h = 0.64f;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "f", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 174,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 222,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 4:
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CMD);
if (ui->cmd_pc) {
UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN,
UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 96,
ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 120,
ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET);
} else {
UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN,
UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 56,
ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 80,
ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET);
}
break;
default:
fsm = 0;
if (ui->del_counter >= REF_UI_MAX_DEL_NUM ||
ui->character_counter > REF_UI_MAX_STRING_NUM ||
ui->grapic_counter > REF_UI_MAX_GRAPIC_NUM)
BSP_UART_GetHandle(BSP_UART_REF)->gState = HAL_UART_STATE_READY;
}
}
if (osThreadFlagsGet() & SIGNAL_REFEREE_SLOW_REFRESH_UI) {
osThreadFlagsClear(SIGNAL_REFEREE_SLOW_REFRESH_UI);
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CONST);
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "1", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.7, "CHAS GMBL SHOT FIRE");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "2", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.68, "FLLW RELX RELX SNGL");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "3", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.66, "FL35 ABSL SAFE BRST");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "4", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.64, "ROTR RLTV LOAD CONT");
UI_DrawLine(Referee_GetGrapicAdd(ui), "5", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 3,
ui->screen->width * 0.4, ui->screen->height * 0.2,
ui->screen->width * 0.4, ui->screen->height * 0.2 + 50);
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "d", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.4, "CTRL RC PC");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "e", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 20, 80,
UI_CHAR_DEFAULT_WIDTH * 2, ui->screen->width * 0.6 - 26,
ui->screen->height * 0.2 + 10, "CAP");
}
return 0;
}

507
User/device/referee.h Normal file
View File

@@ -0,0 +1,507 @@
/*
裁判系统抽象。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include "component\cmd.h"
#include "component\ui.h"
#include "component\user_math.h"
#include "device\can.h"
#include "device\device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
#define CHASSIS_POWER_MAX_WITHOUT_REF 2000.0f /* 裁判系统离线底盘最大功率 */
#define REF_UI_MAX_GRAPIC_NUM (7)
#define REF_UI_MAX_STRING_NUM (7)
#define REF_UI_MAX_DEL_NUM (3)
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint8_t sof;
uint16_t data_length;
uint8_t seq;
uint8_t crc8;
} Referee_Header_t;
typedef enum { REF_STATUS_OFFLINE = 0, REF_STATUS_RUNNING } Referee_Status_t;
typedef enum {
REF_CMD_ID_GAME_STATUS = 0x0001,
REF_CMD_ID_GAME_RESULT = 0x0002,
REF_CMD_ID_GAME_ROBOT_HP = 0x0003,
REF_CMD_ID_DART_STATUS = 0x0004,
REF_CMD_ID_ICRA_ZONE_STATUS = 0x0005,
REF_CMD_ID_FIELD_EVENTS = 0x0101,
REF_CMD_ID_SUPPLY_ACTION = 0x0102,
REF_CMD_ID_REQUEST_SUPPLY = 0x0103,
REF_CMD_ID_WARNING = 0x0104,
REF_CMD_ID_DART_COUNTDOWN = 0x0105,
REF_CMD_ID_ROBOT_STATUS = 0x0201,
REF_CMD_ID_POWER_HEAT_DATA = 0x0202,
REF_CMD_ID_ROBOT_POS = 0x0203,
REF_CMD_ID_ROBOT_BUFF = 0x0204,
REF_CMD_ID_DRONE_ENERGY = 0x0205,
REF_CMD_ID_ROBOT_DMG = 0x0206,
REF_CMD_ID_SHOOT_DATA = 0x0207,
REF_CMD_ID_BULLET_REMAINING = 0x0208,
REF_CMD_ID_RFID = 0x0209,
REF_CMD_ID_DART_CLIENT = 0x020A,
REF_CMD_ID_INTER_STUDENT = 0x0301,
REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302,
REF_CMD_ID_CLIENT_MAP = 0x0303,
REF_CMD_ID_KEYBOARD_MOUSE = 0x0304,
} Referee_CMDID_t;
typedef struct __packed {
uint8_t game_type : 4;
uint8_t game_progress : 4;
uint16_t stage_remain_time;
uint64_t sync_time_stamp;
} Referee_GameStatus_t;
typedef struct __packed {
uint8_t winner;
} Referee_GameResult_t;
typedef struct __packed {
uint16_t red_1;
uint16_t red_2;
uint16_t red_3;
uint16_t red_4;
uint16_t red_5;
uint16_t red_6;
uint16_t red_7;
uint16_t red_outpose;
uint16_t red_base;
uint16_t blue_1;
uint16_t blue_2;
uint16_t blue_3;
uint16_t blue_4;
uint16_t blue_5;
uint16_t blue_6;
uint16_t blue_7;
uint16_t blue_outpose;
uint16_t blue_base;
} Referee_GameRobotHP_t;
typedef struct __packed {
uint8_t dart_belong;
uint16_t stage_remain_time;
} Referee_DartStatus_t;
typedef struct __packed {
uint8_t f1_status : 1;
uint8_t f1_buff_status : 3;
uint8_t f2_status : 1;
uint8_t f2_buff_status : 3;
uint8_t f3_status : 1;
uint8_t f3_buff_status : 3;
uint8_t f4_status : 1;
uint8_t f4_buff_status : 3;
uint8_t f5_status : 1;
uint8_t f5_buff_status : 3;
uint8_t f6_status : 1;
uint8_t f6_buff_status : 3;
uint16_t red1_bullet_remain;
uint16_t red2_bullet_remain;
uint16_t blue1_bullet_remain;
uint16_t blue2_bullet_remain;
} Referee_ICRAZoneStatus_t;
typedef struct __packed {
uint8_t copter_pad : 2;
uint8_t energy_mech : 2;
uint8_t virtual_shield : 1;
uint32_t res : 27;
} Referee_FieldEvents_t;
typedef struct __packed {
uint8_t supply_id;
uint8_t robot_id;
uint8_t supply_step;
uint8_t supply_sum;
} Referee_SupplyAction_t;
typedef struct __packed {
uint8_t place_holder; /* TODO */
} Referee_RequestSupply_t;
typedef struct __packed {
uint8_t level;
uint8_t robot_id;
} Referee_Warning_t;
typedef struct __packed {
uint8_t countdown;
} Referee_DartCountdown_t;
// typedef struct __packed {
// uint8_t robot_id;
// uint8_t robot_level;
// uint16_t remain_hp;
// uint16_t max_hp;
// uint16_t shoot_id1_17_cooling_rate;
// uint16_t shoot_id1_17_heat_limit;
// uint16_t shoot_id1_17_speed_limit;
// uint16_t shoot_id2_17_cooling_rate;
// uint16_t shoot_id2_17_heat_limit;
// uint16_t shoot_id2_17_speed_limit;
// uint16_t shoot_42_cooling_rate;
// uint16_t shoot_42_heat_limit;
// uint16_t shoot_42_speed_limit;
// uint16_t chassis_power_limit;
// uint8_t power_gimbal_output : 1;
// uint8_t power_chassis_output : 1;
// uint8_t power_shoot_output : 1;
// } Referee_RobotStatus_t;
typedef struct __packed {
uint8_t robot_id;
uint8_t robot_level;
uint16_t remain_hp;
uint16_t max_hp;
uint16_t shooter_cooling_value;
uint16_t shooter_heat_limit;
uint16_t chassis_power_limit;
uint8_t power_gimbal_output : 1;
uint8_t power_chassis_output : 1;
uint8_t power_shoot_output : 1;
} Referee_RobotStatus_t; /* 0x0201 */
typedef struct __packed {
uint16_t chassis_volt;
uint16_t chassis_amp;
float chassis_watt;
uint16_t chassis_pwr_buff;
uint16_t shoot_id1_17_heat;
uint16_t shoot_id2_17_heat;
uint16_t shoot_42_heat;
} Referee_PowerHeat_t;
typedef struct __packed {
float x;
float y;
float z;
float yaw;
} Referee_RobotPos_t;
// typedef struct __packed {
// uint8_t healing : 1;
// uint8_t cooling_acc : 1;
// uint8_t defense_buff : 1;
// uint8_t attack_buff : 1;
// uint8_t res : 4;
// } Referee_RobotBuff_t;
typedef struct __packed {
uint8_t healing_buff;
uint8_t cooling_acc;
uint8_t defense_buff;
uint8_t vulnerability_buff;
uint16_t attack_buff;
} Referee_RobotBuff_t;
typedef struct __packed {
uint8_t attack_countdown;
} Referee_DroneEnergy_t;
typedef struct __packed {
uint8_t armor_id : 4;
uint8_t damage_type : 4;
} Referee_RobotDamage_t;
typedef struct __packed {
uint8_t bullet_type;
uint8_t shooter_id;
uint8_t bullet_freq;
float bullet_speed;
} Referee_ShootData_t;/* 0x0207 */
typedef struct __packed {
uint16_t bullet_17_remain;
uint16_t bullet_42_remain;
uint16_t coin_remain;
} Referee_BulletRemain_t;
typedef struct __packed {
uint8_t base : 1;
uint8_t high_ground : 1;
uint8_t energy_mech : 1;
uint8_t slope : 1;
uint8_t outpose : 1;
uint8_t resource : 1;
uint8_t healing_card : 1;
uint32_t res : 24;
} Referee_RFID_t;
typedef struct __packed {
uint8_t opening;
uint8_t target;
uint8_t target_changable_countdown;
uint8_t dart1_speed;
uint8_t dart2_speed;
uint8_t dart3_speed;
uint8_t dart4_speed;
uint16_t last_dart_launch_time;
uint16_t operator_cmd_launch_time;
} Referee_DartClient_t;
typedef struct __packed {
float position_x;
float position_y;
float position_z;
uint8_t commd_keyboard;
uint16_t robot_id;
} Referee_ClientMap_t;
typedef struct __packed {
int16_t mouse_x;
int16_t mouse_y;
int16_t mouse_wheel;
int8_t button_l;
int8_t button_r;
uint16_t keyboard_value;
uint16_t res;
} Referee_KeyboardMouse_t;
typedef uint16_t Referee_Tail_t;
typedef enum {
REF_BOT_RED_HERO = 1,
REF_BOT_RED_ENGINEER = 2,
REF_BOT_RED_INFANTRY_1 = 3,
REF_BOT_RED_INFANTRY_2 = 4,
REF_BOT_RED_INFANTRY_3 = 5,
REF_BOT_RED_DRONE = 6,
REF_BOT_RED_SENTRY = 7,
REF_BOT_RED_RADER = 9,
REF_BOT_BLU_HERO = 101,
REF_BOT_BLU_ENGINEER = 102,
REF_BOT_BLU_INFANTRY_1 = 103,
REF_BOT_BLU_INFANTRY_2 = 104,
REF_BOT_BLU_INFANTRY_3 = 105,
REF_BOT_BLU_DRONE = 106,
REF_BOT_BLU_SENTRY = 107,
REF_BOT_BLU_RADER = 109,
} Referee_RobotID_t;
typedef enum {
REF_CL_RED_HERO = 0x0101,
REF_CL_RED_ENGINEER = 0x0102,
REF_CL_RED_INFANTRY_1 = 0x0103,
REF_CL_RED_INFANTRY_2 = 0x0104,
REF_CL_RED_INFANTRY_3 = 0x0105,
REF_CL_RED_DRONE = 0x0106,
REF_CL_BLU_HERO = 0x0165,
REF_CL_BLU_ENGINEER = 0x0166,
REF_CL_BLU_INFANTRY_1 = 0x0167,
REF_CL_BLU_INFANTRY_2 = 0x0168,
REF_CL_BLU_INFANTRY_3 = 0x0169,
REF_CL_BLU_DRONE = 0x016A,
} Referee_ClientID_t;
typedef enum {
REF_STDNT_CMD_ID_UI_DEL = 0x0100,
REF_STDNT_CMD_ID_UI_DRAW1 = 0x0101,
REF_STDNT_CMD_ID_UI_DRAW2 = 0x0102,
REF_STDNT_CMD_ID_UI_DRAW5 = 0x0103,
REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104,
REF_STDNT_CMD_ID_UI_STR = 0x0110,
REF_STDNT_CMD_ID_CUSTOM = 0x0200,
} Referee_StudentCMDID_t;
typedef struct __packed {
Referee_StudentCMDID_t data_cmd_id;
uint16_t id_sender;
uint16_t id_receiver;
uint8_t *data;
} Referee_InterStudent_t;
typedef struct __packed {
uint8_t place_holder;
} Referee_InterStudent_Custom_t;
typedef struct {
Referee_Status_t ref_status;
Referee_GameStatus_t game_status;
Referee_GameResult_t game_result;
Referee_GameRobotHP_t game_robot_hp;
Referee_DartStatus_t dart_status;
Referee_ICRAZoneStatus_t icra_zone;
Referee_FieldEvents_t field_event;
Referee_SupplyAction_t supply_action;
Referee_RequestSupply_t request_supply;
Referee_Warning_t warning;
Referee_DartCountdown_t dart_countdown;
Referee_RobotStatus_t robot_status;
Referee_PowerHeat_t power_heat;
Referee_RobotPos_t robot_pos;
Referee_RobotBuff_t robot_buff;
Referee_DroneEnergy_t drone_energy;
Referee_RobotDamage_t robot_danage;
Referee_ShootData_t shoot_data;
Referee_BulletRemain_t bullet_remain;
Referee_RFID_t rfid;
Referee_DartClient_t dart_client;
Referee_InterStudent_Custom_t custom;
Referee_ClientMap_t client_map;
Referee_KeyboardMouse_t keyboard_mouse;
osTimerId_t ui_fast_timer_id;
osTimerId_t ui_slow_timer_id;
} Referee_t;
typedef struct {
CMD_ChassisMode_t mode;
float angle;
} Referee_ChassisUI_t;
typedef struct {
float percentage;
CAN_CapStatus_t status;
} Referee_CapUI_t;
typedef struct {
CMD_GimbalMode_t mode;
} Referee_GimbalUI_t;
typedef struct {
CMD_ShootMode_t mode;
CMD_FireMode_t fire;
} Referee_ShootUI_t;
typedef struct __packed {
/* UI缓冲数据 */
UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM];
UI_Drawcharacter_t character_data[REF_UI_MAX_STRING_NUM];
UI_Del_t del[REF_UI_MAX_DEL_NUM];
/* 待发送数量 */
uint8_t grapic_counter;
uint8_t character_counter;
uint8_t del_counter;
/* UI所需信息 */
Referee_CapUI_t cap_ui;
Referee_ChassisUI_t chassis_ui;
Referee_ShootUI_t shoot_ui;
Referee_GimbalUI_t gimbal_ui;
bool cmd_pc;
/* 屏幕分辨率 */
const CMD_Screen_t *screen;
} Referee_UI_t;
typedef struct __packed {
uint16_t data_cmd_id;
uint16_t sender_ID;
uint16_t receiver_ID;
} Referee_Interactive_Header_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_1_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_1_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_2_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_2_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_5_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_5_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_7_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_7_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawcharacter_t data;
uint16_t crc16;
} Referee_UI_Drawcharacter_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Del_t data;
uint16_t crc16;
} Referee_UI_Del_t;
typedef struct {
Referee_Status_t ref_status;
float chassis_watt;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForCap_t;
typedef struct {
Referee_Status_t ref_status;
} Referee_ForAI_t;
typedef struct {
Referee_Status_t ref_status;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForChassis_t;
typedef struct {
Referee_Status_t ref_status;
Referee_PowerHeat_t power_heat;
Referee_RobotStatus_t robot_status;
Referee_ShootData_t shoot_data;
} Referee_ForShoot_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui,
const CMD_Screen_t *screen);
int8_t Referee_Restart(void);
int8_t Referee_StartReceiving(Referee_t *ref);
int8_t Referee_Parse(Referee_t *ref);
void Referee_HandleOffline(Referee_t *referee);
int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
Referee_StudentCMDID_t cmd_id, uint8_t sender_id);
int8_t Referee_StartSend(uint8_t *data, uint32_t len);
int8_t Referee_MoveData(void *data, void *tmp, uint32_t len);
int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref);
UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui);
UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui);
uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, CMD_UI_t cmd);
uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref);
uint8_t Referee_PackShoot(Referee_ForShoot_t *ai, Referee_t *ref);
uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis,
const Referee_t *ref);
uint8_t Referee_PackAI(Referee_ForAI_t *shoot, const Referee_t *ref);
uint8_t Referee_UIRefresh(Referee_UI_t *ui);
#ifdef __cplusplus
}
#endif

55
User/module/cap.c Normal file
View File

@@ -0,0 +1,55 @@
/*
* 电容模组
*/
/* Includes ----------------------------------------------------------------- */
#include "cap.h"
#include "component\capacity.h"
#include "component\limiter.h"
#include "device\can.h"
#include "device\referee.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define CAP_CUTOFF_VOLT 12.0f
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/**
* @brief 运行电容控制逻辑
*
* @param cap 电容数据结构体
* @param referee 裁判系统数据
* @param cap_out 电容输出结构体
*/
void Cap_Control(CAN_Capacitor_t *cap, const Referee_ForCap_t *referee,
CAN_CapOutput_t *cap_out) {
if (referee->ref_status != REF_STATUS_RUNNING) {
/* 当裁判系统离线时，依然使用裁判系统进程传来的数据 */
cap_out->power_limit = referee->chassis_power_limit;
} else {
/* 当裁判系统在线时，使用算法控制裁判系统输出（即电容输入） */
cap_out->power_limit =
PowerLimit_CapInput(referee->chassis_watt, referee->chassis_power_limit,
referee->chassis_pwr_buff);
}
/* 更新电容状态和百分比 */
cap->cap_status = CAN_CAP_STATUS_RUNNING;
cap->percentage = Capacity_GetCapacitorRemain(cap->cap_feedback.cap_volt,
cap->cap_feedback.input_volt,
CAP_CUTOFF_VOLT);
}
/**
* @brief 导出电容数据
*
* @param cap 电容数据
* @param ui 结构体
*/
void Cap_DumpUI(const CAN_Capacitor_t *cap, Referee_CapUI_t *ui) {
ui->percentage = cap->percentage;
ui->status = cap->cap_status;
}

Some files were not shown because too many files have changed in this diff Show More