From dcb33f8a99c32ab3dadcd86226ea3bed55841a61 Mon Sep 17 00:00:00 2001 From: robinson <18656133516@qq.com> Date: Sat, 18 Apr 2026 11:02:40 +0800 Subject: [PATCH] [add] add app level code --- .claude/settings.local.json | 3 +- CMakeLists.txt | 15 +- Core/Src/freertos.c | 17 +- STM32F407XX_FLASH.ld | 10 +- app/app.c | 93 +++- app/app.h | 8 + bsp/stm32f4/can/bsp_can.c | 31 +- bsp/stm32f4/can/bsp_can.h | 17 +- bsp/stm32f4/i2c/bsp_i2c.c | 18 + bsp/stm32f4/i2c/bsp_i2c.h | 14 + bsp/stm32f4/led/bsp_led.c | 5 +- hal/stm32f4/can/hal_can.c | 122 +++++ hal/stm32f4/can/hal_can.h | 18 + hal/stm32f4/can/hal_can_stm32f4.c | 60 --- hal/stm32f4/can/hal_can_stm32f4.h | 14 - hal/stm32f4/i2c/hal_i2c.c | 67 +++ hal/stm32f4/i2c/hal_i2c.h | 14 + .../led/{hal_led_stm32f4.c => hal_led.c} | 0 hal/stm32f4/led/hal_led.h | 5 + interfaces/can/can_if.c | 16 +- interfaces/can/can_if.h | 16 +- interfaces/i2c/i2c_if.c | 43 ++ interfaces/i2c/i2c_if.h | 31 ++ modules/control/pid/pid.c | 55 ++ modules/control/pid/pid.h | 25 + modules/device/can/can_device.c | 48 ++ modules/device/can/can_device.h | 18 + modules/device/motor/mf4010v2.c | 99 +++- modules/device/motor/mf4010v2.h | 37 +- modules/sensor/imu/j901p.c | 493 ------------------ modules/sensor/imu/j901p_hal.c | 16 +- modules/sensor/imu/j901p_module.c | 112 ++++ modules/sensor/imu/j901p_module.h | 36 ++ prompt.md | 367 +++++++++++++ 34 files changed, 1265 insertions(+), 678 deletions(-) create mode 100644 app/app.h create mode 100644 bsp/stm32f4/i2c/bsp_i2c.c create mode 100644 bsp/stm32f4/i2c/bsp_i2c.h create mode 100644 hal/stm32f4/can/hal_can.c create mode 100644 hal/stm32f4/can/hal_can.h delete mode 100644 hal/stm32f4/can/hal_can_stm32f4.c delete mode 100644 hal/stm32f4/can/hal_can_stm32f4.h create mode 100644 hal/stm32f4/i2c/hal_i2c.c create mode 100644 hal/stm32f4/i2c/hal_i2c.h rename hal/stm32f4/led/{hal_led_stm32f4.c => hal_led.c} (100%) create mode 100644 hal/stm32f4/led/hal_led.h create mode 100644 interfaces/i2c/i2c_if.c create mode 100644 interfaces/i2c/i2c_if.h create mode 100644 modules/control/pid/pid.c create mode 100644 modules/control/pid/pid.h create mode 100644 modules/device/can/can_device.c create mode 100644 modules/device/can/can_device.h delete mode 100644 modules/sensor/imu/j901p.c create mode 100644 modules/sensor/imu/j901p_module.c create mode 100644 modules/sensor/imu/j901p_module.h create mode 100644 prompt.md diff --git a/.claude/settings.local.json b/.claude/settings.local.json index 5ac6e3d..9af76e0 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -3,7 +3,8 @@ "allow": [ "Bash(cmake --build build)", "Bash(cmake -G \"Ninja\" -DCMAKE_TOOLCHAIN_FILE=../../cmake/gcc-arm-none-eabi.cmake ../..)", - "Bash(ninja)" + "Bash(ninja)", + "Bash(ninja clean *)" ] } } diff --git a/CMakeLists.txt b/CMakeLists.txt index e689f80..68ba1d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,15 +50,21 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # Add user sources here modules/sensor/imu/j901p.c modules/sensor/imu/j901p_hal.c + modules/sensor/imu/j901p_module.c modules/device/motor/mf4010v2.c modules/device/led/led.c + modules/device/can/can_device.c + modules/control/pid/pid.c bsp/stm32f4/can/bsp_can.c bsp/stm32f4/led/bsp_led.c - hal/stm32f4/led/hal_led_stm32f4.c - hal/stm32f4/can/hal_can_stm32f4.c + bsp/stm32f4/i2c/bsp_i2c.c + hal/stm32f4/can/hal_can.c + hal/stm32f4/led/hal_led.c + hal/stm32f4/i2c/hal_i2c.c app/app.c interfaces/led/led_if.c interfaces/can/can_if.c + interfaces/i2c/i2c_if.c ) @@ -67,14 +73,19 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE # Add user defined include paths ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/led ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/motor + ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/can ${CMAKE_CURRENT_SOURCE_DIR}/modules/sensor/imu + ${CMAKE_CURRENT_SOURCE_DIR}/modules/control/pid ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/can ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/led + ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/i2c ${CMAKE_CURRENT_SOURCE_DIR}/app ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/led ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/can + ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/i2c ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/led ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/can + ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/i2c ) # Add project symbols (macros) diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index efe3fec..ac2b52b 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -25,14 +25,7 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ -#include -#include -#include -#include "usart.h" -#include "can.h" -#include "j901p.h" -#include "j901p_hal.h" -#include "mf4010v2.h" +#include "app.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -94,7 +87,7 @@ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ */ void MX_FREERTOS_Init(void) { /* USER CODE BEGIN Init */ - + app_init(); /* USER CODE END Init */ /* USER CODE BEGIN RTOS_MUTEX */ @@ -143,7 +136,7 @@ void MX_FREERTOS_Init(void) { void StartMonitorTask(void *argument) { /* USER CODE BEGIN StartMonitorTask */ - + monitor_task(); /* USER CODE END StartMonitorTask */ } @@ -163,7 +156,7 @@ void StartMonitorTask(void *argument) void StartControlTask(void *argument) { /* USER CODE BEGIN StartControlTask */ - + control_task(); /* USER CODE END StartControlTask */ } @@ -177,7 +170,7 @@ void StartControlTask(void *argument) void StartSensorTask(void *argument) { /* USER CODE BEGIN StartSensorTask */ - + sensor_task(); /* USER CODE END StartSensorTask */ } diff --git a/STM32F407XX_FLASH.ld b/STM32F407XX_FLASH.ld index 14c9dbe..42fdc51 100644 --- a/STM32F407XX_FLASH.ld +++ b/STM32F407XX_FLASH.ld @@ -103,14 +103,14 @@ SECTIONS . = ALIGN(4); } >FLASH - .ARM.extab (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + .ARM.extab : { . = ALIGN(4); *(.ARM.extab* .gnu.linkonce.armextab.*) . = ALIGN(4); } >FLASH - .ARM (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + .ARM : { . = ALIGN(4); __exidx_start = .; @@ -119,7 +119,7 @@ SECTIONS . = ALIGN(4); } >FLASH - .preinit_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + .preinit_array : { . = ALIGN(4); PROVIDE_HIDDEN (__preinit_array_start = .); @@ -128,7 +128,7 @@ SECTIONS . = ALIGN(4); } >FLASH - .init_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + .init_array : { . = ALIGN(4); PROVIDE_HIDDEN (__init_array_start = .); @@ -138,7 +138,7 @@ SECTIONS . = ALIGN(4); } >FLASH - .fini_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + .fini_array : { . = ALIGN(4); PROVIDE_HIDDEN (__fini_array_start = .); diff --git a/app/app.c b/app/app.c index 01c17f8..47fbfb4 100644 --- a/app/app.c +++ b/app/app.c @@ -1,13 +1,102 @@ #include "led.h" +#include "mf4010v2.h" +#include "j901p_module.h" +#include "hal_i2c.h" +#include "hal_can.h" +#include "freertos.h" +#include "pid.h" +#include +#include +#include "cmsis_os.h" static led_t led0; +static mf4010v2_t motor1; // Pitch 轴电机 +static mf4010v2_t motor2; // Roll 轴电机 +static PIDController_t pid_pitch; +static PIDController_t pid_roll; + +// 云台自稳 PID 参数 (需要根据实际机械结构调优) +// 比例增益：角度误差对应的速度指令 +#define PITCH_Kp 50.0f // Pitch 轴比例增益 +#define PITCH_Ki 0.5f // Pitch 轴积分增益 +#define PITCH_Kd 10.0f // Pitch 轴微分增益 + +#define ROLL_Kp 50.0f // Roll 轴比例增益 +#define ROLL_Ki 0.5f // Roll 轴积分增益 +#define ROLL_Kd 10.0f // Roll 轴微分增益 + +// 最大速度限制 (0.01 DPS) +#define MAX_SPEED 30000 // 300 DPS 最大速度 void app_init(void) { + // 初始化硬件抽象层 + hal_i2c_init_all(); + hal_can_init(0); + + // 初始化外设 led_init(&led0, 0); + mf4010v2_init(&motor1, 0x141, 0); // Pitch 轴电机 ID: 0x141, CAN1 + mf4010v2_init(&motor2, 0x142, 0); // Roll 轴电机 ID: 0x142, CAN1 + + // 初始化 IMU + j901p_module_init(&g_imu, 1, 0x50); + + // 初始化 PID 控制器 + PIDController_Init(&pid_pitch); + pid_pitch.Kp = PITCH_Kp; + pid_pitch.Ki = PITCH_Ki; + pid_pitch.Kd = PITCH_Kd; + pid_pitch.output_max = MAX_SPEED; + + PIDController_Init(&pid_roll); + pid_roll.Kp = ROLL_Kp; + pid_roll.Ki = ROLL_Ki; + pid_roll.Kd = ROLL_Kd; + pid_roll.output_max = MAX_SPEED; + + // 启动电机 + mf4010v2_run(&motor1); + mf4010v2_run(&motor2); } -void task_1_app_run(void) +void sensor_task(void) { - led_toggle(&led0); + while (1) { + j901p_module_read(&g_imu); + vTaskDelay(pdMS_TO_TICKS(10)); // 100Hz 更新率 + } +} + +void control_task(void) +{ + float target_pitch = 0.0f; // 目标 Pitch 角 = 0 度（水平） + float target_roll = 0.0f; // 目标 Roll 角 = 0 度（水平） + while (1) { + // 云台自稳控制：目标是使 Pitch 和 Roll 保持 0 度（水平） + + + // 从 IMU 读取当前角度 (0.01 度单位) + float current_pitch = g_imu.angle[1] / 100.0f; // Pitch = angle[1] + float current_roll = g_imu.angle[0] / 100.0f; // Roll = angle[0] + + // 计算 PID 输出 (速度指令，0.01 DPS 单位) + float pitch_speed = PIDController_Compute(&pid_pitch, target_pitch, current_pitch); + float roll_speed = PIDController_Compute(&pid_roll, target_roll, current_roll); + + // 发送速度指令给电机 + mf4010v2_set_speed(&motor1, (int32_t)pitch_speed); + mf4010v2_set_speed(&motor2, (int32_t)roll_speed); + + vTaskDelay(pdMS_TO_TICKS(10)); // 100Hz 控制循环 + } +} + +void monitor_task(void) +{ + while (1) { + // 简单的监控任务：每秒打印一次当前状态 + printf("Pitch: %.2f deg, Roll: %.2f deg\n", g_imu.angle[1] / 100.0f, g_imu.angle[0] / 100.0f); + vTaskDelay(pdMS_TO_TICKS(1000)); // 1Hz 更新率 + } } \ No newline at end of file diff --git a/app/app.h b/app/app.h new file mode 100644 index 0000000..7c432ba --- /dev/null +++ b/app/app.h @@ -0,0 +1,8 @@ +#ifndef __APP_H__ +#define __APP_H__ + +void app_init(void); +void sensor_task(void); +void control_task(void); +void monitor_task(void); +#endif diff --git a/bsp/stm32f4/can/bsp_can.c b/bsp/stm32f4/can/bsp_can.c index b201f2b..b0fa42c 100644 --- a/bsp/stm32f4/can/bsp_can.c +++ b/bsp/stm32f4/can/bsp_can.c @@ -1,26 +1,15 @@ #include "bsp_can.h" #include "can.h" -void bsp_can_init(void) +// CAN 通道句柄数组 +void *g_bsp_can_handle[] = { + &hcan1, + &hcan2 +}; + +void* bsp_can_get_handle(int ch) { - // CAN GPIO 和时钟已在 MX_CAN2_Init 中配置 - // 这里可以添加额外的初始化 + if (ch < 0 || ch >= 2) + return 0; + return g_bsp_can_handle[ch]; } - -int bsp_can_send(uint32_t id, uint8_t* data, uint8_t length) -{ - if (!data || length > 8) - return -1; - - HAL_StatusTypeDef ret = CAN_SendFrame(&hcan2, id, data, length, 100); - return (ret == HAL_OK) ? 0 : -1; -} - -int bsp_can_recv(uint32_t *id, uint8_t* data, uint8_t *length) -{ - if (!id || !data || !length) - return -1; - - HAL_StatusTypeDef ret = CAN_ReceiveFrame(&hcan2, (uint16_t*)id, data, length, 0); - return (ret == HAL_OK) ? 0 : -1; -} \ No newline at end of file diff --git a/bsp/stm32f4/can/bsp_can.h b/bsp/stm32f4/can/bsp_can.h index 5b1834e..c66b8bc 100644 --- a/bsp/stm32f4/can/bsp_can.h +++ b/bsp/stm32f4/can/bsp_can.h @@ -1,24 +1,17 @@ #ifndef __BSP_CAN__H__ #define __BSP_CAN__H__ -#include -#include "main.h" - #ifdef __cplusplus extern "C" { #endif -typedef struct { - uint32_t id; // CAN ID - uint8_t data[8]; // CAN 数据 - uint8_t length; // 数据长度 (0-8) -} can_message_t; +// CAN 通道句柄引用 +extern void *g_bsp_can_handle[]; -void bsp_can_init(void); -int bsp_can_send(uint32_t id, uint8_t* data, uint8_t length); -int bsp_can_recv(uint32_t *id, uint8_t* data, uint8_t *length); +// 获取 CAN 句柄 +void* bsp_can_get_handle(int ch); #ifdef __cplusplus } #endif -#endif /* __BSP_CAN__H__ */ \ No newline at end of file +#endif diff --git a/bsp/stm32f4/i2c/bsp_i2c.c b/bsp/stm32f4/i2c/bsp_i2c.c new file mode 100644 index 0000000..3dfe098 --- /dev/null +++ b/bsp/stm32f4/i2c/bsp_i2c.c @@ -0,0 +1,18 @@ +#include "bsp_i2c.h" +#include "i2c.h" + +static void *g_bsp_i2c_handle[] = { +#ifdef I2C1_HANDLE + &hi2c1, +#else + 0, +#endif + &hi2c2 +}; + +void* bsp_i2c_get_handle(int ch) +{ + if (ch < 0 || ch >= 2) + return 0; + return g_bsp_i2c_handle[ch]; +} diff --git a/bsp/stm32f4/i2c/bsp_i2c.h b/bsp/stm32f4/i2c/bsp_i2c.h new file mode 100644 index 0000000..1f82729 --- /dev/null +++ b/bsp/stm32f4/i2c/bsp_i2c.h @@ -0,0 +1,14 @@ +#ifndef __BSP_I2C_H__ +#define __BSP_I2C_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +void* bsp_i2c_get_handle(int ch); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/bsp/stm32f4/led/bsp_led.c b/bsp/stm32f4/led/bsp_led.c index 4b39fea..b6de479 100644 --- a/bsp/stm32f4/led/bsp_led.c +++ b/bsp/stm32f4/led/bsp_led.c @@ -1,8 +1,9 @@ #include "bsp_led.h" static bsp_led_t leds[] = { - {GPIOA, GPIO_PIN_5}, // LED0 - {GPIOB, GPIO_PIN_0}, // LED1 + {GPIOE, GPIO_PIN_3}, // LED0 + {GPIOE, GPIO_PIN_4}, // LED1 + {GPIOG, GPIO_PIN_9}, // LED2 }; bsp_led_t* bsp_led_get(int id) diff --git a/hal/stm32f4/can/hal_can.c b/hal/stm32f4/can/hal_can.c new file mode 100644 index 0000000..c4c5a80 --- /dev/null +++ b/hal/stm32f4/can/hal_can.c @@ -0,0 +1,122 @@ +#include "can_if.h" +#include "bsp_can.h" +#include "can.h" + +static int do_hal_can_init(int ch) +{ + CAN_HandleTypeDef *hcan = (CAN_HandleTypeDef*)bsp_can_get_handle(ch); + if (!hcan) return -1; + + // 配置滤波器：接收所有帧（掩码模式） + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterBank = 0; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_16BIT; + sFilterConfig.FilterIdHigh = 0x0000; + sFilterConfig.FilterIdLow = 0x0000; + sFilterConfig.FilterMaskIdHigh = 0x0000; + sFilterConfig.FilterMaskIdLow = 0x0000; + sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; + sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; + sFilterConfig.SlaveStartFilterBank = 14; + + if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) + return -1; + + // 启动 CAN + if (HAL_CAN_Start(hcan) != HAL_OK) + return -1; + + return 0; +} + +static int hal_can_send(int ch, const can_message_t *msg) +{ + if (!msg || msg->length > 8) + return -1; + + CAN_HandleTypeDef *hcan = (CAN_HandleTypeDef*)bsp_can_get_handle(ch); + if (!hcan) return -1; + + CAN_TxHeaderTypeDef TxHeader; + TxHeader.StdId = msg->id & 0x7FF; + TxHeader.IDE = CAN_ID_STD; + TxHeader.RTR = CAN_RTR_DATA; + TxHeader.DLC = msg->length; + TxHeader.TransmitGlobalTime = DISABLE; + + uint32_t TxMailbox; + if (HAL_CAN_AddTxMessage(hcan, &TxHeader, (uint8_t*)msg->data, &TxMailbox) != HAL_OK) + return -1; + + // 等待发送完成 + while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) + ; + + return 0; +} + +static int hal_can_recv(int ch, can_message_t *msg) +{ + if (!msg) + return -1; + + CAN_HandleTypeDef *hcan = (CAN_HandleTypeDef*)bsp_can_get_handle(ch); + if (!hcan) return -1; + + // 检查是否有消息 + if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) == 0) + return -1; + + CAN_RxHeaderTypeDef RxHeader; + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, msg->data) != HAL_OK) + return -1; + + msg->id = RxHeader.StdId; + msg->length = RxHeader.DLC; + + return 0; +} + +static int hal_can_filter_config(int ch, uint8_t filter_id, uint32_t id, uint32_t mask) +{ + CAN_HandleTypeDef *hcan = (CAN_HandleTypeDef*)bsp_can_get_handle(ch); + if (!hcan) return -1; + + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterBank = filter_id; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_16BIT; + sFilterConfig.FilterIdHigh = (id >> 16) & 0xFFFF; + sFilterConfig.FilterIdLow = id & 0xFFFF; + sFilterConfig.FilterMaskIdHigh = (mask >> 16) & 0xFFFF; + sFilterConfig.FilterMaskIdLow = mask & 0xFFFF; + sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; + sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; + sFilterConfig.SlaveStartFilterBank = 14; + + if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) + return -1; + + return 0; +} + +// 注册接口 +static const can_ops_t ops = { + .init = do_hal_can_init, + .send = hal_can_send, + .recv = hal_can_recv, + .filter_config = hal_can_filter_config +}; + +void hal_can_init_all(void) +{ + can_register_ops(&ops); +} + +int hal_can_init(int ch) +{ + (void)ch; // 暂时不使用参数 + // 初始化指定通道 - 已经在 hal_can_init_all 中注册 + return 0; +} diff --git a/hal/stm32f4/can/hal_can.h b/hal/stm32f4/can/hal_can.h new file mode 100644 index 0000000..32a42ac --- /dev/null +++ b/hal/stm32f4/can/hal_can.h @@ -0,0 +1,18 @@ +#ifndef HAL_CAN_H +#define HAL_CAN_H + +#ifdef __cplusplus +extern "C" { +#endif + +// 初始化所有 CAN 通道（注册 ops 到接口层） +void hal_can_init_all(void); + +// 初始化指定 CAN 通道 +int hal_can_init(int ch); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/hal/stm32f4/can/hal_can_stm32f4.c b/hal/stm32f4/can/hal_can_stm32f4.c deleted file mode 100644 index bcbc353..0000000 --- a/hal/stm32f4/can/hal_can_stm32f4.c +++ /dev/null @@ -1,60 +0,0 @@ -#include "can_if.h" -#include "bsp_can.h" -#include "can.h" - -static int hal_can_init(void) -{ - // 配置滤波器：接收所有帧 - CAN_ConfigFilterAll(&hcan2, 0); - - // 启动 CAN - if (CAN_Start(&hcan2) != HAL_OK) - return -1; - - return 0; -} - -static int hal_can_send(const can_message_t *msg) -{ - if (!msg || msg->length > 8) - return -1; - - HAL_StatusTypeDef ret = CAN_SendFrame(&hcan2, msg->id, msg->data, msg->length, 100); - return (ret == HAL_OK) ? 0 : -1; -} - -static int hal_can_recv(can_message_t *msg) -{ - if (!msg) - return -1; - - uint16_t id; - uint8_t len; - - HAL_StatusTypeDef ret = CAN_ReceiveFrame(&hcan2, &id, msg->data, &len, 0); - if (ret != HAL_OK) - return -1; - - msg->id = id; - msg->length = len; - return 0; -} - -static int hal_can_filter_config(uint8_t filter_id, uint32_t id, uint32_t mask) -{ - CAN_ConfigFilter(&hcan2, filter_id, id, mask); - return 0; -} - -// 注册接口 -static const can_ops_t ops = { - .init = hal_can_init, - .send = hal_can_send, - .recv = hal_can_recv, - .filter_config = hal_can_filter_config -}; - -void hal_can_init_all(void) -{ - can_register_ops(&ops); -} diff --git a/hal/stm32f4/can/hal_can_stm32f4.h b/hal/stm32f4/can/hal_can_stm32f4.h deleted file mode 100644 index 0fa1ced..0000000 --- a/hal/stm32f4/can/hal_can_stm32f4.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef HAL_CAN_STM32F4_H -#define HAL_CAN_STM32F4_H - -#ifdef __cplusplus -extern "C" { -#endif - -void hal_can_init_all(void); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/hal/stm32f4/i2c/hal_i2c.c b/hal/stm32f4/i2c/hal_i2c.c new file mode 100644 index 0000000..c48b288 --- /dev/null +++ b/hal/stm32f4/i2c/hal_i2c.c @@ -0,0 +1,67 @@ +#include "i2c_if.h" +#include "bsp_i2c.h" +#include "i2c.h" + +static int hal_i2c_init(int ch) +{ + I2C_HandleTypeDef *hi2c = (I2C_HandleTypeDef*)bsp_i2c_get_handle(ch); + if (!hi2c) return -1; + return 0; +} + +static int hal_i2c_mem_write(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout) +{ + I2C_HandleTypeDef *hi2c = (I2C_HandleTypeDef*)bsp_i2c_get_handle(ch); + if (!hi2c || !data) return -1; + + if (HAL_I2C_Mem_Write(hi2c, dev_addr, mem_addr, mem_add_size, (uint8_t*)data, size, timeout) != HAL_OK) + return -1; + + return 0; +} + +static int hal_i2c_mem_read(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout) +{ + I2C_HandleTypeDef *hi2c = (I2C_HandleTypeDef*)bsp_i2c_get_handle(ch); + if (!hi2c || !data) return -1; + + if (HAL_I2C_Mem_Read(hi2c, dev_addr, mem_addr, mem_add_size, data, size, timeout) != HAL_OK) + return -1; + + return 0; +} + +static int hal_i2c_write(int ch, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout) +{ + I2C_HandleTypeDef *hi2c = (I2C_HandleTypeDef*)bsp_i2c_get_handle(ch); + if (!hi2c || !data) return -1; + + if (HAL_I2C_Master_Transmit(hi2c, dev_addr, (uint8_t*)data, size, timeout) != HAL_OK) + return -1; + + return 0; +} + +static int hal_i2c_read(int ch, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout) +{ + I2C_HandleTypeDef *hi2c = (I2C_HandleTypeDef*)bsp_i2c_get_handle(ch); + if (!hi2c || !data) return -1; + + if (HAL_I2C_Master_Receive(hi2c, dev_addr, data, size, timeout) != HAL_OK) + return -1; + + return 0; +} + +static const i2c_ops_t ops = { + .init = hal_i2c_init, + .mem_write = hal_i2c_mem_write, + .mem_read = hal_i2c_mem_read, + .write = hal_i2c_write, + .read = hal_i2c_read +}; + +void hal_i2c_init_all(void) +{ + i2c_register_ops(&ops); +} diff --git a/hal/stm32f4/i2c/hal_i2c.h b/hal/stm32f4/i2c/hal_i2c.h new file mode 100644 index 0000000..d85286e --- /dev/null +++ b/hal/stm32f4/i2c/hal_i2c.h @@ -0,0 +1,14 @@ +#ifndef __HAL_I2C_H__ +#define __HAL_I2C_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +void hal_i2c_init_all(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/hal/stm32f4/led/hal_led_stm32f4.c b/hal/stm32f4/led/hal_led.c similarity index 100% rename from hal/stm32f4/led/hal_led_stm32f4.c rename to hal/stm32f4/led/hal_led.c diff --git a/hal/stm32f4/led/hal_led.h b/hal/stm32f4/led/hal_led.h new file mode 100644 index 0000000..7ad44e7 --- /dev/null +++ b/hal/stm32f4/led/hal_led.h @@ -0,0 +1,5 @@ +#ifndef HAL_LED_H +#define HAL_LED_H + +void hal_led_init_all(void); +#endif diff --git a/interfaces/can/can_if.c b/interfaces/can/can_if.c index fc72b66..9fcc855 100644 --- a/interfaces/can/can_if.c +++ b/interfaces/can/can_if.c @@ -7,30 +7,30 @@ void can_register_ops(const can_ops_t *ops) g_can_ops = ops; } -int can_if_init(void) +int can_if_init(int ch) { if (!g_can_ops || !g_can_ops->init) return -1; - return g_can_ops->init(); + return g_can_ops->init(ch); } -int can_if_send(const can_message_t *msg) +int can_if_send(int ch, const can_message_t *msg) { if (!g_can_ops || !g_can_ops->send) return -1; - return g_can_ops->send(msg); + return g_can_ops->send(ch, msg); } -int can_if_recv(can_message_t *msg) +int can_if_recv(int ch, can_message_t *msg) { if (!g_can_ops || !g_can_ops->recv) return -1; - return g_can_ops->recv(msg); + return g_can_ops->recv(ch, msg); } -int can_if_filter_config(uint8_t filter_id, uint32_t id, uint32_t mask) +int can_if_filter_config(int ch, uint8_t filter_id, uint32_t id, uint32_t mask) { if (!g_can_ops || !g_can_ops->filter_config) return -1; - return g_can_ops->filter_config(filter_id, id, mask); + return g_can_ops->filter_config(ch, filter_id, id, mask); } diff --git a/interfaces/can/can_if.h b/interfaces/can/can_if.h index 2931b0a..4586d99 100644 --- a/interfaces/can/can_if.h +++ b/interfaces/can/can_if.h @@ -10,19 +10,19 @@ typedef struct { } can_message_t; typedef struct { - int (*init)(void); - int (*send)(const can_message_t *msg); - int (*recv)(can_message_t *msg); - int (*filter_config)(uint8_t filter_id, uint32_t id, uint32_t mask); + int (*init)(int ch); + int (*send)(int ch, const can_message_t *msg); + int (*recv)(int ch, can_message_t *msg); + int (*filter_config)(int ch, uint8_t filter_id, uint32_t id, uint32_t mask); } can_ops_t; // 注册接口（由 HAL 调用） void can_register_ops(const can_ops_t *ops); // 对上层暴露统一 API -int can_if_init(void); -int can_if_send(const can_message_t *msg); -int can_if_recv(can_message_t *msg); -int can_if_filter_config(uint8_t filter_id, uint32_t id, uint32_t mask); +int can_if_init(int ch); +int can_if_send(int ch, const can_message_t *msg); +int can_if_recv(int ch, can_message_t *msg); +int can_if_filter_config(int ch, uint8_t filter_id, uint32_t id, uint32_t mask); #endif diff --git a/interfaces/i2c/i2c_if.c b/interfaces/i2c/i2c_if.c new file mode 100644 index 0000000..64b2f8b --- /dev/null +++ b/interfaces/i2c/i2c_if.c @@ -0,0 +1,43 @@ +#include "i2c_if.h" + +static const i2c_ops_t *g_i2c_ops = 0; + +void i2c_register_ops(const i2c_ops_t *ops) +{ + g_i2c_ops = ops; +} + +int i2c_if_init(int ch) +{ + if (!g_i2c_ops || !g_i2c_ops->init) + return -1; + return g_i2c_ops->init(ch); +} + +int i2c_if_mem_write(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout) +{ + if (!g_i2c_ops || !g_i2c_ops->mem_write) + return -1; + return g_i2c_ops->mem_write(ch, dev_addr, mem_addr, mem_add_size, data, size, timeout); +} + +int i2c_if_mem_read(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout) +{ + if (!g_i2c_ops || !g_i2c_ops->mem_read) + return -1; + return g_i2c_ops->mem_read(ch, dev_addr, mem_addr, mem_add_size, data, size, timeout); +} + +int i2c_if_write(int ch, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout) +{ + if (!g_i2c_ops || !g_i2c_ops->write) + return -1; + return g_i2c_ops->write(ch, dev_addr, data, size, timeout); +} + +int i2c_if_read(int ch, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout) +{ + if (!g_i2c_ops || !g_i2c_ops->read) + return -1; + return g_i2c_ops->read(ch, dev_addr, data, size, timeout); +} diff --git a/interfaces/i2c/i2c_if.h b/interfaces/i2c/i2c_if.h new file mode 100644 index 0000000..64ea381 --- /dev/null +++ b/interfaces/i2c/i2c_if.h @@ -0,0 +1,31 @@ +#ifndef __I2C_IF_H__ +#define __I2C_IF_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct { + int (*init)(int ch); + int (*mem_write)(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout); + int (*mem_read)(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout); + int (*write)(int ch, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout); + int (*read)(int ch, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout); +} i2c_ops_t; + +void i2c_register_ops(const i2c_ops_t *ops); + +int i2c_if_init(int ch); +int i2c_if_mem_write(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout); +int i2c_if_mem_read(int ch, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout); +int i2c_if_write(int ch, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout); +int i2c_if_read(int ch, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/modules/control/pid/pid.c b/modules/control/pid/pid.c new file mode 100644 index 0000000..366fff8 --- /dev/null +++ b/modules/control/pid/pid.c @@ -0,0 +1,55 @@ +#include "pid.h" +#include + +void PIDController_Init(PIDController_t *pid) +{ + if (!pid) return; + pid->Kp = 0.0f; + pid->Ki = 0.0f; + pid->Kd = 0.0f; + pid->integral = 0.0f; + pid->prev_error = 0.0f; + pid->output_max = 204800.0f; // 电机最大速度 (0.01 DPS) +} + +float PIDController_Compute(PIDController_t *pid, float target, float actual) +{ + if (!pid) return 0.0f; + + float error = target - actual; + + // 比例项 + float p_term = pid->Kp * error; + + // 积分项 (带抗饱和) + pid->integral += error; + if (pid->integral > pid->output_max) { + pid->integral = pid->output_max; + } else if (pid->integral < -pid->output_max) { + pid->integral = -pid->output_max; + } + float i_term = pid->Ki * pid->integral; + + // 微分项 + float derivative = error - pid->prev_error; + float d_term = pid->Kd * derivative; + pid->prev_error = error; + + float output = p_term + i_term + d_term; + + // 输出限幅 + if (output > pid->output_max) { + output = pid->output_max; + } else if (output < -pid->output_max) { + output = -pid->output_max; + } + + return output; +} + +void PIDController_Reset(PIDController_t *pid) +{ + if (!pid) return; + pid->integral = 0.0f; + pid->prev_error = 0.0f; +} diff --git a/modules/control/pid/pid.h b/modules/control/pid/pid.h new file mode 100644 index 0000000..acc18d9 --- /dev/null +++ b/modules/control/pid/pid.h @@ -0,0 +1,25 @@ +#ifndef __PID_H__ +#define __PID_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef struct { + float Kp, Ki, Kd; + float integral; + float prev_error; + float output_max; +} PIDController_t; + +void PIDController_Init(PIDController_t *pid); +float PIDController_Compute(PIDController_t *pid, float target, float actual); +void PIDController_Reset(PIDController_t *pid); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/modules/device/can/can_device.c b/modules/device/can/can_device.c new file mode 100644 index 0000000..0507319 --- /dev/null +++ b/modules/device/can/can_device.c @@ -0,0 +1,48 @@ +#include "can_device.h" +#include "can_if.h" + +int can_device_init(can_device_t *dev, int ch) +{ + if (!dev) return -1; + + dev->ch = ch; + dev->initialized = 0; + + int ret = can_if_init(ch); + if (ret == 0) { + dev->initialized = 1; + } + + return ret; +} + +int can_device_send(can_device_t *dev, uint32_t id, const uint8_t *data, uint8_t len) +{ + if (!dev || !data || len > 8) return -1; + if (!dev->initialized) return -1; + + can_message_t msg; + msg.id = id; + msg.length = len; + for (int i = 0; i < len; i++) { + msg.data[i] = data[i]; + } + + return can_if_send(dev->ch, &msg); +} + +int can_device_recv(can_device_t *dev, can_message_t *msg) +{ + if (!dev || !msg) return -1; + if (!dev->initialized) return -1; + + return can_if_recv(dev->ch, msg); +} + +int can_device_set_filter(can_device_t *dev, uint8_t filter_id, uint32_t id, uint32_t mask) +{ + if (!dev || filter_id > 13) return -1; + if (!dev->initialized) return -1; + + return can_if_filter_config(dev->ch, filter_id, id, mask); +} diff --git a/modules/device/can/can_device.h b/modules/device/can/can_device.h new file mode 100644 index 0000000..172ef22 --- /dev/null +++ b/modules/device/can/can_device.h @@ -0,0 +1,18 @@ +#ifndef CAN_DEVICE_H +#define CAN_DEVICE_H + +#include "can_if.h" + +typedef struct { + int ch; // CAN 通道 + can_message_t tx_msg; // 发送消息 + can_message_t rx_msg; // 接收消息 + int initialized; // 初始化标志 +} can_device_t; + +int can_device_init(can_device_t *dev, int ch); +int can_device_send(can_device_t *dev, uint32_t id, const uint8_t *data, uint8_t len); +int can_device_recv(can_device_t *dev, can_message_t *msg); +int can_device_set_filter(can_device_t *dev, uint8_t filter_id, uint32_t id, uint32_t mask); + +#endif diff --git a/modules/device/motor/mf4010v2.c b/modules/device/motor/mf4010v2.c index 4ddb8b0..9f3dd99 100644 --- a/modules/device/motor/mf4010v2.c +++ b/modules/device/motor/mf4010v2.c @@ -9,7 +9,7 @@ */ #include "mf4010v2.h" -#include "bsp_can.h" +#include "can_if.h" #include #include #include @@ -17,21 +17,21 @@ /** * @brief 初始化电机结构体 */ -mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id) +int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch) { - // 参数验证 if(motor == NULL) { return MF4010_ERR_INVALID_PARAM; } - - // CAN ID 范围验证 (0x000-0x7FF for standard frame) + if(id > 0x7FF) { printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id); return MF4010_ERR_INVALID_ID; } - - // 初始化结构体 + + motor->can_ch = can_ch; motor->id = id; + motor->target_angle = 0; + motor->current_angle = 0; memset(motor->last_response, 0, sizeof(motor->last_response)); motor->last_comm_time = 0; motor->error_count = 0; @@ -43,8 +43,89 @@ mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id) /** * @brief 发送命令并接收响应（带超时） */ -mf4010_status_t mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8], uint32_t timeout_ms) +int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]) { - + can_message_t msg; + msg.id = motor->id; + memcpy(msg.data, command, 8); + msg.length = 8; + int ret = can_if_send(motor->can_ch, &msg); + if (ret != 0) { + motor->error_count++; + printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id); + return MF4010_ERR_CAN_SEND; + } + motor->success_count++; + return MF4010_OK; } +/** + * @brief 设置电机目标角度 (位置闭环) + * @param motor 电机结构体 + * @param angle_0_01deg 目标角度，单位 0.01 度 + */ +int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg) +{ + if (!motor) return MF4010_ERR_INVALID_PARAM; + + uint8_t cmd[8]; + // 使用增量位置控制 (A7 命令) + // 计算角度增量 + int32_t delta = angle_0_01deg - motor->current_angle; + + // 限制单次增量在合理范围内 + if (delta > 18000) delta = 18000; // 最多 +180 度 + if (delta < -18000) delta = -18000; // 最多 -180 度 + + cmd_position_add(cmd, delta); + motor->target_angle = angle_0_01deg; + + return mf4010v2_send_command(motor, motor->can_ch, cmd); +} + +/** + * @brief 设置电机速度 (速度闭环) + * @param motor 电机结构体 + * @param speed_0_01dps 目标速度，单位 0.01 DPS + */ +int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps) +{ + if (!motor) return MF4010_ERR_INVALID_PARAM; + + uint8_t cmd[8]; + cmd_speed_control(cmd, speed_0_01dps); + return mf4010v2_send_command(motor, motor->can_ch, cmd); +} + +/** + * @brief 启动电机 + */ +void mf4010v2_run(mf4010v2_t* motor) +{ + if (!motor) return; + uint8_t cmd[8] = COMMAND_MOTOR_RUNNING; + mf4010v2_send_command(motor, motor->can_ch, cmd); + motor->flags |= MF4010_FLAG_RUNNING; +} + +/** + * @brief 停止电机 + */ +void mf4010v2_stop(mf4010v2_t* motor) +{ + if (!motor) return; + uint8_t cmd[8] = COMMAND_MOTOR_STOP; + mf4010v2_send_command(motor, motor->can_ch, cmd); + motor->flags &= ~MF4010_FLAG_RUNNING; +} + +/** + * @brief 获取当前速度 (返回目标速度，因为实际速度需要从编码器反馈) + */ +int32_t mf4010v2_get_speed(mf4010v2_t* motor) +{ + if (!motor) return 0; + uint8_t cmd[8] = + mf4010v2_send_command(motor, motor->can_ch, cmd); + return motor->target_angle; // 占位返回 +} diff --git a/modules/device/motor/mf4010v2.h b/modules/device/motor/mf4010v2.h index 933ee9a..fc53117 100644 --- a/modules/device/motor/mf4010v2.h +++ b/modules/device/motor/mf4010v2.h @@ -39,7 +39,8 @@ typedef enum { MF4010_ERR_INVALID_PARAM = -4, /**< 无效参数 */ MF4010_ERR_BUS_OFF = -5, /**< CAN 总线关闭 */ MF4010_ERR_CRC = -6, /**< CRC 校验错误 */ - MF4010_ERR_HARDWARE = -7 /**< 硬件错误 */ + MF4010_ERR_HARDWARE = -7, /**< 硬件错误 */ + MF4010_ERR_CAN_SEND = -8 /**< CAN 发送失败 */ } mf4010_status_t; /** @@ -60,21 +61,18 @@ typedef enum { /** * @brief 电机结构体（优化版） */ -struct __mf4010v2_t; -typedef struct __mf4010v2_t mf4010v2_t; // 前向声明 - typedef struct { - mf4010_status_t (*init)(); - mf4010_status_t (*send_command)(); -}mf4010v2_ops_t; + struct __mf4010v2_t { - mf4010v2_ops_t ops; /**< 操作函数指针 */ + int can_ch; /**< CAN 通道 (0 或 1) */ uint16_t id; /**< CAN ID (0x000-0x7FF) */ uint8_t last_response[8]; /**< 最后一次响应数据 */ uint32_t last_comm_time; /**< 最后通信时间戳 (ms) */ uint16_t error_count; /**< 通信错误计数 */ uint16_t success_count; /**< 成功通信计数 */ uint8_t flags; /**< 状态标志 (@ref mf4010_flag_t) */ + int32_t target_angle; /**< 目标角度 (0.01 度) */ + int32_t current_angle; /**< 当前角度 (0.01 度) - 从编码器读取 */ }; @@ -89,31 +87,26 @@ struct __mf4010v2_t { * @brief 初始化电机结构体 * @param motor: 电机结构体指针 * @param id: 电机 CAN ID (范围 0x000-0x7FF，如 0x141) + * @param can_ch: CAN 通道 (0=CAN1, 1=CAN2) * @retval 成功返回 MF4010_OK，失败返回错误码 */ -mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id); +int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch); /** * @brief 发送命令并接收响应（带超时） * @param motor: 电机结构体指针 + * @param can_ch: CAN 通道 * @param command: 8 字节命令数据 - * @param timeout_ms: 超时时间 (ms)，0 表示无限等待 * @retval 成功返回 MF4010_OK，失败返回错误码 - * - * @note 命令格式参考下面的宏定义 - * @example - * // 关闭电机 - * uint8_t cmd[8] = COMMAND_MOTOR_CLOSE; - * mf4010v2_send_command(&motor, cmd, 1000); - * - * // 速度控制 (100 DPS) - * uint8_t cmd[8]; - * cmd_speed_control(cmd, 10000); // 100 * 100 (0.01 DPS/LSB) - * mf4010v2_send_command(&motor, cmd, 1000); */ -mf4010_status_t mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8], uint32_t timeout_ms); +int mf4010v2_send_command(mf4010v2_t* motor, int can_ch, const uint8_t command[8]); +void mf4010v2_run(mf4010v2_t* motor); +void mf4010v2_stop(mf4010v2_t* motor); +int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps); +int32_t mf4010v2_get_speed(mf4010v2_t* motor); + /* ============================================================================ * 命令宏定义（8 字节 CAN 命令帧） * ============================================================================ */ diff --git a/modules/sensor/imu/j901p.c b/modules/sensor/imu/j901p.c deleted file mode 100644 index b456d94..0000000 --- a/modules/sensor/imu/j901p.c +++ /dev/null @@ -1,493 +0,0 @@ -#include "j901p.h" - -static SerialWrite p_WitSerialWriteFunc = NULL; -static WitI2cWrite p_WitI2cWriteFunc = NULL; -static WitI2cRead p_WitI2cReadFunc = NULL; -static CanWrite p_WitCanWriteFunc = NULL; -static RegUpdateCb p_WitRegUpdateCbFunc = NULL; -static DelaymsCb p_WitDelaymsFunc = NULL; - -static uint8_t s_ucAddr = 0xff; -static uint8_t s_ucWitDataBuff[WIT_DATA_BUFF_SIZE]; -static uint32_t s_uiWitDataCnt = 0, s_uiProtoclo = 0, s_uiReadRegIndex = 0; -int16_t sReg[REGSIZE]; - - -#define FuncW 0x06 -#define FuncR 0x03 - -static const uint8_t __auchCRCHi[256] = { - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, - 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, - 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, - 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, - 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, - 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, - 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, - 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, - 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, - 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, - 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, - 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, - 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, - 0x40 -}; -static const uint8_t __auchCRCLo[256] = { - 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, - 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, - 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, - 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, - 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, - 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, - 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, - 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, - 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, - 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, - 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, - 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, - 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, - 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, - 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, - 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, - 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, - 0x40 -}; - - -static uint16_t __CRC16(uint8_t *puchMsg, uint16_t usDataLen) -{ - uint8_t uchCRCHi = 0xFF; - uint8_t uchCRCLo = 0xFF; - uint8_t uIndex; - int i = 0; - uchCRCHi = 0xFF; - uchCRCLo = 0xFF; - for (; i= 11) - { - ucSum = __CaliSum(s_ucWitDataBuff, 10); - if(ucSum != s_ucWitDataBuff[10]) - { - s_uiWitDataCnt--; - memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt); - return ; - } - usData[0] = ((uint16_t)s_ucWitDataBuff[3] << 8) | (uint16_t)s_ucWitDataBuff[2]; - usData[1] = ((uint16_t)s_ucWitDataBuff[5] << 8) | (uint16_t)s_ucWitDataBuff[4]; - usData[2] = ((uint16_t)s_ucWitDataBuff[7] << 8) | (uint16_t)s_ucWitDataBuff[6]; - usData[3] = ((uint16_t)s_ucWitDataBuff[9] << 8) | (uint16_t)s_ucWitDataBuff[8]; - CopeWitData(s_ucWitDataBuff[1], usData, 4); - s_uiWitDataCnt = 0; - } - break; - case WIT_PROTOCOL_MODBUS: - if(s_uiWitDataCnt > 2) - { - if(s_ucWitDataBuff[1] != FuncR) - { - s_uiWitDataCnt--; - memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt); - return ; - } - if(s_uiWitDataCnt < (s_ucWitDataBuff[2] + 5))return ; - usTemp = ((uint16_t)s_ucWitDataBuff[s_uiWitDataCnt-2] << 8) | s_ucWitDataBuff[s_uiWitDataCnt-1]; - usCRC16 = __CRC16(s_ucWitDataBuff, s_uiWitDataCnt-2); - if(usTemp != usCRC16) - { - s_uiWitDataCnt--; - memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt); - return ; - } - usTemp = s_ucWitDataBuff[2] >> 1; - for(i = 0; i < usTemp; i++) - { - sReg[i+s_uiReadRegIndex] = ((uint16_t)s_ucWitDataBuff[(i<<1)+3] << 8) | s_ucWitDataBuff[(i<<1)+4]; - } - p_WitRegUpdateCbFunc(s_uiReadRegIndex, usTemp); - s_uiWitDataCnt = 0; - } - break; - case WIT_PROTOCOL_CAN: - case WIT_PROTOCOL_I2C: - s_uiWitDataCnt = 0; - break; - } - if(s_uiWitDataCnt == WIT_DATA_BUFF_SIZE)s_uiWitDataCnt = 0; -} -int32_t WitI2cFuncRegister(WitI2cWrite write_func, WitI2cRead read_func) -{ - if(!write_func)return WIT_HAL_INVAL; - if(!read_func)return WIT_HAL_INVAL; - p_WitI2cWriteFunc = write_func; - p_WitI2cReadFunc = read_func; - return WIT_HAL_OK; -} -int32_t WitCanWriteRegister(CanWrite Write_func) -{ - if(!Write_func)return WIT_HAL_INVAL; - p_WitCanWriteFunc = Write_func; - return WIT_HAL_OK; -} -void WitCanDataIn(uint8_t ucData[8], uint8_t ucLen) -{ - uint16_t usData[3]; - if(p_WitRegUpdateCbFunc == NULL)return ; - if(ucLen < 8)return ; - switch(s_uiProtoclo) - { - case WIT_PROTOCOL_CAN: - if(ucData[0] != 0x55)return ; - usData[0] = ((uint16_t)ucData[3] << 8) | ucData[2]; - usData[1] = ((uint16_t)ucData[5] << 8) | ucData[4]; - usData[2] = ((uint16_t)ucData[7] << 8) | ucData[6]; - CopeWitData(ucData[1], usData, 3); - break; - case WIT_PROTOCOL_NORMAL: - case WIT_PROTOCOL_MODBUS: - case WIT_PROTOCOL_I2C: - break; - } -} -int32_t WitRegisterCallBack(RegUpdateCb update_func) -{ - if(!update_func)return WIT_HAL_INVAL; - p_WitRegUpdateCbFunc = update_func; - return WIT_HAL_OK; -} -int32_t WitWriteReg(uint32_t uiReg, uint16_t usData) -{ - uint16_t usCRC; - uint8_t ucBuff[8]; - if(uiReg >= REGSIZE)return WIT_HAL_INVAL; - switch(s_uiProtoclo) - { - case WIT_PROTOCOL_NORMAL: - if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; - ucBuff[0] = 0xFF; - ucBuff[1] = 0xAA; - ucBuff[2] = uiReg & 0xFF; - ucBuff[3] = usData & 0xff; - ucBuff[4] = usData >> 8; - p_WitSerialWriteFunc(ucBuff, 5); - break; - case WIT_PROTOCOL_MODBUS: - if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; - ucBuff[0] = s_ucAddr; - ucBuff[1] = FuncW; - ucBuff[2] = uiReg >> 8; - ucBuff[3] = uiReg & 0xFF; - ucBuff[4] = usData >> 8; - ucBuff[5] = usData & 0xff; - usCRC = __CRC16(ucBuff, 6); - ucBuff[6] = usCRC >> 8; - ucBuff[7] = usCRC & 0xff; - p_WitSerialWriteFunc(ucBuff, 8); - break; - case WIT_PROTOCOL_CAN: - if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY; - ucBuff[0] = 0xFF; - ucBuff[1] = 0xAA; - ucBuff[2] = uiReg & 0xFF; - ucBuff[3] = usData & 0xff; - ucBuff[4] = usData >> 8; - p_WitCanWriteFunc(s_ucAddr, ucBuff, 5); - break; - case WIT_PROTOCOL_I2C: - if(p_WitI2cWriteFunc == NULL)return WIT_HAL_EMPTY; - ucBuff[0] = usData & 0xff; - ucBuff[1] = usData >> 8; - if(p_WitI2cWriteFunc(s_ucAddr << 1, uiReg, ucBuff, 2) != 1) - { - //printf("i2c write fail\r\n"); - } - break; - default: - return WIT_HAL_INVAL; - } - return WIT_HAL_OK; -} -int32_t WitReadReg(uint32_t uiReg, uint32_t uiReadNum) -{ - uint16_t usTemp, i; - uint8_t ucBuff[8]; - if((uiReg + uiReadNum) >= REGSIZE)return WIT_HAL_INVAL; - switch(s_uiProtoclo) - { - case WIT_PROTOCOL_NORMAL: - if(uiReadNum > 4)return WIT_HAL_INVAL; - if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; - ucBuff[0] = 0xFF; - ucBuff[1] = 0xAA; - ucBuff[2] = 0x27; - ucBuff[3] = uiReg & 0xff; - ucBuff[4] = uiReg >> 8; - p_WitSerialWriteFunc(ucBuff, 5); - break; - case WIT_PROTOCOL_MODBUS: - if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; - usTemp = uiReadNum << 1; - if((usTemp + 5) > WIT_DATA_BUFF_SIZE)return WIT_HAL_NOMEM; - ucBuff[0] = s_ucAddr; - ucBuff[1] = FuncR; - ucBuff[2] = uiReg >> 8; - ucBuff[3] = uiReg & 0xFF; - ucBuff[4] = uiReadNum >> 8; - ucBuff[5] = uiReadNum & 0xff; - usTemp = __CRC16(ucBuff, 6); - ucBuff[6] = usTemp >> 8; - ucBuff[7] = usTemp & 0xff; - p_WitSerialWriteFunc(ucBuff, 8); - break; - case WIT_PROTOCOL_CAN: - if(uiReadNum > 3)return WIT_HAL_INVAL; - if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY; - ucBuff[0] = 0xFF; - ucBuff[1] = 0xAA; - ucBuff[2] = 0x27; - ucBuff[3] = uiReg & 0xff; - ucBuff[4] = uiReg >> 8; - p_WitCanWriteFunc(s_ucAddr, ucBuff, 5); - break; - case WIT_PROTOCOL_I2C: - if(p_WitI2cReadFunc == NULL)return WIT_HAL_EMPTY; - usTemp = uiReadNum << 1; - if(WIT_DATA_BUFF_SIZE < usTemp)return WIT_HAL_NOMEM; - if(p_WitI2cReadFunc(s_ucAddr << 1, uiReg, s_ucWitDataBuff, usTemp) == 1) - { - if(p_WitRegUpdateCbFunc == NULL)return WIT_HAL_EMPTY; - for(i = 0; i < uiReadNum; i++) - { - sReg[i+uiReg] = ((uint16_t)s_ucWitDataBuff[(i<<1)+1] << 8) | s_ucWitDataBuff[i<<1]; - } - p_WitRegUpdateCbFunc(uiReg, uiReadNum); - } - - break; - default: - return WIT_HAL_INVAL; - } - s_uiReadRegIndex = uiReg; - - return WIT_HAL_OK; -} -int32_t WitInit(uint32_t uiProtocol, uint8_t ucAddr) -{ - if(uiProtocol > WIT_PROTOCOL_I2C)return WIT_HAL_INVAL; - s_uiProtoclo = uiProtocol; - s_ucAddr = ucAddr; - s_uiWitDataCnt = 0; - return WIT_HAL_OK; -} -void WitDeInit(void) -{ - p_WitSerialWriteFunc = NULL; - p_WitI2cWriteFunc = NULL; - p_WitI2cReadFunc = NULL; - p_WitCanWriteFunc = NULL; - p_WitRegUpdateCbFunc = NULL; - s_ucAddr = 0xff; - s_uiWitDataCnt = 0; - s_uiProtoclo = 0; -} - -int32_t WitDelayMsRegister(DelaymsCb delayms_func) -{ - if(!delayms_func)return WIT_HAL_INVAL; - p_WitDelaymsFunc = delayms_func; - return WIT_HAL_OK; -} - -char CheckRange(short sTemp,short sMin,short sMax) -{ - if ((sTemp>=sMin)&&(sTemp<=sMax)) return 1; - else return 0; -} -/*Acceleration calibration demo*/ -int32_t WitStartAccCali(void) -{ -/* - First place the equipment horizontally, and then perform the following operations -*/ - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;// unlock reg - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(CALSW, CALGYROACC) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} -int32_t WitStopAccCali(void) -{ - if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(SAVE, SAVE_PARAM) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} -/*Magnetic field calibration*/ -int32_t WitStartMagCali(void) -{ - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(CALSW, CALMAGMM) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} -int32_t WitStopMagCali(void) -{ - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} -/*change Band*/ -int32_t WitSetUartBaud(int32_t uiBaudIndex) -{ - if(!CheckRange(uiBaudIndex,WIT_BAUD_4800,WIT_BAUD_230400)) - { - return WIT_HAL_INVAL; - } - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} -/*change Can Band*/ -int32_t WitSetCanBaud(int32_t uiBaudIndex) -{ - if(!CheckRange(uiBaudIndex,CAN_BAUD_1000000,CAN_BAUD_3000)) - { - return WIT_HAL_INVAL; - } - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} -/*change Bandwidth*/ -int32_t WitSetBandwidth(int32_t uiBaudWidth) -{ - if(!CheckRange(uiBaudWidth,BANDWIDTH_256HZ,BANDWIDTH_5HZ)) - { - return WIT_HAL_INVAL; - } - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(BANDWIDTH, uiBaudWidth) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} - -/*change output rate */ -int32_t WitSetOutputRate(int32_t uiRate) -{ - if(!CheckRange(uiRate,RRATE_02HZ,RRATE_NONE)) - { - return WIT_HAL_INVAL; - } - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(RRATE, uiRate) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} - -/*change WitSetContent */ -int32_t WitSetContent(int32_t uiRsw) -{ - if(!CheckRange(uiRsw,RSW_TIME,RSW_MASK)) - { - return WIT_HAL_INVAL; - } - if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; - if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); - else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); - if(WitWriteReg(RSW, uiRsw) != WIT_HAL_OK) return WIT_HAL_ERROR; - return WIT_HAL_OK; -} - - - diff --git a/modules/sensor/imu/j901p_hal.c b/modules/sensor/imu/j901p_hal.c index 7d54738..9c0dce7 100644 --- a/modules/sensor/imu/j901p_hal.c +++ b/modules/sensor/imu/j901p_hal.c @@ -1,20 +1,21 @@ #include "j901p_hal.h" #include "j901p.h" -#include "i2c.h" +#include "i2c_if.h" + +#define J901P_I2C_CH 1 +#define I2C_MEMADD_SIZE_8BIT ((uint16_t)0x0001) int32_t J901P_I2C_Write(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) { - if(HAL_I2C_Mem_Write(&hi2c2, ucAddr, ucReg, - I2C_MEMADD_SIZE_8BIT, p_ucVal, uiLen, 100) == HAL_OK) - return 1; // SDK expects 1 for success + if (i2c_if_mem_write(J901P_I2C_CH, ucAddr, ucReg, I2C_MEMADD_SIZE_8BIT, p_ucVal, uiLen, 100) == 0) + return 1; return 0; } int32_t J901P_I2C_Read(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) { - if(HAL_I2C_Mem_Read(&hi2c2, ucAddr, ucReg, - I2C_MEMADD_SIZE_8BIT, p_ucVal, uiLen, 100) == HAL_OK) - return 1; // SDK expects 1 for success + if (i2c_if_mem_read(J901P_I2C_CH, ucAddr, ucReg, I2C_MEMADD_SIZE_8BIT, p_ucVal, uiLen, 100) == 0) + return 1; return 0; } @@ -27,5 +28,6 @@ int32_t J901P_HAL_Init(uint8_t i2cAddr) void J901P_Delay(uint16_t ms) { + extern void HAL_Delay(uint32_t Delay); HAL_Delay(ms); } diff --git a/modules/sensor/imu/j901p_module.c b/modules/sensor/imu/j901p_module.c new file mode 100644 index 0000000..f3f7190 --- /dev/null +++ b/modules/sensor/imu/j901p_module.c @@ -0,0 +1,112 @@ +#include "j901p_module.h" +#include "j901p_hal.h" +#include "i2c_if.h" +#include "j901p_reg.h" +#include "j901p.h" + +j901p_module_t g_imu = {0}; + +static void on_reg_update(uint32_t uiReg, uint32_t uiRegNum) +{ + // 根据寄存器地址更新对应的数据 + switch (uiReg) { + case AX: + if (uiRegNum >= 3) { + g_imu.acc[0] = sReg[AX]; + g_imu.acc[1] = sReg[AY]; + g_imu.acc[2] = sReg[AZ]; + } + break; + case GX: + if (uiRegNum >= 3) { + g_imu.gyro[0] = sReg[GX]; + g_imu.gyro[1] = sReg[GY]; + g_imu.gyro[2] = sReg[GZ]; + } + break; + case Roll: + if (uiRegNum >= 3) { + g_imu.angle[0] = sReg[Roll]; // Roll + g_imu.angle[1] = sReg[Pitch]; // Pitch + g_imu.angle[2] = sReg[Yaw]; // Yaw + } + break; + case HX: + if (uiRegNum >= 3) { + g_imu.mag[0] = sReg[HX]; + g_imu.mag[1] = sReg[HY]; + g_imu.mag[2] = sReg[HZ]; + } + break; + } +} + +int j901p_module_init(j901p_module_t *dev, int i2c_ch, uint8_t i2c_addr) +{ + if (!dev) return -1; + + dev->ch = i2c_ch; + dev->i2c_addr = i2c_addr; + dev->initialized = 0; + + // 复制地址到全局变量 + g_imu.ch = i2c_ch; + g_imu.i2c_addr = i2c_addr; + + int ret = i2c_if_init(i2c_ch); + if (ret != 0) { + return -1; + } + + WitRegisterCallBack(on_reg_update); + ret = J901P_HAL_Init(i2c_addr); + if (ret != WIT_HAL_OK) { + return -1; + } + + dev->initialized = 1; + g_imu.initialized = 1; + return 0; +} + +int j901p_module_read(j901p_module_t *dev) +{ + if (!dev || !dev->initialized) return -1; + + // 读取加速度计、陀螺仪、角度、磁力计 + WitReadReg(AX, 3); + WitReadReg(GX, 3); + WitReadReg(Roll, 3); + WitReadReg(HX, 3); + + return 0; +} + +void j901p_module_register_callback(void) +{ + WitRegisterCallBack(on_reg_update); +} + +int j901p_module_start_acc_cali(j901p_module_t *dev) +{ + if (!dev || !dev->initialized) return -1; + return WitStartAccCali(); +} + +int j901p_module_stop_acc_cali(j901p_module_t *dev) +{ + if (!dev || !dev->initialized) return -1; + return WitStopAccCali(); +} + +int j901p_module_start_mag_cali(j901p_module_t *dev) +{ + if (!dev || !dev->initialized) return -1; + return WitStartMagCali(); +} + +int j901p_module_stop_mag_cali(j901p_module_t *dev) +{ + if (!dev || !dev->initialized) return -1; + return WitStopMagCali(); +} diff --git a/modules/sensor/imu/j901p_module.h b/modules/sensor/imu/j901p_module.h new file mode 100644 index 0000000..7b5fe09 --- /dev/null +++ b/modules/sensor/imu/j901p_module.h @@ -0,0 +1,36 @@ +#ifndef __J901P_MODULE_H__ +#define __J901P_MODULE_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef struct { + int ch; + uint8_t i2c_addr; + int initialized; + // 原始数据 (0.01 度/0.01DPS 单位) + int16_t acc[3]; // AX, AY, AZ + int16_t gyro[3]; // GX, GY, GZ + int16_t angle[3]; // Roll, Pitch, Yaw (0.01 度) + int16_t mag[3]; // HX, HY, HZ +} j901p_module_t; + +// 全局 IMU 实例 (供回调使用) +extern j901p_module_t g_imu; + +int j901p_module_init(j901p_module_t *dev, int i2c_ch, uint8_t i2c_addr); +int j901p_module_read(j901p_module_t *dev); +void j901p_module_register_callback(void); +int j901p_module_start_acc_cali(j901p_module_t *dev); +int j901p_module_stop_acc_cali(j901p_module_t *dev); +int j901p_module_start_mag_cali(j901p_module_t *dev); +int j901p_module_stop_mag_cali(j901p_module_t *dev); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/prompt.md b/prompt.md new file mode 100644 index 0000000..1f935c5 --- /dev/null +++ b/prompt.md @@ -0,0 +1,367 @@ +# Embedded Firmware Architecture Prompt (强化版 · 带检查规则与自动纠错) + +--- + +# 🎯 目标 + +本 Prompt 用于驱动 Agent 在嵌入式工程中执行**规范化开发 + 自动审查 + 自动修复**： + +* 严格分层（APP / Module / Interface / HAL / BSP） +* 自动检测架构违规 +* 自动修复跨层调用与职责污染 +* 输出可直接工程化代码 + +--- + +# 🧱 一、分层架构（不可违反） + +```text +APP + ↓ +Module + ↓ +Interface + ↓ +HAL + ↓ +BSP + ↓ +Driver / 寄存器 +``` + +--- + +# 📌 二、职责定义（强约束） + +| 层 | 允许做 | 禁止做 | +| --------- | ----- | ------------ | +| APP | 业务逻辑 | 调用 HAL/BSP | +| Module | 功能封装 | 绑定硬件 / 调 HAL | +| Interface | 抽象API | 写实现 / 用 BSP | +| HAL | 外设操作 | 写业务 / 写板级映射 | +| BSP | 硬件描述 | 写逻辑 | + +--- + +# 🔗 三、调用链规则（强制执行） + +## ✅ 唯一合法路径 + +```text +APP → Module → Interface → HAL → BSP +``` + +## ❌ 非法调用（必须报错 + 修复） + +```text +Module → HAL +Module → BSP +Interface → BSP +HAL → Module +BSP → HAL（主动调用） +``` + +--- + +# 🧪 四、自动检查规则（Agent必须执行） + +Agent 在生成或分析代码时，必须执行以下检查： + +--- + +## ✅ 规则1：依赖方向检查 + +**检测：** + +* 是否存在“跨层调用” + +**实现逻辑：** + +```pseudo +if caller_layer > callee_layer: + ERROR +``` + +--- + +## ✅ 规则2：语义污染检查 + +### HAL层检查 + +```pseudo +if file == HAL: + 禁止出现关键词: led, motor, sensor +``` + +### BSP层检查 + +```pseudo +if file == BSP: + 禁止出现: + if / for / while（复杂逻辑） + 业务函数（如 led_on） +``` + +--- + +## ✅ 规则3：接口唯一性检查 + +```pseudo +if hardware_access not via Interface: + ERROR +``` + +--- + +## ✅ 规则4：函数指针绑定位置 + +```pseudo +if ops赋值出现在 Module: + ERROR +``` + +--- + +## ✅ 规则5：寄存器访问检查 + +```pseudo +if 寄存器操作出现在 HAL以上: + ERROR +``` + +--- + +# 🔧 五、自动纠错策略（核心） + +Agent 必须在发现问题时**自动重构代码**，而不是仅提示错误。 + +--- + +## ❌ 错误1：Module直接调用HAL + +### 输入（错误）： + +```c +HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, 1); +``` + +### 自动修复： + +```text +1. 创建 Interface API： + led_if_write() + +2. Module 改为： + led_if_write(id, state) + +3. HAL 实现该接口 +``` + +--- + +## ❌ 错误2：Module绑定ops + +### 输入： + +```c +led->ops.write = bsp_led_write; +``` + +### 修复： + +```text +1. 删除 Module 中 ops 绑定 +2. 在 HAL 中定义 ops +3. 在 HAL 初始化中注册： + led_register_ops(&ops) +``` + +--- + +## ❌ 错误3：HAL包含业务语义 + +### 输入： + +```c +void hal_led_on() +``` + +### 修复： + +```text +重命名为： +hal_gpio_write() + +将“LED语义”移至 Module +``` + +--- + +## ❌ 错误4：BSP写逻辑 + +### 输入： + +```c +void bsp_led_on() +``` + +### 修复： + +```text +拆分为： +BSP：只保留 GPIO映射 +HAL：实现写操作 +Module：调用接口 +``` + +--- + +## ❌ 错误5：Interface直接调用BSP + +### 修复： + +```text +Interface → HAL → BSP +``` + +--- + +# 🔄 六、自动重构流程（Agent执行流程） + +```pseudo +1. 解析代码 +2. 标注每个函数所属层 +3. 检查调用关系 +4. 标记违规点 +5. 应用纠错规则： + - 提取接口 + - 下沉实现到HAL + - 上移语义到Module +6. 重构代码结构 +7. 输出修复版本 +``` + +--- + +# 🧠 七、层级识别规则（Agent用） + +```pseudo +if 使用 GPIO/UART: + HAL + +if 定义 pin mapping: + BSP + +if 使用 xxx_if: + Interface + +if 使用设备结构体: + Module + +if 控制流程: + APP +``` + +--- + +# 📦 八、输出规范（强制） + +Agent 输出代码必须： + +* 按目录分层 +* 每层文件独立 +* 无跨层调用 +* 接口统一 +* 可编译 + +--- + +# 🧩 九、标准代码模板（自动生成） + +## Interface模板 + +```c +typedef struct { + int (*init)(int id); + int (*write)(int id, int state); +} xxx_ops_t; +``` + +--- + +## HAL模板 + +```c +static int hal_xxx_write(...) { } +``` + +--- + +## Module模板 + +```c +xxx_if_write(...) +``` + +--- + +## BSP模板 + +```c +{GPIOA, GPIO_PIN_5} +``` + +--- + +# 🚨 十、错误等级定义 + +| 等级 | 类型 | +| ------- | ---- | +| FATAL | 架构破坏 | +| ERROR | 设计错误 | +| WARNING | 风格问题 | + +--- + +# 🧭 十一、Agent行为模式 + +Agent必须： + +1. **优先保证架构正确** +2. **自动修复错误** +3. **不允许带病输出代码** +4. **必要时重构整个模块** + +--- + +# 🧠 十二、核心哲学（必须遵守） + +> **宁可重构，也不妥协分层** + +--- + +# 🚀 十三、增强能力（可选） + +* 自动生成 Interface +* 自动拆分 BSP / HAL +* 自动生成 Mock HAL（用于测试） +* 自动识别平台（STM32 / Linux） + +--- + +# ✅ 使用方式 + +将本 Prompt 用于： + +* AI代码生成 +* Code Review +* 架构审查 +* 自动重构工具 + +--- + +# 🧩 最终一句话 + +> **任何硬件访问必须经过 Interface，任何硬件位置必须由 BSP 描述，任何硬件操作必须在 HAL 完成，任何功能逻辑必须在 Module，任何决策必须在 APP** + +---