[add] soft i2c rewrite

This commit is contained in:
robinson committed 2026-05-11 08:25:49 +08:00
1 parent ed81f89cd6
commit 5bf1920867
36 files changed
+1371 -3794

No files matched your search

+5 -1
View File
@@ -5,7 +5,11 @@
"Bash(cmake -G \"Ninja\" -DCMAKE_TOOLCHAIN_FILE=../../cmake/gcc-arm-none-eabi.cmake ../..)",
"Bash(ninja)",
"Bash(ninja clean *)",
"Bash(git add *)"
"Bash(git add *)",
"Bash(ninja -j1)",
"Bash(ninja -j1 CMakeFiles/CloudPlant.dir/modules/bus/soft_i2c/soft_i2c.c.obj CMakeFiles/CloudPlant.dir/modules/bus/soft_i2c/soft_i2c_hal.c.obj CMakeFiles/CloudPlant.dir/hal/stm32f4/soft_i2c/hal_soft_i2c.c.obj CMakeFiles/CloudPlant.dir/bsp/stm32f4/soft_i2c/bsp_soft_i2c.c.obj)",
"Bash(ninja:*)",
"Bash(findstr:*)"
]
}
}
-75
View File
@@ -1,75 +0,0 @@
#include "PID.h"
void PIDController_Init(PIDController* pid)
{
/* Clear controller variables */
pid->integrator = 0.0f;
pid->prevError = 0.0f;
pid->differentiator = 0.0f;
pid->prevMeasurement = 0.0f;
pid->out = 0.0f;
}
float PIDController_Update(PIDController* pid, float setpoint, float measurement)
{
/*
* Error signal
*/
float error = setpoint - measurement;
/*
* Proportional
*/
float proportional = pid->Kp * error;
/*
* Integral
*/
pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
/* Anti-wind-up via integrator clamping */
if (pid->integrator > pid->limMaxInt) {
pid->integrator = pid->limMaxInt;
} else if (pid->integrator < pid->limMinInt) {
pid->integrator = pid->limMinInt;
}
/*
* Derivative (band-limited differentiator)
*/
pid->differentiator =
-(2.0f * pid->Kd *
(measurement -
pid->prevMeasurement) /* Note: derivative on measurement, therefore minus sign in front of equation! */
+ (2.0f * pid->tau - pid->T) * pid->differentiator) /
(2.0f * pid->tau + pid->T);
/*
* Compute output and apply limits
*/
pid->out = proportional + pid->integrator + pid->differentiator;
if (pid->out > pid->limMax) {
pid->out = pid->limMax;
} else if (pid->out < pid->limMin) {
pid->out = pid->limMin;
}
/* Store error and measurement for later use */
pid->prevError = error;
pid->prevMeasurement = measurement;
/* Return controller output */
return pid->out;
}
-39
View File
@@ -1,39 +0,0 @@
#ifndef PID_CONTROLLER_H
#define PID_CONTROLLER_H
typedef struct {
/* Controller gains */
float Kp;
float Ki;
float Kd;
/* Derivative low-pass filter time constant */
float tau;
/* Output limits */
float limMin;
float limMax;
/* Integrator limits */
float limMinInt;
float limMaxInt;
/* Sample time (in seconds) */
float T;
/* Controller "memory" */
float integrator;
float prevError; /* Required for integrator */
float differentiator;
float prevMeasurement; /* Required for differentiator */
/* Controller output */
float out;
} PIDController;
void PIDController_Init(PIDController *pid);
float PIDController_Update(PIDController *pid, float setpoint, float measurement);
#endif
+6 -4
View File
@@ -21,8 +21,6 @@ endif()
# Set the project name
set(CMAKE_PROJECT_NAME CloudPlant)
# Include toolchain file
include("cmake/gcc-arm-none-eabi.cmake")
# Enable compile command to ease indexing with e.g. clangd
set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE)
@@ -54,23 +52,24 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
modules/device/can/can_device.c
modules/control/pid/pid.c
modules/bus/soft_i2c/soft_i2c.c
modules/bus/soft_i2c/soft_i2c_hal.c
bsp/stm32f4/can/bsp_can.c
bsp/stm32f4/led/bsp_led.c
bsp/stm32f4/i2c/bsp_i2c.c
bsp/stm32f4/gpio/bsp_gpio.c
bsp/stm32f4/soft_i2c/bsp_soft_i2c.c
hal/stm32f4/can/hal_can.c
hal/stm32f4/led/hal_led.c
hal/stm32f4/i2c/hal_i2c.c
hal/stm32f4/gpio/hal_gpio.c
hal/stm32f4/soft_i2c/hal_soft_i2c.c
app/app_init.c
app/control_task.c
app/sensor_task.c
app/sensor_i2c_port.c
interfaces/led/led_if.c
interfaces/can/can_if.c
interfaces/i2c/i2c_if.c
interfaces/gpio/gpio_if.c
interfaces/soft_i2c/soft_i2c_if.c
)
@@ -88,15 +87,18 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/led
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/i2c
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/gpio
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/soft_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}/hal/stm32f4/gpio
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/soft_i2c
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/led
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/can
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/i2c
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/gpio
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/soft_i2c
)
# Add project symbols (macros)
+13 -4
View File
@@ -91,6 +91,14 @@ PB12.Mode=CAN_Activate
PB12.Signal=CAN2_RX
PB13.Mode=CAN_Activate
PB13.Signal=CAN2_TX
PCC.Checker=false
PCC.Display=Plot\: All Steps
PCC.Line=STM32F407/417
PCC.MCU=STM32F407Z(E-G)Tx
PCC.PartNumber=STM32F407ZGTx
PCC.Series=STM32F4
PCC.Temperature=25
PCC.Vdd=3.3
PD0.Locked=true
PD0.Mode=CAN_Activate
PD0.Signal=CAN1_RX
@@ -111,16 +119,16 @@ PF1.Mode=I2C
PF1.Signal=I2C2_SCL
PF12.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultOutputPP
PF12.GPIO_Label=SDA
PF12.GPIO_ModeDefaultOutputPP=GPIO_MODE_OUTPUT_PP
PF12.GPIO_PuPd=GPIO_NOPULL
PF12.GPIO_ModeDefaultOutputPP=GPIO_MODE_OUTPUT_OD
PF12.GPIO_PuPd=GPIO_PULLUP
PF12.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PF12.Locked=true
PF12.PinState=GPIO_PIN_SET
PF12.Signal=GPIO_Output
PF14.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultOutputPP
PF14.GPIO_Label=SCL
PF14.GPIO_ModeDefaultOutputPP=GPIO_MODE_OUTPUT_PP
PF14.GPIO_PuPd=GPIO_NOPULL
PF14.GPIO_ModeDefaultOutputPP=GPIO_MODE_OUTPUT_OD
PF14.GPIO_PuPd=GPIO_PULLUP
PF14.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PF14.Locked=true
PF14.PinState=GPIO_PIN_SET
@@ -146,6 +154,7 @@ ProjectManager.DeletePrevious=true
ProjectManager.DeviceId=STM32F407ZGTx
ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.28.3
ProjectManager.FreePins=false
ProjectManager.FreePinsContext=
ProjectManager.HalAssertFull=false
ProjectManager.HeapSize=0x200
ProjectManager.KeepUserCode=true
+2 -2
View File
@@ -71,8 +71,8 @@ void MX_GPIO_Init(void)
/*Configure GPIO pins : SDA_Pin SCL_Pin */
GPIO_InitStruct.Pin = SDA_Pin|SCL_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);
+1 -1
View File
@@ -6,6 +6,7 @@
#include "hal_i2c.h"
#include "hal_can.h"
#include "hal_led.h"
#include "hal_soft_i2c.h"
#include "freertos.h"
#include "pid.h"
#include <stdio.h>
@@ -13,7 +14,6 @@
#include "cmsis_os.h"
#include "projdefs.h"
#include "wit_c_sdk.h"
#include "soft_i2c.h"
void app_init(void);
void sensor_task(void);
void control_task(void);
+4 -13
View File
@@ -4,11 +4,6 @@
static led_t led0, led1, led2;
soft_i2c_t i2c;
void app_init(void)
{
// 初始化硬件抽象层
@@ -16,6 +11,7 @@ void app_init(void)
hal_led_init_all();
hal_can_init_all();
hal_gpio_init_all();
hal_soft_i2c_init_all();
// 初始化外设
led_init(&led0, 0);
led_init(&led1, 1);
@@ -23,15 +19,10 @@ void app_init(void)
led_on(&led0);
led_on(&led1);
led_on(&led2);
soft_i2c_config_t config = {
.sda_gpio_ch = 0, /* 根据实际硬件修改 */
.scl_gpio_ch = 1,
.delay_us = 5, /* 100kHz */
.open_drain = true
};
soft_i2c_init(&i2c, &config);
led_off(&led0);
printf("Hardware initialized\r\n");
printf("[app_init] Hardware initialized\r\n");
}
+27 -24
View File
@@ -93,19 +93,20 @@ float PID_Compute(PID_t *pid, float target, float actual)
void control_task(void)
{
int32_t yaw_cmd, pitch_cmd;
float roll_comp, pitch_comp;
// int32_t yaw_cmd, pitch_cmd;
// float roll_comp, pitch_comp;
printf("[gimbal] starting stabilization control\n");
printf("[control_task] starting stabilization control\n");
/* 1. 初始化 PID 参数 */
/* Roll 轴 PID:控制 Yaw 电机来补偿 Roll 倾斜 */
PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f); /* 最大补偿 180 度 */
// PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f); /* 最大补偿 180 度 */
/* Pitch 轴 PID:控制 Pitch 电机补偿 Pitch 倾斜 */
PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f); /* 最大补偿 90 度 */
// PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f); /* 最大补偿 90 度 */
/* 2. 初始化电机 */
// osDelay(pdMS_TO_TICKS(1000));
mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
@@ -113,38 +114,40 @@ void control_task(void)
mf4010v2_close(&motor_yaw);
mf4010v2_close(&motor_pitch);
printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw));
printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch));
/* 等待 IMU 稳定 */
osDelay(pdMS_TO_TICKS(1000));
osDelay(pdMS_TO_TICKS(5000));
/* 4. 使能电机 */
mf4010v2_run(&motor_yaw);
mf4010v2_run(&motor_pitch);
printf("[gimbal] stabilization enabled\n");
printf("[control_task] stabilization enabled\n");
/* 主控制循环 (100Hz) */
while (1) {
/* 读取 IMU 数据 */
float current_roll = fAngle[0]; /* Roll 角 */
float current_pitch = fAngle[1]; /* Pitch 角 */
// float current_roll = fAngle[0]; /* Roll 角 */
// float current_pitch = fAngle[1]; /* Pitch 角 */
/* PID 计算补偿量 */
/* 目标:Roll=0, Pitch=0 */
roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll);
pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch);
// /* PID 计算补偿量 */
// /* 目标:Roll=0, Pitch=0 */
// roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll);
// pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch);
/* 运动学映射:补偿量 -> 电机角度 */
/* 对于 Pitch-Yaw 云台:
* - Pitch 倾斜 → Pitch 电机反向补偿
* - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿(简化为 Yaw 电机)
*/
yaw_cmd = (int32_t)(roll_comp * 100.0f); /* 转为 0.01 度 */
pitch_cmd = (int32_t)(pitch_comp * 100.0f);
// /* 运动学映射:补偿量 -> 电机角度 */
// /* 对于 Pitch-Yaw 云台:
// * - Pitch 倾斜 → Pitch 电机反向补偿
// * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿(简化为 Yaw 电机)
// */
// yaw_cmd = (int32_t)(roll_comp * 100.0f); /* 转为 0.01 度 */
// pitch_cmd = (int32_t)(pitch_comp * 100.0f);
/* 发送位置指令到电机 */
/* 使用增量位置控制,直接叠加补偿量 */
mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd);
mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd);
// /* 发送位置指令到电机 */
// /* 使用增量位置控制,直接叠加补偿量 */
// mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd);
// mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd);
/* 100Hz 控制循环 */
osDelay(pdMS_TO_TICKS(10));
-83
View File
@@ -1,83 +0,0 @@
/**
* @file sensor_i2c_port.c
* @brief Sensor I2C 端口适配层(基于 Soft I2C)
* @note 替换原有的 IIC_* 函数,使用 Soft I2C 实现
*/
#include "soft_i2c.h"
#include <stdint.h>
/* ============================================================================
* 配置区域
* ============================================================================ */
/**
* @brief Soft I2C 实例配置
* @note 根据实际硬件连接修改引脚
*/
#define SOFT_I2C_SDA_GPIO 0 /* SDA 引脚通道 */
#define SOFT_I2C_SCL_GPIO 1 /* SCL 引脚通道 */
#define SOFT_I2C_DELAY_US 5 /* 5us = 约 100kHz */
/* 全局 Soft I2C 实例 */
static soft_i2c_t g_sensor_i2c;
static int g_i2c_initialized = 0;
/* ============================================================================
* I2C 底层函数实现(替换原有的 IIC_* 函数)
* ============================================================================ */
/**
* @brief 初始化 Soft I2C(单次调用)
*/
static void SoftI2C_EnsureInit(void)
{
if (!g_i2c_initialized) {
soft_i2c_config_t config = {
.sda_gpio_ch = SOFT_I2C_SDA_GPIO,
.scl_gpio_ch = SOFT_I2C_SCL_GPIO,
.delay_us = SOFT_I2C_DELAY_US,
.open_drain = true
};
soft_i2c_init(&g_sensor_i2c, &config);
g_i2c_initialized = 1;
}
}
/**
* @brief 写入多个字节(带寄存器地址)
* @param dev: 设备地址(7 位)
* @param reg: 寄存器地址
* @param data: 数据缓冲区
* @param length: 数据长度
* @retval 成功返回 1,失败返回 0
*/
int32_t IICwriteBytes(uint8_t dev, uint8_t reg, uint8_t* data, uint32_t length)
{
int ret;
SoftI2C_EnsureInit();
ret = soft_i2c_mem_write(&g_sensor_i2c, dev, reg, 1, data, length, 0);
return (ret == SOFT_I2C_OK) ? 1 : 0;
}
/**
* @brief 读取多个字节(带寄存器地址)
* @param dev: 设备地址(7 位)
* @param reg: 寄存器地址
* @param data: 数据缓冲区
* @param length: 数据长度
* @retval 成功返回 1,失败返回 0
*/
int32_t IICreadBytes(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length)
{
int ret;
SoftI2C_EnsureInit();
ret = soft_i2c_mem_read(&g_sensor_i2c, dev, reg, 1, data, length, 0);
return (ret == SOFT_I2C_OK) ? 1 : 0;
}
-40
View File
@@ -1,40 +0,0 @@
/**
* @file sensor_i2c_port.h
* @brief Sensor I2C 端口适配层头文件
* @note 替换原有的 IIC_* 函数声明
*/
#ifndef __SENSOR_I2C_PORT_H__
#define __SENSOR_I2C_PORT_H__
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 写入多个字节(带寄存器地址)
* @param dev: 设备地址(7 位)
* @param reg: 寄存器地址
* @param data: 数据缓冲区
* @param length: 数据长度
* @retval 成功返回 1,失败返回 0
*/
int32_t IICwriteBytes(uint8_t dev, uint8_t reg, uint8_t* data, uint32_t length);
/**
* @brief 读取多个字节(带寄存器地址)
* @param dev: 设备地址(7 位)
* @param reg: 寄存器地址
* @param data: 数据缓冲区
* @param length: 数据长度
* @retval 成功返回 1,失败返回 0
*/
int32_t IICreadBytes(uint8_t dev, uint8_t reg, uint8_t *data, uint32_t length);
#ifdef __cplusplus
}
#endif
#endif /* __SENSOR_I2C_PORT_H__ */
+152 -65
View File
@@ -1,7 +1,7 @@
#include <wit_c_sdk.h>
#include <app.h>
#include "app.h"
#include "cmsis_os2.h"
#include "sensor_i2c_port.h"
#include "soft_i2c.h"
static soft_i2c_t si2c;
#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
@@ -9,6 +9,150 @@
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
static void CmdProcess(void);
static void AutoScanSensor(void);
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);
int32_t IICreadBytes(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
int ret = soft_i2c_mem_read(
&si2c,
ucAddr,
ucReg,
1,
p_ucVal,
uiLen,
200
);
return ret == 0 ? 1 : 0;
}
int32_t IICwriteBytes (uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
int ret = soft_i2c_mem_write(
&si2c,
ucAddr,
ucReg,
1,
p_ucVal,
uiLen,
200
);
return ret == 0 ? 1 : 0;
}
void sensor_task(void)
{
float fAcc[3], fGyro[3], fAngle[3];
int i;
int ret;
ret = soft_i2c_init(&si2c, 0);
printf("[sensor] soft_i2c_init = %d\r\n", ret);
WitInit(WIT_PROTOCOL_I2C, 0x50);
WitI2cFuncRegister(IICwriteBytes, IICreadBytes);
WitRegisterCallBack(CopeSensorData);
WitDelayMsRegister(Delayms);
AutoScanSensor();
while (1)
{
WitReadReg(AX, 12);
Delayms(5);
CmdProcess();
if(s_cDataUpdate)
{
for(i = 0; i < 3; i++)
{
fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
}
if(s_cDataUpdate & ACC_UPDATE)
{
printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{
printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{
printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
s_cDataUpdate &= ~MAG_UPDATE;
}
}
}
}
void CopeCmdData(unsigned char ucData)
{
static unsigned char s_ucData[50], s_ucRxCnt = 0;
s_ucData[s_ucRxCnt++] = ucData;
if(s_ucRxCnt<3)return; //Less than three data returned
if(s_ucRxCnt >= 50) s_ucRxCnt = 0;
if(s_ucRxCnt >= 3)
{
if((s_ucData[1] == '\r') && (s_ucData[2] == '\n'))
{
s_cCmd = s_ucData[0];
memset(s_ucData,0,50);//
s_ucRxCnt = 0;
}
else
{
s_ucData[0] = s_ucData[1];
s_ucData[1] = s_ucData[2];
s_ucRxCnt = 2;
}
}
}
static void CmdProcess(void)
{
switch(s_cCmd)
{
case 'a':
if(WitStartAccCali() != WIT_HAL_OK)
printf("\r\nSet AccCali Error\r\n");
break;
case 'm':
if(WitStartMagCali() != WIT_HAL_OK)
printf("\r\nStart MagCali Error\r\n");
break;
case 'e':
if(WitStopMagCali() != WIT_HAL_OK)
printf("\r\nEnd MagCali Error\r\n");
break;
case 'u':
if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK)
printf("\r\nSet Bandwidth Error\r\n");
break;
case 'U':
if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK)
printf("\r\nSet Bandwidth Error\r\n");
break;
case 'B':
if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK)
printf("\r\nSet Baud Error\r\n");
break;
case 'b':
if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
printf("\r\nSet Baud Error\r\n");
break;
default : return ;
}
s_cCmd = 0xff;
}
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
{
@@ -44,19 +188,12 @@ static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
uiReg++;
}
}
static void ShowHelp(void)
static void Delayms(uint16_t ucMs)
{
printf("\r\n************************ WIT_SDK_DEMO ************************");
printf("\r\n************************ HELP ************************\r\n");
printf("UART SEND:a\\r\\n Acceleration calibration.\r\n");
printf("UART SEND:m\\r\\n Magnetic field calibration,After calibration send: e\\r\\n to indicate the end\r\n");
printf("UART SEND:U\\r\\n Bandwidth increase.\r\n");
printf("UART SEND:u\\r\\n Bandwidth reduction.\r\n");
printf("UART SEND:B\\r\\n Baud rate increased to 115200.\r\n");
printf("UART SEND:b\\r\\n Baud rate reduction to 9600.\r\n");
printf("UART SEND:h\\r\\n help.\r\n");
printf("******************************************************************************\r\n");
osDelay(pdMS_TO_TICKS(ucMs));
}
static void AutoScanSensor(void)
{
int i, iRetry;
@@ -69,11 +206,10 @@ static void AutoScanSensor(void)
{
s_cDataUpdate = 0;
WitReadReg(AX, 3);
osDelay(5);
Delayms(5);
if(s_cDataUpdate != 0)
{
printf("find %02X addr sensor\r\n", i);
ShowHelp();
return ;
}
iRetry--;
@@ -82,52 +218,3 @@ static void AutoScanSensor(void)
printf("can not find sensor\r\n");
printf("please check your connection\r\n");
}
static void Delayms(uint16_t ucMs)
{
osDelay(pdMS_TO_TICKS(ucMs));
}
float fAcc[3], fGyro[3], fAngle[3];
void sensor_task(void)
{
int i;
WitInit(WIT_PROTOCOL_I2C, 0x50);
WitI2cFuncRegister(IICwriteBytes, IICreadBytes);
WitRegisterCallBack(CopeSensorData);
WitDelayMsRegister(Delayms);
AutoScanSensor();
while (1) {
WitReadReg(AX, 12);
vTaskDelay(pdMS_TO_TICKS(500));
if(s_cDataUpdate)
{
for(i = 0; i < 3; i++)
{
fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
}
if(s_cDataUpdate & ACC_UPDATE)
{
printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{
printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{
printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
s_cDataUpdate &= ~MAG_UPDATE;
}
}
}
}
+1 -1
View File
@@ -1,4 +1,4 @@
#include "bsp_i2c.h"
#include "bsp_i2c.h"
#include "i2c.h"
static bsp_i2c_t i2cs[] = {
+18
View File
@@ -0,0 +1,18 @@
#include "bsp_soft_i2c.h"
#include "main.h"
static bsp_soft_i2c_t soft_i2cs[] = {
{
.scl_port = GPIOF,
.scl_pin = SCL_Pin,
.sda_port = GPIOF,
.sda_pin = SDA_Pin,
}
};
bsp_soft_i2c_t* bsp_soft_i2c_get_handle(int ch) {
if (ch < 0 || ch >= (int)(sizeof(soft_i2cs) / sizeof(bsp_soft_i2c_t))) {
return NULL;
}
return &soft_i2cs[ch];
}
+14
View File
@@ -0,0 +1,14 @@
#ifndef BSP_SOFT_I2C_H
#define BSP_SOFT_I2C_H
#include "stm32f407xx.h"
typedef struct {
GPIO_TypeDef *scl_port, *sda_port;
uint16_t scl_pin, sda_pin;
} bsp_soft_i2c_t;
bsp_soft_i2c_t* bsp_soft_i2c_get_handle(int ch);
#endif
+65
View File
@@ -0,0 +1,65 @@
set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_PROCESSOR arm)
set(CMAKE_C_COMPILER_ID Clang)
set(CMAKE_CXX_COMPILER_ID Clang)
# Some default llvm settings
set(TOOLCHAIN_PREFIX starm-)
set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}clang)
set(CMAKE_ASM_COMPILER ${CMAKE_C_COMPILER})
set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}clang++)
set(CMAKE_LINKER ${TOOLCHAIN_PREFIX}clang)
set(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}objcopy)
set(CMAKE_SIZE ${TOOLCHAIN_PREFIX}size)
set(CMAKE_EXECUTABLE_SUFFIX_ASM ".elf")
set(CMAKE_EXECUTABLE_SUFFIX_C ".elf")
set(CMAKE_EXECUTABLE_SUFFIX_CXX ".elf")
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
# STARM_TOOLCHAIN_CONFIG allows you to choose the toolchain configuration.
# Possible values are:
# "STARM_HYBRID" : Hybrid configuration using starm-clang Assemler and Compiler and GNU Linker
# "STARM_NEWLIB" : starm-clang toolchain with NEWLIB C library
# "STARM_PICOLIBC" : starm-clang toolchain with PICOLIBC C library
set(STARM_TOOLCHAIN_CONFIG "STARM_PICOLIBC")
if(STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_HYBRID")
set(TOOLCHAIN_MULTILIBS "--multi-lib-config=\"$ENV{CLANG_GCC_CMSIS_COMPILER}/multilib.gnu_tools_for_stm32.yaml\" --gcc-toolchain=\"$ENV{GCC_TOOLCHAIN_ROOT}/..\"")
elseif (STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_NEWLIB")
set(TOOLCHAIN_MULTILIBS "--config=newlib.cfg")
endif()
# MCU specific flags
set(TARGET_FLAGS "-mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard ${TOOLCHAIN_MULTILIBS}")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${TARGET_FLAGS}")
set(CMAKE_ASM_FLAGS "${CMAKE_C_FLAGS} -x assembler-with-cpp -MP")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -fdata-sections -ffunction-sections -ftls-model=local-exec -fstack-usage")
set(CMAKE_C_FLAGS_DEBUG "-Og -g3")
set(CMAKE_C_FLAGS_RELEASE "-Oz -g0")
set(CMAKE_CXX_FLAGS_DEBUG "-Og -g3")
set(CMAKE_CXX_FLAGS_RELEASE "-Oz -g0")
set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fno-exceptions -fno-threadsafe-statics")
set(CMAKE_EXE_LINKER_FLAGS "${TARGET_FLAGS}")
if (STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_HYBRID")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --gcc-specs=nano.specs")
set(TOOLCHAIN_LINK_LIBRARIES "m")
elseif(STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_NEWLIB")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -lcrt0-nosys")
elseif(STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_PICOLIBC")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -lcrt0-hosted -z norelro")
endif()
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -T \"${CMAKE_SOURCE_DIR}/STM32F407XX_FLASH.ld\"")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-Map=${CMAKE_PROJECT_NAME}.map -Wl,--gc-sections")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--print-memory-usage ")
+51 -50
View File
@@ -11,70 +11,70 @@ set(MX_Defines_Syms
# STM32CubeMX generated include paths
set(MX_Include_Dirs
${CMAKE_SOURCE_DIR}/Core/Inc
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Inc
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/include
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
${CMAKE_SOURCE_DIR}/Drivers/CMSIS/Device/ST/STM32F4xx/Include
${CMAKE_SOURCE_DIR}/Drivers/CMSIS/Include
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Inc
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Inc
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/include
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Device/ST/STM32F4xx/Include
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Include
)
# STM32CubeMX generated application sources
set(MX_Application_Src
${CMAKE_SOURCE_DIR}/Core/Src/main.c
${CMAKE_SOURCE_DIR}/Core/Src/gpio.c
${CMAKE_SOURCE_DIR}/Core/Src/freertos.c
${CMAKE_SOURCE_DIR}/Core/Src/can.c
${CMAKE_SOURCE_DIR}/Core/Src/i2c.c
${CMAKE_SOURCE_DIR}/Core/Src/usart.c
${CMAKE_SOURCE_DIR}/Core/Src/stm32f4xx_it.c
${CMAKE_SOURCE_DIR}/Core/Src/stm32f4xx_hal_msp.c
${CMAKE_SOURCE_DIR}/Core/Src/stm32f4xx_hal_timebase_tim.c
${CMAKE_SOURCE_DIR}/Core/Src/sysmem.c
${CMAKE_SOURCE_DIR}/Core/Src/syscalls.c
${CMAKE_SOURCE_DIR}/startup_stm32f407xx.s
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/main.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/i2c.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usart.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_it.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_timebase_tim.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/sysmem.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/syscalls.c
${CMAKE_CURRENT_SOURCE_DIR}/../../startup_stm32f407xx.s
)
# STM32 HAL/LL Drivers
set(STM32_Drivers_Src
${CMAKE_SOURCE_DIR}/Core/Src/system_stm32f4xx.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c
${CMAKE_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/system_stm32f4xx.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
)
# Drivers Midllewares
set(FreeRTOS_Src
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/croutine.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/event_groups.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/list.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/queue.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/tasks.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/timers.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c
${CMAKE_SOURCE_DIR}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/croutine.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/list.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/queue.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/tasks.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/timers.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c
)
# Link directories setup
@@ -84,6 +84,7 @@ set(MX_LINK_DIRS
# Project static libraries
set(MX_LINK_LIBS
STM32_Drivers
${TOOLCHAIN_LINK_LIBRARIES}
FreeRTOS␍
)
# Interface library for includes and symbols
-455
View File
@@ -1,455 +0,0 @@
# 二轴云台开发指南
## 1. 云台系统概述
### 1.1 系统组成
二轴云台系统由以下部分组成:
- **STM32F407 主控** - 运行姿态解算和电机控制算法
- **JY-901 姿态传感器** - 提供云台姿态反馈(加速度计 + 陀螺仪 + 角度)
- **两个无刷电机** - Pitch(俯仰)和 Roll(横滚)轴
- **无刷电机驱动器** - 如 SimpleFOC、BLDrive 等
- **遥控器/地面站** - 输入控制指令
### 1.2 控制原理
```
┌─────────────┐ ┌──────────┐ ┌─────────┐ ┌──────┐
│ 遥控器输入 │───>│ 姿态解算 │───>│ PID 控制 │───>│ 电机 │
│ (目标角度) │ │ (JY-901) │ │ 算法 │ │ 驱动 │
└─────────────┘ └──────────┘ └─────────┘ └──────┘
↑ │
│────────── 反馈 ──────────────┘
```
---
## 2. 硬件连接
### 2.1 JY-901 传感器连接
| JY-901 | STM32F407 | 说明 |
|--------|-----------|------|
| VCC | 3.3V / 5V | 供电 |
| GND | GND | 地 |
| SDA | PF0 (I2C2_SDA) | I2C 数据 |
| SCL | PF1 (I2C2_SCL) | I2C 时钟 |
### 2.2 电机驱动器连接(以 SimpleFOC 为例)
**Pitch 轴(俯仰)**:
| 信号 | STM32 引脚 | 定时器 |
|------|-----------|--------|
| PWM_U | PA8 | TIM1_CH1 |
| PWM_V | PA9 | TIM1_CH2 |
| PWM_W | PA10 | TIM1_CH3 |
| EN | PB12 | GPIO |
**Roll 轴(横滚)**:
| 信号 | STM32 引脚 | 定时器 |
|------|-----------|--------|
| PWM_U | PA6 | TIM3_CH1 |
| PWM_V | PA7 | TIM3_CH2 |
| PWM_W | PB0 | TIM3_CH3 |
| EN | PB13 | GPIO |
### 2.3 遥控器接收机连接
| 通道 | 功能 | STM32 引脚 | 定时器 |
|------|------|-----------|--------|
| CH1 | Pitch 控制 | PA0 | TIM2_CH1 |
| CH2 | Roll 控制 | PA1 | TIM2_CH2 |
| CH3 | 模式切换 | PA2 | TIM2_CH3 |
---
## 3. 软件架构
### 3.1 系统架构
```
User/
├── Sensors/
│ ├── j901p.c/h/reg.h # JY-901 驱动
│ └── j901p_hal.c/h # HAL 适配层
├── Gimbal/
│ ├── gimbal_control.c/h # ⭐ 云台控制主逻辑
│ ├── pid.c/h # ⭐ PID 控制器
│ └── motor_driver.c/h # ⭐ 电机驱动接口
├── RC/
│ └── rc_receiver.c/h # 遥控器接收
└── Utils/
└── filter.c/h # 滤波算法(低通、互补滤波)
```
### 3.2 FreeRTOS 任务设计
```c
// 任务优先级定义
#define TASK_PRIORITY_GIMBAL osPriorityHigh // 云台控制(最高)
#define TASK_PRIORITY_SENSOR osPriorityNormal // 传感器读取
#define TASK_PRIORITY_RC osPriorityNormal // 遥控接收
#define TASK_PRIORITY_LOG osPriorityLow // 日志输出
// 任务栈大小
#define TASK_STACK_GIMBAL 512 // 云台控制需要较大栈
#define TASK_STACK_SENSOR 256
#define TASK_STACK_RC 256
#define TASK_STACK_LOG 256
```
### 3.3 控制循环
```c
void StartGimbalTask(void *argument)
{
// 初始化
Gimbal_Init();
PID_Init(&pitchPID, Kp, Ki, Kd);
PID_Init(&rollPID, Kp, Ki, Kd);
float targetPitch = 0.0f;
float targetRoll = 0.0f;
for(;;)
{
// 1. 读取当前姿态(从传感器任务获取)
float currentPitch = gimbalState.pitch;
float currentRoll = gimbalState.roll;
// 2. 读取遥控器输入
if (rcConnected) {
targetPitch = rcChannels[0] * 45.0f; // -45° ~ +45°
targetRoll = rcChannels[1] * 45.0f;
}
// 3. PID 计算
float pitchOutput = PID_Calculate(&pitchPID, targetPitch, currentPitch);
float rollOutput = PID_Calculate(&rollPID, targetRoll, currentRoll);
// 4. 输出到电机
Motor_SetPitch(pitchOutput);
Motor_SetRoll(rollOutput);
// 5. 控制循环:1kHz(1ms)
osDelay(1);
}
}
```
---
## 4. PID 控制器实现
### 4.1 PID 结构定义
```c
typedef struct {
float Kp; // 比例系数
float Ki; // 积分系数
float Kd; // 微分系数
float integral; // 积分累积
float prevError; // 上一次误差
float outputMax; // 输出限幅
// 抗积分饱和
float integralMax;
} PID_TypeDef;
```
### 4.2 PID 计算函数
```c
float PID_Calculate(PID_TypeDef *pid, float target, float current)
{
float error = target - current;
// 比例项
float pTerm = pid->Kp * error;
// 积分项(带抗饱和)
pid->integral += error;
if (pid->integral > pid->integralMax) {
pid->integral = pid->integralMax;
} else if (pid->integral < -pid->integralMax) {
pid->integral = -pid->integralMax;
}
float iTerm = pid->Ki * pid->integral;
// 微分项
float dTerm = pid->Kd * (error - pid->prevError);
pid->prevError = error;
// 输出限幅
float output = pTerm + iTerm + dTerm;
if (output > pid->outputMax) {
output = pid->outputMax;
} else if (output < -pid->outputMax) {
output = -pid->outputMax;
}
return output;
}
```
### 4.3 参数整定
**初始参数建议**:
```c
// Pitch 轴(俯仰)
PID_Init(&pitchPID,
0.5f, // Kp - 比例
0.01f, // Ki - 积分
0.1f, // Kd - 微分
1.0f // 输出限幅
);
// Roll 轴(横滚)
PID_Init(&rollPID,
0.5f, // Kp
0.01f, // Ki
0.1f, // Kd
1.0f // 输出限幅
);
```
**整定步骤**:
1. 设置 Ki=0, Kd=0,逐渐增大 Kp 直到系统开始振荡
2. 增大 Kd 抑制振荡
3. 如有稳态误差,加入 Ki
4. 微调三个参数直到响应快速且稳定
---
## 5. 传感器数据处理
### 5.1 数据滤波
**低通滤波**(去除高频噪声):
```c
typedef struct {
float alpha; // 滤波系数 (0~1)
float lastOutput;
} LowPassFilter_TypeDef;
float LowPassFilter(LowPassFilter_TypeDef *filter, float newValue)
{
float output = filter->alpha * newValue +
(1.0f - filter->alpha) * filter->lastOutput;
filter->lastOutput = output;
return output;
}
// 使用示例
LowPassFilter_TypeDef pitchFilter = {.alpha = 0.2f, .lastOutput = 0};
float filteredPitch = LowPassFilter(&pitchFilter, rawPitch);
```
**互补滤波**(融合加速度计和陀螺仪):
```c
// 加速度计计算的角度
float accAngle = atan2(accY, accZ) * 180.0f / 3.14159f;
// 陀螺仪积分的角度
gyroAngle += gyroZ * dt;
// 互补滤波融合
float angle = 0.98f * gyroAngle + 0.02f * accAngle;
```
### 5.2 坐标变换
云台安装时可能需要坐标变换:
```c
// 假设传感器安装旋转了 90 度
float sensorPitch = angleData.Pitch;
float sensorRoll = angleData.Roll;
// 转换到云台坐标系
float gimbalPitch = sensorRoll;
float gimbalRoll = -sensorPitch;
```
---
## 6. 工作模式
### 6.1 模式定义
```c
typedef enum {
GIMBAL_MODE_LOCK = 0, // 锁定模式(保持水平)
GIMBAL_MODE_FOLLOW = 1, // 跟随模式(跟随遥控器)
GIMBAL_MODE_PAN = 2, // 航向跟随(仅横滚锁定)
} GimbalMode_TypeDef;
```
### 6.2 模式切换
```c
void Gimbal_SetMode(GimbalMode_TypeDef mode)
{
switch(mode) {
case GIMBAL_MODE_LOCK:
targetPitch = 0.0f;
targetRoll = 0.0f;
break;
case GIMBAL_MODE_FOLLOW:
// 目标角度由遥控器控制
break;
case GIMBAL_MODE_PAN:
targetRoll = 0.0f; // 横滚锁定
// Pitch 跟随遥控
break;
}
}
```
---
## 7. 调试与测试
### 7.1 串口监控
```c
// 输出云台状态
void Gimbal_PrintStatus(void)
{
char buf[128];
int len = snprintf(buf, sizeof(buf),
"PITCH: %6.2f ROLL: %6.2f | OUT_P: %5.2f OUT_R: %5.2f\r\n",
gimbalState.pitch,
gimbalState.roll,
motorOutput.pitch,
motorOutput.roll);
HAL_UART_Transmit(&huart1, (uint8_t*)buf, len, 50);
}
```
### 7.2 地面站软件
推荐使用:
- **SimpleFOC Studio** - 图形化调参
- **Mission Planner** - 地面站监控
- **Betaflight Configurator** - 云台调参
### 7.3 常见问题
**Q: 云台抖动?**
- 降低 Kp 值
- 增加微分滤波
- 检查机械结构是否松动
**Q: 响应太慢?**
- 增加 Kp
- 提高控制循环频率(从 1kHz 到 2kHz)
- 检查电机驱动电流是否足够
**Q: 有稳态误差?**
- 增加 Ki(注意抗饱和)
- 检查传感器安装是否水平
- 校准传感器零偏
---
## 8. 性能优化
### 8.1 控制循环优化
```c
// 使用高精度定时器
// 1kHz 控制循环
TIM_HandleTypeDef htim_gimbal;
// 配置为 1ms 中断
// 在中断中执行控制算法
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM_GIMBAL) {
Gimbal_ControlLoop(); // 执行控制算法
}
}
```
### 8.2 浮点运算优化
STM32F407 有 FPU,可以使用浮点运算:
```c
// 启用 FPU 和浮点指令
-mcpu=cortex-m4
-mfloat-abi=hard
-mfpu=fpv4-sp-d16
```
### 8.3 内存优化
```c
// 使用定点数代替浮点数(如果需要)
typedef int32_t fix16_t; // 16 位小数
#define FIX16_ONE 65536
fix16_t fix16_mul(fix16_t a, fix16_t b) {
return (a * b) >> 16;
}
```
---
## 9. 项目文件结构示例
```
CloudPlant/
├── User/
│ ├── Gimbal/
│ │ ├── gimbal_control.c/h # 云台主控制
│ │ ├── pid.c/h # PID 控制器
│ │ └── motor_driver.c/h # 电机驱动
│ ├── RC/
│ │ └── rc_receiver.c/h # 遥控接收
│ ├── Sensors/
│ │ └── j901p* # 姿态传感器
│ └── Utils/
│ └── filter.c/h # 滤波算法
├── Core/
│ ├── Src/
│ │ ├── main.c
│ │ ├── freertos.c # FreeRTOS 任务
│ │ └── ...
└── docs/
├── JY901_Sensor_Guide.md
└── 2Axis_Gimbal_Guide.md # 本文档
```
---
## 10. 下一步开发计划
### Phase 1: 基础功能
- [ ] 完成电机驱动接口
- [ ] 实现基本 PID 控制
- [ ] 云台自稳功能
### Phase 2: 遥控功能
- [ ] SBUS/PPM 接收机支持
- [ ] 多模式切换
- [ ] 遥控器校准
### Phase 3: 高级功能
- [ ] 自动跟踪(视觉/激光)
- [ ] 蓝牙/WiFi 配置
- [ ] 日志记录与回放
### Phase 4: 性能优化
- [ ] 自适应 PID
- [ ] 前馈控制
- [ ] 振动抑制算法
---
**文档版本**: v1.0
**创建日期**: 2026-03-30
**作者**: CloudPlant Team
**参考项目**: SimpleFOC, Baseflight, Cleanflight
-273
View File
@@ -1,273 +0,0 @@
# JY-901 姿态传感器使用指南
## 1. 传感器简介
JY-901 是一款 9 轴姿态传感器,集成了:
- **三轴加速度计** (Accelerometer)
- **三轴陀螺仪** (Gyroscope)
- **三轴磁力计** (Magnetometer)
传感器通过 **I2C 接口** 与 STM32 通信,默认 I2C 地址为 `0x50`,支持最大 400kHz 通信速率。
---
## 2. 数据寄存器
### 2.1 寄存器映射
| 寄存器 | 地址 | 说明 | 量程 |
|--------|------|------|------|
| AX, AY, AZ | 0x34-0x36 | 加速度计原始数据 | ±16g |
| GX, GY, GZ | 0x37-0x39 | 陀螺仪原始数据 | ±2000°/s |
| HX, HY, HZ | 0x3A-0x3C | 磁力计原始数据 | - |
| Roll, Pitch, Yaw | 0x3D-0x3F | 解算后的角度 | ±180° |
### 2.2 数据格式
所有寄存器数据均为 **16 位有符号整数** (int16_t),采用小端格式存储。
---
## 3. 数据获取与转换
### 3.1 加速度计 (ACC)
**物理意义**:测量物体在三个轴向上的加速度,单位为重力加速度 g (1g ≈ 9.8m/s²)。
**寄存器读取**:
```c
int16_t ax_raw = sReg[AX]; // X 轴原始数据
int16_t ay_raw = sReg[AY]; // Y 轴原始数据
int16_t az_raw = sReg[AZ]; // Z 轴原始数据
```
**转换公式**:
```
加速度 (g) = 原始值 / 32768 × 16
```
**代码实现**:
```c
// 方法 1:浮点数输出(需要启用浮点 printf 支持)
float acc_x = ax_raw / 32768.0f * 16.0f;
// 方法 2:定点数输出(推荐,节省空间)
int32_t acc_scaled = (ax_raw * 16 * 1000) / 32768; // 放大 1000 倍
int32_t acc_int = acc_scaled / 1000; // 整数部分
int32_t acc_frac = acc_scaled % 1000; // 小数部分
```
**典型值**:
- 静止水平放置时:AX≈0, AY≈0, AZ≈1g (1000mg)
- 加速度计可以用于检测设备的倾斜角度和运动状态
---
### 3.2 陀螺仪 (GYRO)
**物理意义**:测量物体绕三个轴旋转的角速度,单位为度每秒 (°/s 或 dps)。
**寄存器读取**:
```c
int16_t gx_raw = sReg[GX]; // X 轴原始数据
int16_t gy_raw = sReg[GY]; // Y 轴原始数据
int16_t gz_raw = sReg[GZ]; // Z 轴原始数据
```
**转换公式**:
```
角速度 (°/s) = 原始值 / 32768 × 2000
```
**代码实现**:
```c
// 方法 1:浮点数输出
float gyro_x = gx_raw / 32768.0f * 2000.0f;
// 方法 2:定点数输出(推荐)
int32_t gyro_scaled = (gx_raw * 2000 * 1000) / 32768;
int32_t gyro_int = gyro_scaled / 1000;
int32_t gyro_frac = gyro_scaled % 1000;
```
**典型值**:
- 静止时:GX≈0, GY≈0, GZ≈0(可能有少量零偏)
- 陀螺仪可以用于检测设备的旋转运动和角振动
**注意**:陀螺仪存在零漂,长时间积分会产生累积误差,通常需要与加速度计融合使用。
---
### 3.3 角度 (ANGLE)
**物理意义**:传感器通过内部姿态解算算法(互补滤波或卡尔曼滤波)计算出的姿态角,单位为度 (°)。
**寄存器读取**:
```c
int16_t roll_raw = sReg[Roll]; // 横滚角
int16_t pitch_raw = sReg[Pitch]; // 俯仰角
int16_t yaw_raw = sReg[Yaw]; // 航向角
```
**转换公式**:
```
角度 (°) = 原始值 / 32768 × 180
```
**代码实现**:
```c
// 方法 1:浮点数输出
float roll = roll_raw / 32768.0f * 180.0f;
// 方法 2:定点数输出(推荐)
int32_t angle_scaled = (roll_raw * 180 * 1000) / 32768;
int32_t angle_int = angle_scaled / 1000;
int32_t angle_frac = angle_scaled % 1000;
```
**角度定义**:
| 角度 | 范围 | 说明 |
|------|------|------|
| **Roll (横滚角)** | ±180° | 绕 X 轴旋转的角度 |
| **Pitch (俯仰角)** | ±90° | 绕 Y 轴旋转的角度 |
| **Yaw (航向角)** | ±180° | 绕 Z 轴旋转的角度(电子罗盘) |
**典型应用**:
- Roll/Pitch:用于平衡车、云台稳定、姿态控制
- Yaw:用于航向指示、指南针功能
---
## 4. 软件架构
### 4.1 驱动层
```
User/Sensors/
├── j901p.c/h/reg.h # 官方 SDK
└── j901p_hal.c/h # HAL 硬件 I2C 适配层
```
**关键函数**:
- `J901P_HAL_Init(0x50)` - 初始化传感器
- `WitReadReg(AX, 12)` - 读取 12 个寄存器(AX 到 Yaw)
- `WitRegisterCallBack(CopeSensorData)` - 注册数据更新回调
### 4.2 数据读取流程
```
1. 调用 WitReadReg(AX, 12)
↓
2. I2C 读取 12 个寄存器到缓冲区
↓
3. 解析数据到 sReg[] 数组
↓
4. 触发回调函数 CopeSensorData()
↓
5. 设置数据更新标志 s_cDataUpdate
↓
6. 主任务读取 sReg[] 并转换
```
### 4.3 FreeRTOS 任务
```c
void StartControlTask(void *argument)
{
// 初始化
J901P_HAL_Init(0x50);
WitRegisterCallBack(CopeSensorData);
// 主循环
for(;;)
{
if(s_cDataUpdate)
{
WitReadReg(AX, 12); // 读取数据
// 转换并输出加速度
if(s_cDataUpdate & 0x01) {
// 处理加速度数据
}
// 转换并输出角速度
if(s_cDataUpdate & 0x02) {
// 处理陀螺仪数据
}
// 转换并输出角度
if(s_cDataUpdate & 0x04) {
// 处理角度数据
}
s_cDataUpdate = 0;
osDelay(200); // 200ms 采样率
}
}
}
```
---
## 5. 数据输出示例
### 5.1 串口输出格式
```
ACC: 0.098 0.030 0.968
GYRO: 0.015 -0.023 0.008
ANGLE: 0.125 -0.089 89.992
```
### 5.2 数据解读
**静止水平放置时**:
```
ACC: 0.000 0.000 1.000 # Z 轴为 1g,X/Y 轴为 0
GYRO: 0.000 0.000 0.000 # 静止,角速度为 0
ANGLE: 0.000 0.000 0.000 # 水平,角度为 0
```
**倾斜 45 度时**:
```
ACC: 0.707 0.000 0.707 # X/Z 轴分量
ANGLE: 45.000 0.000 0.000 # Roll 角 45 度
```
---
## 6. 常见问题
### Q1: 数据全为 0?
- 检查 I2C 接线(SDA/SCL 是否接反)
- 检查传感器供电(3.3V 或 5V)
- 确认 I2C 地址是否正确(默认 0x50)
### Q2: 加速度数据异常?
- 确认传感器安装方向
- 检查是否有强烈振动
- 静态时应该有 1g 的重力分量
### Q3: 角度漂移?
- 陀螺仪零漂是正常现象
- 长时间运行后需要重新校准
- 可以使用传感器融合算法优化
### Q4: 浮点数无法打印?
- ARM GCC 默认不支持浮点 printf
- 使用定点数格式(本代码已实现)
- 或启用浮点支持:`-u _printf_float`
---
## 7. 参考资料
- [JY-901 数据手册](http://wit-motion.cn/)
- [STM32 HAL I2C 使用指南](https://www.st.com/)
- [FreeRTOS 官方文档](https://www.freertos.org/)
---
**文档版本**: v1.0
**更新日期**: 2026-03-30
**作者**: CloudPlant Team
-535
View File
@@ -1,535 +0,0 @@
# 电机控制方法指南
## 1. 系统概述
本电机控制系统基于 **STM32F103** 微控制器,采用 **PID 控制算法** 实现对 **MF4010V2 无刷电机** 的精确控制。系统通过 **MPU6050** 姿态传感器获取云台姿态,使用 **CAN 总线** 与电机通信。
### 1.1 系统架构
```
┌─────────────┐ ┌──────────┐ ┌─────────┐ ┌──────────┐
│ MPU6050 │───>│ PID 算法 │───>│ CAN 总线 │───>│MF4010V2 │
│ 姿态传感器 │ │ 控制器 │ │ 通信 │ │ 无刷电机 │
└─────────────┘ └──────────┘ └─────────┘ └──────────┘
I2C 计算 CAN 执行
```
### 1.2 硬件组成
| 组件 | 型号 | 接口 | 说明 |
|------|------|------|------|
| **MCU** | STM32F103xC/xE | - | 主控制器,72MHz |
| **电机** | MF4010V2 | CAN | 无刷力矩电机,ID: 0x141/0x142 |
| **姿态传感器** | MPU6050 | I2C | 6 轴 IMU(加速度计 + 陀螺仪) |
| **CAN 收发器** | TJA1050 | CAN | CAN 总线物理层 |
---
## 2. 控制方法
### 2.1 控制模式
MF4010V2 电机支持多种控制模式:
#### 2.1.1 速度闭环控制(使用)
**命令格式**:
```c
#define COMMAND_SPEED_CONTROL(speedControl)
```
**参数说明**:
- `speedControl`: int32_t,速度控制量
- 分辨率:0.01 DPS/LSB
- 范围:-204800 ~ +204800 (对应 -2048 ~ +2048 DPS)
**使用示例**:
```c
// 设置电机速度为 100 DPS
int32_t speed = 10000; // 100 * 100
uint8_t cmd[8] = COMMAND_SPEED_CONTROL(speed);
mf4010v2_send_command(&motor, cmd, 0);
```
#### 2.1.2 转矩闭环控制
**命令格式**:
```c
#define COMMAND_TORQUE_CONTROL(iqControl)
```
**参数说明**:
- `iqControl`: int16_t,转矩控制量
- 范围:-2048 ~ +2048
#### 2.1.3 位置闭环控制
**单圈位置控制**:
```c
#define COMMAND_SINGLERING_CONTROL_1(spinDirection, angleControl)
```
**增量位置控制**:
```c
#define COMMAND_POSITION_ADD_CONTROL(angleIncrement)
```
**参数说明**:
- `angleControl`: uint32_t,目标角度
- 分辨率:0.01°/LSB (36000 = 360°)
- `spinDirection`: 旋转方向 (0x00 顺时针,0x01 逆时针)
---
### 2.2 PID 控制算法
系统使用 **增量式 PID** 控制算法,具有抗积分饱和和微分滤波功能。
#### 2.2.1 PID 结构定义
```c
typedef struct {
float Kp; // 比例增益
float Ki; // 积分增益
float Kd; // 微分增益
float tau; // 微分低通滤波器时间常数
float limMin; // 输出最小值
float limMax; // 输出最大值
float limMinInt; // 积分最小值
float limMaxInt; // 积分最大值
float T; // 采样时间 (秒)
// 控制器状态
float integrator; // 积分项
float prevError; // 上一次误差
float differentiator; // 微分项
float prevMeasurement; // 上一次测量值
float out; // 控制器输出
} PIDController;
```
#### 2.2.2 PID 参数整定
**默认参数**(针对 MF4010V2 电机):
```c
#define PID_KP1 3.5f // 比例增益
#define PID_KI1 1.2f // 积分增益
#define PID_KD1 0.0f // 微分增益(暂不使用)
#define PID_TAU1 0.02f // 微分滤波时间常数
#define PID_LIM_MIN1 -500.0f // 输出下限
#define PID_LIM_MAX1 500.0f // 输出上限
#define PID_LIM_MIN_INT1 -100.0f // 积分下限
#define PID_LIM_MAX_INT1 100.0f // 积分上限
#define SAMPLE_TIME_S 0.1f // 采样时间 100ms
```
#### 2.2.3 PID 计算流程
```c
// 1. 初始化
PIDController pid;
pid.Kp = 3.5f;
pid.Ki = 1.2f;
pid.Kd = 0.0f;
pid.T = 0.1f;
// ... 其他参数
PIDController_Init(&pid);
// 2. 控制循环中更新
float output = PIDController_Update(&pid, setpoint, measurement);
```
**PID 更新算法**:
```c
float PIDController_Update(PIDController *pid, float setpoint, float measurement)
{
// 计算误差
float error = setpoint - measurement;
// 比例项
float pTerm = pid->Kp * error;
// 积分项(带抗饱和)
pid->integrator += error * pid->T;
if(pid->integrator > pid->limMaxInt)
pid->integrator = pid->limMaxInt;
else if(pid->integrator < pid->limMinInt)
pid->integrator = pid->limMinInt;
float iTerm = pid->Ki * pid->integrator;
// 微分项(带低通滤波)
float dTerm = (pid->Kd * (measurement - pid->prevMeasurement)) /
(pid->T + pid->tau);
// 总输出
pid->out = pTerm + iTerm + dTerm;
// 输出限幅
if(pid->out > pid->limMax)
pid->out = pid->limMax;
else if(pid->out < pid->limMin)
pid->out = pid->limMin;
// 保存状态
pid->prevError = error;
pid->prevMeasurement = measurement;
return pid->out;
}
```
---
## 3. 硬件驱动层
### 3.1 CAN 总线驱动
**硬件连接**:
- CAN_TX: PA12
- CAN_RX: PA11
- 波特率:125kbps (根据配置)
**初始化**:
```c
void can1_init()
{
// GPIO 配置
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);
// CAN 配置
CAN_InitTypeDef can_init_struct;
can_init_struct.CAN_Mode = CAN_Mode_Normal;
can_init_struct.CAN_SJW = CAN_SJW_1tq;
can_init_struct.CAN_BS1 = CAN_BS1_2tq;
can_init_struct.CAN_BS2 = CAN_BS2_3tq;
can_init_struct.CAN_Prescaler = 6; // 波特率配置
CAN_Init(CAN1, &can_init_struct);
// 过滤器配置(接收所有帧)
CAN_FilterInitTypeDef can_filter_init_struct;
can_filter_init_struct.CAN_FilterNumber = 0;
can_filter_init_struct.CAN_FilterMode = CAN_FilterMode_IdMask;
can_filter_init_struct.CAN_FilterScale = CAN_FilterScale_32bit;
can_filter_init_struct.CAN_FilterIdHigh = 0x0000;
can_filter_init_struct.CAN_FilterMaskIdHigh = 0x0000;
can_filter_init_struct.CAN_FilterActivation = ENABLE;
CAN_FilterInit(&can_filter_init_struct);
}
```
**发送函数**:
```c
void can1_send_message(uint16_t id, uint8_t* data, uint8_t len)
{
CanTxMsg TxMessage;
TxMessage.StdId = id;
TxMessage.IDE = CAN_Id_Standard;
TxMessage.RTR = CAN_RTR_Data;
TxMessage.DLC = len;
for(int i = 0; i < len; i++)
TxMessage.Data[i] = data[i];
CAN_Transmit(CAN1, &TxMessage);
}
```
**接收函数**:
```c
CanRxMsg can1_receive_message()
{
CanRxMsg rx;
if(CAN_MessagePending(CAN1)) {
CAN_Receive(CAN1, CAN_FIFO0, &rx);
return rx;
}
// 返回错误标识
rx.StdId = 0xFFFF;
return rx;
}
```
---
### 3.2 MF4010V2 电机驱动
**电机初始化**:
```c
mf4010v2_t motor1 = mf4010v2_init(0x141); // 电机 ID: 0x141
mf4010v2_t motor2 = mf4010v2_init(0x142); // 电机 ID: 0x142
```
**发送命令并接收响应**:
```c
void mf4010v2_send_command(mf4010v2_t* mf4010v2,
uint8_t command[8],
uint8_t create_json)
{
// 保存命令
for(int i = 0; i < 8; i++)
mf4010v2->command[i] = command[i];
// 通过 CAN 发送
can1_send_message(mf4010v2->id, command, 8);
// 接收响应
CanRxMsg rx = can1_receive_message();
for(int i = 0; i < 8; i++)
mf4010v2->return_code[i] = rx.Data[i];
// 可选:创建 JSON 格式数据
if(create_json)
mf4010v2_to_json(mf4010v2);
}
```
**常用命令宏**:
```c
// 电机启动
COMMAND_USEAGE(&motor1, COMMAND_MOTOR_RUNNING);
// 电机停止
COMMAND_USEAGE(&motor1, COMMAND_MOTOR_STOP);
// 电机关闭
COMMAND_USEAGE(&motor1, COMMAND_MOTOR_CLOSE);
// 速度控制
int32_t speed = 5000; // 50 DPS
COMMAND_USEAGE(&motor1, COMMAND_SPEED_CONTROL(speed));
// 读取 PID 参数
COMMAND_USEAGE(&motor1, COMMAND_READ_PID_PARAMS);
// 写入 PID 参数到 RAM
COMMAND_USEAGE(&motor1,
COMMAND_WRITE_PID_PARAMS_RAM(20, 5, 60, 1, 60, 30));
```
**JSON 数据格式**:
```c
// 转换为 JSON 格式
void mf4010v2_to_json(mf4010v2_t* mf4010v2)
{
cJSON* json_root = cJSON_CreateObject();
cJSON_AddNumberToObject(json_root, "id", mf4010v2->id);
cJSON* json_commands = cJSON_CreateArray();
for(int i = 0; i < 8; i++)
cJSON_AddItemToArray(json_commands,
cJSON_CreateNumber(mf4010v2->command[i]));
cJSON_AddItemToObject(json_root, "commands", json_commands);
cJSON* json_return_codes = cJSON_CreateArray();
for(int i = 0; i < 8; i++)
cJSON_AddItemToArray(json_return_codes,
cJSON_CreateNumber(mf4010v2->return_code[i]));
cJSON_AddItemToObject(json_root, "return_codes", json_return_codes);
mf4010v2->json_data = json_root;
}
// 打印 JSON
char* str = mf4010v2_print_json(&motor1);
printf("%s\n", str);
cJSON_free(str);
```
---
### 3.3 MPU6050 姿态传感器
**硬件连接**:
- SCL: PB6 (I2C1_SCL)
- SDA: PB7 (I2C1_SDA)
- AD0: PF12 (软件控制,用于切换地址)
**初始化**:
```c
u8 MPU_Init(void)
{
// GPIO 初始化
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOF, ENABLE);
GPIO_InitTypeDef gpio;
gpio.GPIO_Pin = GPIO_Pin_12;
gpio.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOF, &gpio);
MPU_AD0_CTRL = 0; // 设置 AD0 为低电平,地址 0x68
MPU_IIC_Init(); // 初始化 I2C
MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x80); // 复位
mirco_delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x00); // 唤醒
MPU_Set_Gyro_Fsr(3); // 2000 DPS
MPU_Set_Accel_Fsr(0); // ±2g
MPU_Set_Rate(50); // 50Hz 采样率
// 验证设备 ID
u8 id = MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(id != MPU_ADDR)
return 1; // 设备不存在
return 0;
}
```
**获取姿态数据**:
```c
// 获取加速度
void MPU_Get_Accelerometer(short* ax, short* ay, short* az)
{
*ax = MPU_Read_ACCEL_XOUT_H();
*ay = MPU_Read_ACCEL_YOUT_H();
*az = MPU_Read_ACCEL_ZOUT_H();
}
// 获取角速度
void MPU_Get_Gyroscope(short* gx, short* gy, short* gz)
{
*gx = MPU_Read_GYRO_XOUT_H();
*gy = MPU_Read_GYRO_YOUT_H();
*gz = MPU_Read_GYRO_ZOUT_H();
}
```
---
## 4. 应用层控制逻辑
### 4.1 控制流程
```c
void app_control(uint8_t enable_motor1, float measurement1, float setpoint1,
uint8_t enable_motor2, float measurement2, float setpoint2)
{
// 1. PID 计算
float output_motor1 = PIDController_Update(&pidControllerMotor1,
setpoint1, measurement1);
float output_motor2 = PIDController_Update(&pidControllerMotor2,
setpoint2, measurement2);
// 2. 电机 1 控制
if(enable_motor1) {
int32_t speed1 = (int32_t)(output_motor1 * 100); // 转换为 0.01 DPS 单位
uint8_t cmd1[8] = COMMAND_SPEED_CONTROL(speed1);
mf4010v2_send_command(&motor1, cmd1, 0);
}
// 3. 电机 2 控制(带死区补偿)
if(enable_motor2) {
int32_t speed2 = (int32_t)(output_motor2 * 100);
// 死区补偿:当测量值接近 0 时停止电机
if(measurement2 < 0.8 && measurement2 > -0.8)
speed2 = 0;
uint8_t cmd2[8] = COMMAND_SPEED_CONTROL(speed2);
mf4010v2_send_command(&motor2, cmd2, 0);
}
}
```
### 4.2 控制循环
**主循环结构**:
```c
void main_loop()
{
float target_angle_pitch = 0.0f; // 目标俯仰角 0°
float target_angle_roll = 0.0f; // 目标横滚角 0°
while(1) {
// 1. 读取当前姿态
float current_pitch = get_pitch_angle();
float current_roll = get_roll_angle();
// 2. PID 控制计算
app_control(1, current_pitch, target_angle_pitch,
1, current_roll, target_angle_roll);
// 3. 延时(保持控制周期)
mirco_delay_ms(100); // 10Hz 控制频率
}
}
```
---
## 5. 调试与优化
### 5.1 PID 参数整定步骤
1. **初始化设置**
- 设置 Ki = 0, Kd = 0
- 设置较小的 Kp (如 1.0)
2. **调整比例增益 Kp**
- 逐渐增大 Kp
- 观察系统响应
- 当出现轻微振荡时,减小 20%
3. **调整积分增益 Ki**
- 如果有稳态误差,逐渐增大 Ki
- 注意防止积分饱和
4. **调整微分增益 Kd**(可选)
- 如果系统振荡,可以加入 Kd
- 设置合适的滤波时间常数 tau
5. **验证与微调**
- 测试不同工况
- 微调参数获得最佳性能
### 5.2 常见问题
**Q1: 电机抖动**
- 降低 Kp 值
- 增加速度反馈滤波
- 检查机械连接是否松动
**Q2: 响应太慢**
- 增加 Kp
- 提高控制频率(减小采样时间 T)
- 检查电机驱动电流是否足够
**Q3: 稳态误差**
- 增加 Ki(注意抗饱和)
- 检查传感器零点校准
- 检查机械结构是否水平
**Q4: CAN 通信失败**
- 检查 CAN 波特率配置
- 检查终端电阻(120Ω)
- 检查电机 ID 是否正确
---
## 6. 性能指标
| 指标 | 数值 | 说明 |
|------|------|------|
| **控制频率** | 10Hz | 可调整(SAMPLE_TIME_S) |
| **速度分辨率** | 0.01 DPS | MF4010V2 速度控制精度 |
| **角度分辨率** | 0.01° | 位置控制精度 |
| **CAN 波特率** | 125kbps | 可配置 |
| **PID 输出范围** | ±500 | 可配置(limMin/limMax) |
---
## 7. 参考资料
- [MF4010V2 电机数据手册](#)
- [MPU6050 数据手册](https://invensense.tdk.com/)
- [STM32F103 参考手册](https://www.st.com/)
- [CAN 总线协议规范](https://www.can-cia.org/)
---
**文档版本**: v1.0
**创建日期**: 2026-03-30
**作者**: CloudPlant Team
**基于**: MotorController 项目代码
-240
View File
@@ -1,240 +0,0 @@
# CAN/电机驱动调试指南
## 重构后代码说明
### 代码结构
```
应用层 (freertos.c)
↓
设备驱动层 (mf4010v2.c/h)
↓
HAL 层 (mf4010v2_hal.c/h)
↓
通用 CAN 层 (can.c/h)
↓
STM32 HAL 库
```
### 文件说明
| 文件 | 说明 |
|------|------|
| `Core/Inc/can.h` | CAN 驱动头文件,包含 API 声明和调试宏 |
| `Core/Src/can.c` | CAN 驱动实现,包含详细调试输出 |
| `User/Device/mf4010v2.h` | 电机驱动头文件,包含命令宏和安全函数 |
| `User/Device/mf4010v2.c` | 电机驱动实现 |
| `User/Device/mf4010v2_hal.h` | 电机 HAL 层头文件 |
| `User/Device/mf4010v2_hal.c` | 电机 HAL 层实现 |
| `Core/Src/freertos.c` | FreeRTOS 任务,包含调试输出 |
---
## 调试输出说明
### 1. 系统启动后会输出:
```
========== CAN/Motor Initialization ==========
=== CAN Debug Info ===
State: 0 (Ready), Error: 0x00000000 (No Error)
ESR (Error Status Register): 0x00000000
LEC (Last Error Code): 0 (No Error)
TEC (Tx Error Counter): 0
REC (Rx Error Counter): 0
BOFF (Bus Off): 0, EPVF (Error Passive): 0, EWGF (Error Warning): 0
TSR (Tx Status Register): 0x1C000000
TME (Tx Mailbox Empty): 0x7
CODE (Mailbox Code): 3
Free mailboxes: 3
RFR0: 0x00000000, FIFO0 fill level: 0
BTR: 0x03020005
BRP=5, BS1=2, BS2=3, SJW=3
Calculated bitrate: 1000000 bps
======================
```
### 2. 关键状态说明
#### CAN 状态值
| 值 | 名称 | 说明 |
|----|------|------|
| 0 | Ready | CAN 就绪,可以发送/接收 |
| 1 | Busy | CAN 忙 |
| 2 | Busy TX | 发送忙 |
| 3 | Busy RX | 接收忙 |
| 4 | Busy TX/RX | 发送/接收都忙 |
| 5 | Reset | CAN 复位 |
| 6 | Error | CAN 错误 |
| 7 | Timeout | CAN 超时 |
#### LEC (最后错误代码)
| 值 | 名称 | 说明 |
|----|------|------|
| 0 | No Error | 无错误 |
| 1 | Stuff Error | 填充错误 |
| 2 | Form Error | 帧格式错误 |
| 3 | Ack Error | 应答错误 |
| 4 | Bit1 Error | 发送 1 但检测到 0 |
| 5 | Bit0 Error | 发送 0 但检测到 1 |
| 6 | CRC Error | CRC 校验错误 |
| 7 | No Change | 无变化 |
#### TEC/REC (错误计数)
- **TEC** (发送错误计数): 发送失败时增加,>255 进入 Bus Off 状态
- **REC** (接收错误计数): 接收失败时增加,>127 进入 Error Passive 状态,>255 进入 Bus Off 状态
---
## 常见问题排查
### 问题 1: CAN State 不是 Ready (0)
**症状**: `CAN State: 2 (Busy)` 或其他非 0 值
**可能原因**:
1. CAN 总线被占用或有持续的主导电平
2. 上一次发送未完成,邮箱被占用
3. CAN 控制器处于错误状态
**解决方法**:
1. 检查 CAN_H/CAN_L 接线是否正确
2. 检查电机是否上电
3. 检查终端电阻是否正确连接
4. 重启系统让 CAN 重新初始化
---
### 问题 2: LEC 显示错误代码
**症状**: `LEC: X (XXX Error)`
**不同错误的含义**:
#### LEC=2 (Form Error)
- **原因**: 帧格式错误,通常是波特率不匹配
- **排查**:
1. 确认电机 CAN 波特率是否为 1Mbps
2. 检查 CAN 接线
3. 检查终端电阻
#### LEC=3 (Ack Error)
- **原因**: 没有收到应答
- **排查**:
1. 电机未上电
2. CAN_H/CAN_L 接反
3. 缺少终端电阻
#### LEC=5 (Bit0 Error)
- **原因**: 发送 dominant (0) 但检测到 recessive (1)
- **排查**:
1. CAN 总线开路
2. 电机未连接
---
### 问题 3: TEC/REC 值很高
**症状**: `TEC=XXX` 或 `REC=XXX` (大于 0)
**可能原因**:
1. 通信持续失败
2. 总线噪声干扰
3. 接线问题
**解决方法**:
1. 检查 CAN 接线
2. 检查地线连接
3. 添加/检查终端电阻
---
### 问题 4: Motor command failed
**症状**: `Error: Motor command failed with status -1`
**状态码说明**:
| 值 | 含义 |
|----|------|
| -1 | 超时 (MF4010_ERR_TIMEOUT) |
| -2 | 无效 CAN ID (MF4010_ERR_INVALID_ID) |
| -3 | 无响应 (MF4010_ERR_NO_RESPONSE) |
| -4 | 无效参数 (MF4010_ERR_INVALID_PARAM) |
| -5 | CAN 总线关闭 (MF4010_ERR_BUS_OFF) |
| -6 | CRC 错误 (MF4010_ERR_CRC) |
| -7 | 硬件错误 (MF4010_ERR_HARDWARE) |
---
## 硬件检查清单
### 接线检查
```
STM32F407 MF4010V2 电机
─────────────────────────────────
PB13 (CAN2_TX) ──── CAN_H (黄/绿)
PB12 (CAN2_RX) ──── CAN_L (白)
GND ──── GND
```
### 终端电阻
- 在 CAN_H 和 CAN_L 之间并联 120Ω 电阻
- 如果电机内部已有终端电阻,可以不加
### 电压测量 (上电状态)
| 测量点 | 正常值 |
|--------|--------|
| CAN_H 对地 | 2.5-3.5V |
| CAN_L 对地 | 1.5-2.5V |
| CAN_H - CAN_L | 0V (隐性) |
### 电阻测量 (断电状态)
| 测量点 | 正常值 |
|--------|--------|
| CAN_H - CAN_L | 60-120Ω |
---
## 调试流程
### 第一步:检查 CAN 初始化输出
1. 确认 `State: 0 (Ready)`
2. 确认 `Error: 0x00000000 (No Error)`
3. 确认 `Calculated bitrate: 1000000 bps`
### 第二步:检查发送过程
1. 观察 `Sending CAN frame: ID=0x141, Data=80 00 00 00 00 00 00 00`
2. 如果长时间超时,检查硬件
### 第三步:分析错误
1. 查看 LEC 值判断错误类型
2. 查看 TEC/REC 判断错误严重程度
3. 根据错误类型进行针对性排查
---
## 启用详细调试
### 启用 CAN 详细调试
在 `Core/Inc/can.h` 中:
```c
#define CAN_DEBUG 1 // 启用调试输出
#define CAN_DEBUG_VERBOSE 1 // 启用最详细的调试信息
```
### 启用 HAL 层调试
在 `User/Device/mf4010v2_hal.h` 中:
```c
#define MF4010V2_HAL_DEBUG 1 // 启用 HAL 层调试输出
```
---
## 联系支持
如果问题仍然无法解决,请提供:
1. 完整的串口输出
2. CAN_PrintDebugInfo 的输出
3. 硬件接线照片
4. 万用表测量结果
-413
View File
@@ -1,413 +0,0 @@
# 📘 嵌入式项目开发规范(STM32 + 分层架构)
# Embedded Project Development Guide
## 1. 设计目标
本项目遵循以下核心原则:
- 分层设计(Layered Architecture)
- 解耦(Decoupling)
- 可移植性(Portability)
- 可维护性(Maintainability)
---
## 2. 工程目录规范
```
project/
├── app/ # 应用层(业务逻辑)
├── modules/ # 功能模块(设备 / 算法)
├── interfaces/ # 硬件抽象接口(关键)
├── bsp/ # 板级支持包(硬件实现)
├── 3rdparty/ # 第三方库(不修改源码)
├── os/ # 操作系统(FreeRTOS等)
├── utils/ # 通用工具
├── Core/ # STM32CubeMX生成(禁止污染)
├── Drivers/ # HAL/CMSIS(厂商提供)
```
---
## 3. 分层说明(必须遵守)
### 3.1 层级关系(单向依赖)
```
app
↓
modules
↓
interfaces
↓
bsp
↓
HAL / Hardware
````
### ❗ 严禁反向依赖:
- modules ❌ 依赖 HAL
- app ❌ 直接调用 bsp
- modules ❌ include Core 代码
---
## 4. 各层职责
### 4.1 app(应用层)
- 负责系统流程控制
- 状态机 / 控制逻辑
- 不允许访问硬件细节
```c
void app_loop(void) {
imu_read(...);
pid_update(...);
motor_set_speed(...);
}
````
---
### 4.2 modules(模块层)
#### 分类:
```
modules/
├── device/ # 电机等执行器
├── sensor/ # IMU/传感器
├── control/ # PID / Kalman
├── protocol/ # 通信协议
```
#### 规则:
* 只依赖 interfaces
* 不允许使用 HAL
```c
// ❌ 禁止
#include "i2c.h"
// ✅ 正确
#include "i2c_if.h"
```
---
### 4.3 interfaces(接口层)
定义硬件抽象接口:
```c
typedef struct {
int (*read)(uint8_t addr, uint8_t reg, uint8_t *buf, int len);
} i2c_if_t;
```
作用:
* 解耦 modules 和 bsp
* 支持多平台替换
---
### 4.4 bsp(板级支持包)
结构:
```
bsp/stm32f4/
├── i2c/
├── can/
├── spi/
├── gpio/
└── board.c
```
职责:
* 封装 HAL
* 提供接口实现
```c
int stm32_i2c_read(...) {
return HAL_I2C_Mem_Read(...);
}
```
---
### 4.5 3rdparty(第三方库)
放置:
* cJSON
* lwIP
* FreeRTOS(如果不是Cube管理)
#### ❗规则:
* 不修改源码
* 不直接暴露给 app
---
### ❗ 示例(推荐封装)
```text
modules/json/
json_wrapper.c
```
---
### 4.6 utils(工具层)
* ringbuffer
* flash
* log
* 数学工具
特点:
* 无业务依赖
* 可复用
---
## 5. Core(CubeMX生成代码)规范
### ✅ 允许:
* 初始化
* 外设配置
```c
MX_I2C1_Init();
MX_CAN1_Init();
```
---
### ❌ 禁止:
* 写业务逻辑
* 写控制算法
* 写模块代码
---
### 推荐 main.c 结构:
```c
int main(void)
{
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_I2C1_Init();
MX_CAN1_Init();
bsp_init();
app_init();
while (1)
{
app_loop();
}
}
```
---
## 6. 第三方库使用规范
### 分类原则:
| 类型 | 放置位置 |
| ---- | -------- |
| 不修改 | 3rdparty |
| 需要修改 | modules |
---
### 示例:
```
3rdparty/
cjson/
modules/control/
pid.c
```
---
## 7. 编码规范
### 7.1 命名
| 类型 | 规范 |
| --- | -------- |
| 文件 | 小写 + 下划线 |
| 结构体 | xxx_t |
| 函数 | 模块名_功能 |
---
### 示例:
```c
motor_set_speed()
imu_read_accel()
pid_update()
```
---
### 7.2 模块结构
每个模块:
```
xxx.c
xxx.h
```
---
### 7.3 禁止全局变量污染
```c
// ❌
extern int state;
// ✅
通过结构体传递
```
---
## 8. 解耦设计规范(核心)
### 必须满足:
* 模块不依赖具体硬件
* 硬件可替换
* 支持模拟测试
---
### 示例:
```c
motor->can->send(...)
imu->i2c->read(...)
```
---
## 9. RTOS 使用规范(如使用)
推荐结构:
```
imu_task
control_task
motor_task
```
通信方式:
* Queue
* Event
---
## 10. 常见错误(必须避免)
### ❌ 直接调用 HAL
```c
HAL_I2C_Mem_Read(...)
```
---
### ❌ 模块 include Core
```c
#include "i2c.h"
```
---
### ❌ ISR 写业务逻辑
```c
void IRQ() {
pid_update(); // ❌
}
```
---
## 11. 开发流程建议
### 阶段:
1. 外设跑通(HAL)
2. 模块拆分
3. 接口抽象
4. BSP封装
5. RTOS化(可选)
---
## 12. 目标
最终实现:
* 硬件可替换
* 模块可复用
* 架构清晰
* 易于扩展
---
## 13. 一句话总结
> **接口解耦 + BSP隔离 + 分层设计 = 工程级嵌入式架构**
```
---
# 最后说一句(很关键)
你现在做这件事(写规范)说明你已经在从:
👉 “会写代码” → “会做工程”
转变了。
---
如果你愿意,我可以下一步帮你:
✅ :contentReference[oaicite:0]{index=0}
或者
✅ :contentReference[oaicite:1]{index=1}
这会让你的工程直接变成“公司级别”的质量。
-224
View File
@@ -1,224 +0,0 @@
# MF4010V2 CAN 驱动重构指南
## 📋 重构内容
### 1. 新增文件
| 文件 | 说明 |
|------|------|
| `Core/Src/can.c` | CAN 通用 API(HAL 层) |
| `Core/Inc/can.h` | CAN 通用 API 头文件 |
| `User/Device/mf4010v2_hal.c` | MF4010V2 专用 HAL 层 |
| `User/Device/mf4010v2_hal.h` | MF4010V2 HAL 层头文件 |
---
## 🏗️ 架构改进
### 重构前
```
mf4010v2.c
↓
can1_send_message() ← 直接绑定 CAN1 外设
can1_receive_message()
```
### 重构后
```
mf4010v2.c
↓
mf4010v2_hal.c ← 电机专用 HAL 层
↓
can.c ← 通用 CAN API
↓
STM32 HAL CAN 驱动
```
---
## 📝 API 变更
### 新增通用 CAN API(can.c/h)
```c
// 启动 CAN
void CAN_Start(void);
// 配置滤波器
void CAN_ConfigFilter(uint8_t filter_id, uint16_t id, uint16_t mask);
void CAN_ConfigFilterAll(uint8_t filter_id); // 接收所有帧
// 发送/接收
HAL_StatusTypeDef CAN_SendFrame(uint16_t id, const uint8_t* data,
uint8_t len, uint32_t timeout);
HAL_StatusTypeDef CAN_ReceiveFrame(uint16_t* id, uint8_t* data,
uint8_t* len, uint32_t timeout);
// 状态查询
uint8_t CAN_HasMessage(void);
uint32_t CAN_GetErrorCode(void);
uint32_t CAN_GetState(void);
```
### 新增电机 HAL 层(mf4010v2_hal.c/h)
```c
// 初始化
HAL_StatusTypeDef MF4010V2_HAL_Init(CAN_HandleTypeDef* hcan);
// 发送/接收
HAL_StatusTypeDef MF4010V2_HAL_Send(uint16_t id, const uint8_t* data,
uint8_t len, uint32_t timeout);
HAL_StatusTypeDef MF4010V2_HAL_Receive(uint16_t* id, uint8_t* data,
uint8_t* len, uint32_t timeout);
```
---
## 🔧 使用示例
### 1. 初始化 CAN 和电机
```c
#include "can.h"
#include "device/mf4010v2.h"
#include "device/mf4010v2_hal.h"
// 在 main.c 或初始化代码中
void System_Init(void)
{
// CubeMX 生成的初始化
MX_CAN1_Init();
// 初始化电机
mf4010v2_t motor;
mf4010v2_init(&motor, 0x141);
}
```
### 2. 发送电机命令
```c
// 速度控制
uint8_t cmd[8];
cmd_speed_control(cmd, 10000); // 100 DPS
mf4010_status_t status = mf4010v2_send_command(&motor, cmd, 100);
if(status == MF4010_OK) {
// 成功
}
```
### 3. 直接使用 CAN API(其他设备)
```c
// 发送自定义 CAN 帧
uint8_t data[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08};
CAN_SendFrame(0x123, data, 8, 100);
// 接收 CAN 帧
uint16_t id;
uint8_t len;
uint8_t rx_data[8];
if(CAN_ReceiveFrame(&id, rx_data, &len, 100) == HAL_OK) {
// 处理接收到的数据
}
```
---
## 🎯 优势
### 1. 硬件抽象
- ✅ `mf4010v2.c` 不再直接依赖 `can1_send_message`
- ✅ 可以轻松移植到其他 CAN 外设(CAN2 等)
- ✅ 便于单元测试(可以 mock HAL 层)
### 2. 可复用性
- ✅ `can.c` 提供的通用 API 可用于其他 CAN 设备
- ✅ 不再绑定特定电机型号
### 3. 代码清晰
- ✅ 分层清晰:应用层 → HAL 层 → 通用 CAN 层 → STM32 HAL
- ✅ 职责分明,易于维护
---
## 📊 CAN 配置(1 Mbps)
CubeMX 配置:
```
CAN1 → Enabled
├─ GPIO: PA11 (RX), PA12 (TX), AF9
└─ Parameter Settings:
├─ Prescaler: 6
├─ Time Seg1: 2
├─ Time Seg2: 4
└─ Mode: Normal
```
**实际波特率计算**:
```
APB1 = 42 MHz
时间量子 = 1 / (42MHz / 6) = 142.857 ns
位时间 = (1 + 2 + 4) × 142.857 ns = 1000 ns
波特率 = 1 / 1000ns = 1 Mbps ✓
```
---
## 🔄 迁移指南
### 旧代码
```c
// 直接调用 CAN 函数
can1_send_message(id, data, len);
CanRxMsg rx = can1_receive_message();
```
### 新代码
```c
// 使用通用 CAN API
CAN_SendFrame(id, data, len, timeout);
CAN_ReceiveFrame(&id, data, &len, timeout);
```
---
## ⚠️ 注意事项
1. **CAN 滤波器配置**
- 默认配置为接收所有帧(`CAN_ConfigFilterAll(0)`)
- 如需过滤特定 ID,使用 `CAN_ConfigFilter(filter_id, id, mask)`
2. **超时处理**
- 所有发送/接收函数都有 `timeout` 参数(毫秒)
- 超时返回 `HAL_TIMEOUT`
3. **错误处理**
- 使用 `CAN_GetErrorCode()` 获取详细错误
- 使用 `CAN_GetState()` 获取 CAN 状态
---
## 📁 文件结构
```
CloudPlant/
├── Core/
│ ├── Inc/
│ │ └── can.h # ✨ 新增:通用 CAN API
│ └── Src/
│ └── can.c # ✨ 新增:通用 CAN 实现
├── User/
│ └── Device/
│ ├── mf4010v2.c/h # 修改:使用 HAL 层
│ └── mf4010v2_hal.c/h # ✨ 新增:电机 HAL 层
└── CMakeLists.txt # 修改:添加新源文件
```
---
**文档版本**: v1.0
**创建日期**: 2026-03-30
**作者**: CloudPlant Team
-317
View File
@@ -1,317 +0,0 @@
# MF4010V2 驱动改进总结
## 已完成的修复
### ✅ 高优先级(已完成)
1. **修复宏定义的内存安全问题**
2. **添加错误处理和返回值**
3. **添加 CAN 通信超时机制**
### ✅ 中优先级(已完成)
#### 4. 优化结构体设计
**改进前**:
```c
typedef struct {
uint16_t id;
uint8_t command[8]; // 未使用
uint8_t return_code[8]; // 只保存最后一次
uint16_t id;
cJSON* json_data; // 每次都创建,效率低
} mf4010v2_t;
```
**改进后**:
```c
typedef struct {
uint16_t id; // CAN ID
uint8_t last_response[8]; // 最后响应数据
uint32_t last_comm_time; // 时间戳
uint16_t error_count; // 错误计数
uint16_t success_count; // 成功计数(新增)
uint8_t flags; // 状态标志
uint8_t reserved; // 保留字节(对齐)
#if MF4010_ENABLE_JSON
void* json_data; // 可选 JSON 数据
#endif
} mf4010v2_t;
```
**新增状态标志**:
```c
typedef enum {
MF4010_FLAG_NONE = 0x00, // 无标志
MF4010_FLAG_RUNNING = 0x01, // 电机运行中
MF4010_FLAG_ENABLED = 0x02, // 电机已使能
MF4010_FLAG_FAULT = 0x04, // 故障状态
MF4010_FLAG_CONNECTED = 0x08 // 已连接
} mf4010_flag_t;
```
---
#### 5. 解耦 JSON 功能
**问题**:
- JSON 功能耦合到核心驱动
- 每次通信都创建 JSON 对象,效率低
- 嵌入式系统应避免动态内存分配
**解决方案**:使用条件编译
```c
/* 可选:启用 JSON 调试功能 */
#ifndef MF4010_ENABLE_JSON
#define MF4010_ENABLE_JSON 0 /* 设为 1 启用,需要 cJSON 库 */
#endif
#if MF4010_ENABLE_JSON
#include <cjson/cJSON.h>
#endif
// JSON 函数也被条件编译保护
#if MF4010_ENABLE_JSON
void mf4010v2_to_json(mf4010v2_t* motor);
void mf4010v2_free_json(mf4010v2_t* motor);
char* mf4010v2_print_json(mf4010v2_t* motor);
#endif
```
**使用方法**:
```c
// 在 mf4010v2.h 之前定义
#define MF4010_ENABLE_JSON 1
#include "device/mf4010v2.h"
// 或者在 CMakeLists.txt 中添加编译定义
target_compile_definitions(your_target PRIVATE
MF4010_ENABLE_JSON=1
)
```
---
#### 6. 增强数据验证
**新增验证功能**:
1. **CAN ID 范围验证**
```c
mf4010_status_t mf4010v2_init(mf4010v2_t* motor, uint16_t id)
{
if(motor == NULL) {
return MF4010_ERR_INVALID_PARAM;
}
// CAN ID 范围验证 (0x000-0x7FF)
if(id > 0x7FF) {
printf("Error: Invalid CAN ID 0x%03X\r\n", id);
return MF4010_ERR_INVALID_ID;
}
// ...
}
```
2. **命令格式验证**
```c
mf4010v2_send_command(...)
{
// 验证命令格式
if((command[0] & 0x80) == 0 && command[0] < 0x30) {
printf("Warning: Invalid command format 0x%02X\r\n", command[0]);
// 继续执行
}
// ...
}
```
3. **重试机制**
```c
mf4010_status_t mf4010v2_send_command(...)
{
uint8_t retry_count = 0;
const uint8_t max_retries = 3;
while(retry_count < max_retries) {
// 发送并等待响应
// ...
if(timeout) {
retry_count++;
continue; // 重试
}
}
return MF4010_ERR_NO_RESPONSE;
}
```
4. **响应错误解析**
```c
// 解析错误状态
if(rx.Data[0] & 0x80) {
uint8_t error_code = rx.Data[0] & 0x7F;
printf("Motor error: 0x%02X\r\n", error_code);
motor->error_count++;
motor->flags |= MF4010_FLAG_FAULT;
return MF4010_ERR_CRC;
}
```
---
### 新增辅助函数
#### 状态检查函数
```c
// 检查电机是否在线
static inline bool mf4010v2_is_connected(mf4010v2_t* motor);
// 检查电机是否运行
static inline bool mf4010v2_is_running(mf4010v2_t* motor);
```
#### 诊断函数
```c
// 获取错误计数
static inline uint16_t mf4010v2_get_error_count(mf4010v2_t* motor);
// 重置错误计数
static inline void mf4010v2_reset_errors(mf4010v2_t* motor);
```
#### 响应数据解析宏
```c
// 获取响应中的命令字节
#define MF4010_GET_RESPONSE_CMD(motor) ((motor)->last_response[0])
// 获取响应中的数据字节
#define MF4010_GET_RESPONSE_DATA(motor, index) \
((motor)->last_response[(index) + 1])
// 检查响应是否包含错误
static inline bool mf4010v2_response_has_error(mf4010v2_t* motor);
// 获取错误码
static inline uint8_t mf4010v2_get_error_code(mf4010v2_t* motor);
```
---
## 使用示例
### 基本使用(无 JSON)
```c
#include "device/mf4010v2.h"
mf4010v2_t motor;
// 初始化
mf4010_status_t status = mf4010v2_init(&motor, 0x141);
if(status != MF4010_OK) {
// 处理错误
}
// 速度控制
uint8_t cmd[8];
cmd_speed_control(cmd, 10000); // 100 DPS
status = mf4010v2_send_command(&motor, cmd, 100);
if(status == MF4010_OK) {
// 成功
} else {
// 失败处理
printf("Error count: %d\r\n", motor.error_count);
}
// 检查连接状态
if(mf4010v2_is_connected(&motor)) {
// 电机在线
}
```
### 启用 JSON 调试
```c
#define MF4010_ENABLE_JSON 1
#include "device/mf4010v2.h"
// ... 初始化和控制 ...
// 输出调试信息
mf4010v2_to_json(&motor);
char* json_str = mf4010v2_print_json(&motor);
printf("%s\r\n", json_str);
cJSON_free(json_str);
mf4010v2_free_json(&motor);
```
---
## 性能改进
| 项目 | 改进前 | 改进后 | 提升 |
|------|--------|--------|------|
| **结构体大小** | 28 字节(含 cJSON 指针) | 24 字节(无 JSON) | -14% |
| **内存分配** | 每次通信可能分配 | 无动态分配(JSON 关闭时) | 100% |
| **错误处理** | 无返回值 | 完整错误码 | - |
| **超时保护** | 可能永久阻塞 | 可配置超时 | - |
| **重试机制** | 无 | 最多 3 次重试 | 提高可靠性 |
---
## 向后兼容性
保留了原有的宏定义以保持向后兼容:
```c
// 旧代码仍然可用
COMMAND_USEAGE(&motor, COMMAND_MOTOR_RUNNING, 100);
// 推荐使用新 API
mf4010v2_send_command(&motor, cmd, 100);
```
---
## 编译配置
### 最小配置(推荐用于生产)
```c
// mf4010v2.h 或 CMakeLists.txt
#define MF4010_ENABLE_JSON 0
```
**优点**:
- 无需 cJSON 库
- 无动态内存分配
- 更小的代码体积
- 更高的实时性
### 调试配置(推荐用于开发)
```c
#define MF4010_ENABLE_JSON 1
// 链接 cJSON 库
```
**优点**:
- 完整的调试信息
- JSON 格式输出
- 便于问题诊断
---
## 总结
通过中优先级的修复,我们实现了:
1. ✅ **更合理的结构体设计** - 移除冗余字段,添加诊断信息
2. ✅ **JSON 功能解耦** - 可选的调试功能,不影响实时性能
3. ✅ **增强的数据验证** - 参数验证、重试机制、错误解析
代码现在更加健壮、高效,并且可以根据应用需求灵活配置。
+839
View File
@@ -0,0 +1,839 @@
/*
* ============================================================================
* Industrial Grade Software I2C Driver
* STM32F4xx
*
* Features:
* - OpenDrain compliant
* - START / STOP / ReSTART
* - ACK/NACK
* - Clock Stretching
* - Bus Recovery
* - Timeout Protection
* - Memory Read/Write
* - Register-level GPIO acceleration
* - DWT precise delay
*
* Author: ChatGPT
* ============================================================================
*/
#include "soft_i2c_if.h"
#include "bsp_soft_i2c.h"
#include "stm32f407xx.h"
#include "stm32f4xx.h"
#include "stm32f4xx_hal.h"
#include <stdint.h>
#include <stdio.h>
/*
* Debug output.
* Set to 1 to enable printf tracing of I2C errors.
*/
#define SOFT_I2C_DEBUG_ENABLE 1
#if SOFT_I2C_DEBUG_ENABLE
#define SOFT_I2C_DEBUG(...) printf(__VA_ARGS__)
#else
#define SOFT_I2C_DEBUG(...)
#endif
/* ============================================================================
* CONFIG
* ============================================================================
*/
#define SOFT_I2C_HALF_PERIOD_US 5U
#define SOFT_I2C_DEFAULT_TIMEOUT_MS 1000U
/* ============================================================================
* DWT DELAY
* ============================================================================
*/
static int dwt_initialized = 0;
static void dwt_init(void)
{
if (dwt_initialized)
return;
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
DWT->CYCCNT = 0;
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
dwt_initialized = 1;
}
static inline void delay_us(uint32_t us)
{
uint32_t start = DWT->CYCCNT;
uint32_t ticks = us * (SystemCoreClock / 1000000U);
while ((DWT->CYCCNT - start) < ticks);
}
static inline void i2c_delay(void)
{
delay_us(SOFT_I2C_HALF_PERIOD_US);
}
/* ============================================================================
* TRANSACTION TIMEOUT
* ============================================================================
*
* Stores an absolute end-tick value. scl_wait_high() consults this so
* that clock-stretching timeouts honour the caller's timeout parameter.
* A value of 0 means "no timeout" (infinite wait).
*/
static uint32_t txn_timeout_start = 0;
static uint32_t txn_timeout_ticks = 0;
static inline void txn_timeout_set(uint32_t timeout_ms)
{
if (timeout_ms == 0)
timeout_ms = SOFT_I2C_DEFAULT_TIMEOUT_MS;
/* Cap to ~25 seconds at 168 MHz to keep the product in uint32_t */
if (timeout_ms > 25000U)
timeout_ms = 25000U;
txn_timeout_start = DWT->CYCCNT;
txn_timeout_ticks = timeout_ms * (SystemCoreClock / 1000U);
}
static inline int txn_timeout_expired(void)
{
if (txn_timeout_ticks == 0)
return 0;
return (DWT->CYCCNT - txn_timeout_start) >= txn_timeout_ticks;
}
/* ============================================================================
* GPIO FAST ACCESS
* ============================================================================
*/
static inline void gpio_set(GPIO_TypeDef *port, uint16_t pin)
{
port->BSRR = pin;
}
static inline void gpio_reset(GPIO_TypeDef *port, uint16_t pin)
{
port->BSRR = ((uint32_t)pin << 16U);
}
static inline GPIO_PinState gpio_read(GPIO_TypeDef *port, uint16_t pin)
{
return (port->IDR & pin)
? GPIO_PIN_SET
: GPIO_PIN_RESET;
}
/* ============================================================================
* LOW LEVEL GPIO
* ============================================================================
*/
/*
* IMPORTANT:
* GPIO MUST CONFIGURED AS:
*
* GPIO_MODE_OUTPUT_OD
* GPIO_PULLUP
* GPIO_SPEED_FREQ_VERY_HIGH
*/
/* ---------------- SCL ---------------- */
static inline void scl_high(bsp_soft_i2c_t *bus)
{
gpio_set(bus->scl_port, bus->scl_pin);
}
static inline void scl_low(bsp_soft_i2c_t *bus)
{
gpio_reset(bus->scl_port, bus->scl_pin);
}
static inline int scl_read(bsp_soft_i2c_t *bus)
{
return gpio_read(bus->scl_port, bus->scl_pin);
}
/* ---------------- SDA ---------------- */
static inline void sda_release(bsp_soft_i2c_t *bus)
{
gpio_set(bus->sda_port, bus->sda_pin);
}
static inline void sda_low(bsp_soft_i2c_t *bus)
{
gpio_reset(bus->sda_port, bus->sda_pin);
}
static inline int sda_read(bsp_soft_i2c_t *bus)
{
return gpio_read(bus->sda_port, bus->sda_pin);
}
/* ============================================================================
* CLOCK STRETCHING
* ============================================================================
*/
static int scl_wait_high(bsp_soft_i2c_t *bus)
{
scl_high(bus);
while (!scl_read(bus))
{
if (txn_timeout_expired()) {
SOFT_I2C_DEBUG("[i2c] scl_wait_high timeout "
"port=0x%08x pin=0x%04x\r\n",
(unsigned)(uint32_t)bus->scl_port,
(unsigned)bus->scl_pin);
return -1;
}
}
return 0;
}
/* ============================================================================
* BUS RECOVERY
* ============================================================================
*/
static void i2c_bus_recovery(bsp_soft_i2c_t *bus)
{
sda_release(bus);
for (int i = 0; i < 9; i++)
{
scl_low(bus);
i2c_delay();
scl_high(bus);
i2c_delay();
}
/* STOP */
sda_low(bus);
i2c_delay();
scl_high(bus);
i2c_delay();
sda_release(bus);
i2c_delay();
}
/* ============================================================================
* I2C PROTOCOL
* ============================================================================
*/
static int i2c_start(bsp_soft_i2c_t *bus)
{
sda_release(bus);
if (scl_wait_high(bus)) {
SOFT_I2C_DEBUG("[i2c] START: scl_wait_high timeout\r\n");
return -1;
}
i2c_delay();
/* bus busy */
if (!sda_read(bus))
{
SOFT_I2C_DEBUG("[i2c] START: bus busy, trying recovery\r\n");
i2c_bus_recovery(bus);
if (!sda_read(bus)) {
SOFT_I2C_DEBUG("[i2c] START: bus busy after recovery\r\n");
return -1;
}
}
/* START */
sda_low(bus);
i2c_delay();
scl_low(bus);
i2c_delay();
return 0;
}
static int i2c_rep_start(bsp_soft_i2c_t *bus)
{
sda_release(bus);
if (scl_wait_high(bus))
return -1;
i2c_delay();
sda_low(bus);
i2c_delay();
scl_low(bus);
i2c_delay();
return 0;
}
static void i2c_stop(bsp_soft_i2c_t *bus)
{
sda_low(bus);
i2c_delay();
scl_high(bus);
i2c_delay();
sda_release(bus);
i2c_delay();
}
/* ============================================================================
* BYTE WRITE
* ============================================================================
*/
static int i2c_write_byte(bsp_soft_i2c_t *bus,
uint8_t data)
{
for (int i = 0; i < 8; i++)
{
if (data & 0x80)
sda_release(bus);
else
sda_low(bus);
i2c_delay();
if (scl_wait_high(bus))
return -1;
i2c_delay();
scl_low(bus);
i2c_delay();
data <<= 1;
}
/* RELEASE SDA FOR ACK */
sda_release(bus);
i2c_delay();
if (scl_wait_high(bus))
return -1;
i2c_delay();
int ack = sda_read(bus);
scl_low(bus);
i2c_delay();
return ack ? -1 : 0;
}
/* ============================================================================
* BYTE READ
* ============================================================================
*
* Returns:
* 0 on success (data written to *out)
* -1 on timeout / error
*/
static int i2c_read_byte(bsp_soft_i2c_t *bus,
int nack,
uint8_t *out)
{
uint8_t data = 0;
sda_release(bus);
for (int i = 0; i < 8; i++)
{
data <<= 1;
if (scl_wait_high(bus))
return -1;
i2c_delay();
if (sda_read(bus))
data |= 1;
scl_low(bus);
i2c_delay();
}
/* ACK / NACK */
if (nack)
sda_release(bus);
else
sda_low(bus);
i2c_delay();
scl_high(bus);
i2c_delay();
scl_low(bus);
i2c_delay();
sda_release(bus);
*out = data;
return 0;
}
/* ============================================================================
* HAL API
* ============================================================================
*/
static int hal_soft_i2c_init(int ch)
{
bsp_soft_i2c_t *bus =
bsp_soft_i2c_get_handle(ch);
if (!bus)
return -1;
dwt_init();
/* RELEASE BUS */
sda_release(bus);
scl_high(bus);
i2c_delay();
/* RECOVERY */
if (!sda_read(bus))
{
i2c_bus_recovery(bus);
if (!sda_read(bus))
return -1;
}
return 0;
}
/* ============================================================================
* WRITE
* ============================================================================
*/
static int hal_soft_i2c_write(
int ch,
uint16_t dev_addr,
const uint8_t *data,
uint16_t size,
uint32_t timeout)
{
bsp_soft_i2c_t *bus =
bsp_soft_i2c_get_handle(ch);
if (!bus || !data || size == 0)
return -1;
txn_timeout_set(timeout);
if (i2c_start(bus))
return -1;
if (i2c_write_byte(bus,
(uint8_t)(dev_addr & 0xFE)))
{
i2c_stop(bus);
return -1;
}
for (uint16_t i = 0; i < size; i++)
{
if (i2c_write_byte(bus, data[i]))
{
i2c_stop(bus);
return -1;
}
}
i2c_stop(bus);
return 0;
}
/* ============================================================================
* READ
* ============================================================================
*/
static int hal_soft_i2c_read(
int ch,
uint16_t dev_addr,
uint8_t *data,
uint16_t size,
uint32_t timeout)
{
bsp_soft_i2c_t *bus =
bsp_soft_i2c_get_handle(ch);
if (!bus || !data || size == 0)
return -1;
txn_timeout_set(timeout);
if (i2c_start(bus))
return -1;
if (i2c_write_byte(bus,
(uint8_t)(dev_addr | 0x01)))
{
i2c_stop(bus);
return -1;
}
for (uint16_t i = 0; i < size; i++)
{
if (i2c_read_byte(bus,
(i == (size - 1)), &data[i]))
{
i2c_stop(bus);
return -1;
}
}
i2c_stop(bus);
return 0;
}
/* ============================================================================
* MEM WRITE
* ============================================================================
*/
static int hal_soft_i2c_mem_write(
int ch,
uint16_t dev_addr,
uint16_t mem_addr,
uint16_t mem_addr_size,
const uint8_t *data,
uint16_t size,
uint32_t timeout)
{
bsp_soft_i2c_t *bus =
bsp_soft_i2c_get_handle(ch);
if (!bus || !data)
return -1;
txn_timeout_set(timeout);
if (i2c_start(bus))
return -1;
if (i2c_write_byte(bus,
(uint8_t)(dev_addr & 0xFE)))
{
i2c_stop(bus);
return -1;
}
if (mem_addr_size == 2)
{
if (i2c_write_byte(bus,
(uint8_t)(mem_addr >> 8)))
{
i2c_stop(bus);
return -1;
}
}
if (i2c_write_byte(bus,
(uint8_t)(mem_addr & 0xFF)))
{
i2c_stop(bus);
return -1;
}
for (uint16_t i = 0; i < size; i++)
{
if (i2c_write_byte(bus, data[i]))
{
i2c_stop(bus);
return -1;
}
}
i2c_stop(bus);
return 0;
}
/* ============================================================================
* MEM READ
* ============================================================================
*/
static int hal_soft_i2c_mem_read(
int ch,
uint16_t dev_addr,
uint16_t mem_addr,
uint16_t mem_addr_size,
uint8_t *data,
uint16_t size,
uint32_t timeout)
{
bsp_soft_i2c_t *bus =
bsp_soft_i2c_get_handle(ch);
if (!bus || !data)
return -1;
txn_timeout_set(timeout);
if (i2c_start(bus)) {
SOFT_I2C_DEBUG("[mem_read] i2c_start failed\r\n");
return -1;
}
/* DEVICE + WRITE */
if (i2c_write_byte(bus,
(uint8_t)(dev_addr & 0xFE)))
{
SOFT_I2C_DEBUG("[mem_read] NACK at addr_write dev_addr=%u (bus=0x%02x)\r\n",
(unsigned)dev_addr, (unsigned)(dev_addr & 0xFE));
i2c_stop(bus);
return -1;
}
/* MEMORY ADDRESS */
if (mem_addr_size == 2)
{
if (i2c_write_byte(bus,
(uint8_t)(mem_addr >> 8)))
{
i2c_stop(bus);
return -1;
}
}
if (i2c_write_byte(bus,
(uint8_t)(mem_addr & 0xFF)))
{
SOFT_I2C_DEBUG("[mem_read] NACK at reg_addr_write reg=0x%02x\r\n",
(unsigned)(mem_addr & 0xFF));
i2c_stop(bus);
return -1;
}
/* RESTART */
if (i2c_rep_start(bus))
{
SOFT_I2C_DEBUG("[mem_read] restart failed\r\n");
i2c_stop(bus);
return -1;
}
/* DEVICE + READ */
if (i2c_write_byte(bus,
(uint8_t)(dev_addr | 0x01)))
{
SOFT_I2C_DEBUG("[mem_read] NACK at addr_read dev_addr=%u (bus=0x%02x)\r\n",
(unsigned)dev_addr, (unsigned)(dev_addr | 0x01));
i2c_stop(bus);
return -1;
}
/* READ DATA */
for (uint16_t i = 0; i < size; i++)
{
if (i2c_read_byte(bus,
(i == (size - 1)), &data[i]))
{
SOFT_I2C_DEBUG("[mem_read] i2c_read_byte timeout byte=%u\r\n",
(unsigned)i);
i2c_stop(bus);
return -1;
}
}
i2c_stop(bus);
return 0;
}
/*
* FRAME OPS
*/
void hal_soft_i2c_start_frame(int ch) {
bsp_soft_i2c_t *bus = bsp_soft_i2c_get_handle(ch);
if (!bus)
return;
sda
gpio_set(bus->scl_port, bus->scl_pin);
delay_us(5);
gpio_reset(bus->sda_port, bus->sda_pin);
delay_us(5);
gpio_reset(bus->scl_port, bus->scl_pin);
}
void hal_soft_i2c_stop_frame(int ch) {
bsp_soft_i2c_t *bus = bsp_soft_i2c_get_handle(ch);
if (!bus)
return;
gpio_reset(bus->scl_port, bus->scl_pin);
gpio_reset(bus->sda_port, bus->sda_pin);
delay_us(5);
gpio_set(bus->scl_port, bus->scl_pin);
gpio_set(bus->sda_port, bus->sda_pin);
delay_us(5);
}
int hal_soft_i2c_wait_ack_frame(int ch) {
bsp_soft_i2c_t *bus = bsp_soft_i2c_get_handle(ch);
if (!bus)
return -1;
gpio_set(bus->sda_port, bus->sda_pin);
delay_us(5);
while (gpio_read(bus->sda_port, bus->sda_pin)) {
}
gpio_set(bus->scl_port, bus->scl_pin);
delay_us(5);
gpio_reset(bus->scl_port, bus->scl_pin);
return 0;
}
void hal_soft_i2c_send_ack_frame(int ch, int ack) {
bsp_soft_i2c_t *bus = bsp_soft_i2c_get_handle(ch);
if (!bus)
return;
gpio_reset(bus->scl_port, bus->scl_pin);
if (ack != 0) {
gpio_set(bus->sda_port, bus.sda_pin);
} else {
gpio_reset(bus->sda_port, bus.sda_pin);
}
delay_us(5);
gpio_set(bus->scl_port, bus->scl_pin);
delay_us(5);
gpio_reset(bus->scl_port, bus->scl_pin);
}
void hal_soft_i2c_send_frame(int ch, uint8_t byte) {
bsp_soft_i2c_t *bus = bsp_soft_i2c_get_handle(ch);
if (!bus)
return;
gpio_reset(bus->scl_port, bus->scl_pin);
uint8_t send_bits;
for (int i = 0; i < 8; i ++) {
send_bits = ((byte << i) & 0x80) >> 7;
if (send_bits) {
gpio_set(bus->sda_port, bus->sda_pin);
} else {
gpio_reset(bus->sda_port, bus->sda_pin);
}
delay_us(2);
gpio_set(bus->scl_port, bus->scl_pin);
delay_us(5);
gpio_reset(bus->scl_port, bus->scl_pin);
delay_us(3);
}
}
uint8_t hal_soft_i2c_read_frame(int ch, uint8_t ack) {
bsp_soft_i2c_t *bus = bsp_soft_i2c_get_handle(ch);
uint8_t ret = 0;
if (!bus)
return -1;
for (int i = 0; i < 8; i ++) {
scl_low(bus);
delay_us(5);
scl_high(bus);
ret <<= 1;
if (sda_read(bus)) ret ++;
delay_us(5);
}
hal_soft_i2c_send_ack_frame(ch, ack);
return ret;
}
/* ============================================================================
* OPS
* ============================================================================
*/
static const soft_i2c_ops_t ops =
{
.init = hal_soft_i2c_init,
.write = hal_soft_i2c_write,
.read = hal_soft_i2c_read,
.mem_write = hal_soft_i2c_mem_write,
.mem_read = hal_soft_i2c_mem_read,
.__start_frame = hal_soft_i2c_start_frame,
.__stop_frame = hal_soft_i2c_stop_frame,
.__wait_ack_frame = hal_soft_i2c_wait_ack_frame,
.__send_ack_frame = hal_soft_i2c_send_ack_frame,
.__send_frame = hal_soft_i2c_send_frame,
.__read_frame = hal_soft_i2c_read_frame,
};
/* ============================================================================
* REGISTER
* ============================================================================
*/
void hal_soft_i2c_init_all(void)
{
soft_i2c_register_ops(&ops);
}
+6
View File
@@ -0,0 +1,6 @@
#ifndef HAL_SOFT_I2C_H
#define HAL_SOFT_I2C_H
void hal_soft_i2c_init_all(void);
#endif
+43
View File
@@ -0,0 +1,43 @@
#include "soft_i2c_if.h"
static const soft_i2c_ops_t *g_soft_i2c_ops = 0;
void soft_i2c_register_ops(const soft_i2c_ops_t *ops)
{
g_soft_i2c_ops = ops;
}
int soft_i2c_if_init(int ch)
{
if (!g_soft_i2c_ops || !g_soft_i2c_ops->init)
return -1;
return g_soft_i2c_ops->init(ch);
}
int soft_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_soft_i2c_ops || !g_soft_i2c_ops->mem_write)
return -1;
return g_soft_i2c_ops->mem_write(ch, dev_addr, mem_addr, mem_add_size, data, size, timeout);
}
int soft_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_soft_i2c_ops || !g_soft_i2c_ops->mem_read)
return -1;
return g_soft_i2c_ops->mem_read(ch, dev_addr, mem_addr, mem_add_size, data, size, timeout);
}
int soft_i2c_if_write(int ch, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout)
{
if (!g_soft_i2c_ops || !g_soft_i2c_ops->write)
return -1;
return g_soft_i2c_ops->write(ch, dev_addr, data, size, timeout);
}
int soft_i2c_if_read(int ch, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout)
{
if (!g_soft_i2c_ops || !g_soft_i2c_ops->read)
return -1;
return g_soft_i2c_ops->read(ch, dev_addr, data, size, timeout);
}
+37
View File
@@ -0,0 +1,37 @@
#ifndef __SOFT_I2C_IF_H__
#define __SOFT_I2C_IF_H__
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
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);
void (*__start_frame)(int ch);
void (*__send_frame)(int ch, uint8_t byte);
int (*__wait_ack_frame)(int ch);
void (*__stop_frame)(int ch);
int (*__read_frame)(int ch, int ack);
void (*__send_ack_frame)(int ch, int ack);
} soft_i2c_ops_t;
void soft_i2c_register_ops(const soft_i2c_ops_t *ops);
int soft_i2c_if_init(int ch);
int soft_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 soft_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 soft_i2c_if_write(int ch, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout);
int soft_i2c_if_read(int ch, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout);
#ifdef __cplusplus
}
#endif
#endif
+36 -447
View File
@@ -1,454 +1,43 @@
/**
* @file soft_i2c.c
* @brief 软件 I2C(位模拟)驱动程序实现
* @note 基于 GPIO 位时序实现
*/
#include "soft_i2c.h"
#include "cmsis_os2.h"
#include "gpio_if.h"
#include "cmsis_compiler.h"
#include "projdefs.h"
/* ============================================================================
* 内部辅助函数
* ============================================================================ */
/**
* @brief 微秒级延时
* @note 简单软件延时,实际时间取决于编译器优化和主频
* 对于精确时序,建议替换为定时器延时
*/
void soft_i2c_delay_us(uint32_t us)
{
#ifndef
volatile uint32_t count;
/* 假设 168MHz 主频,每个循环约 1us */
/* 根据实际主频调整系数 */
while (us--) {
for (count = 0; count < 21; count++) {
__NOP();
}
#include "soft_i2c_if.h"
#include <reent.h>
#include <string.h>
int soft_i2c_init(soft_i2c_t *obj, int ch) {
if (obj == NULL) {
return -1;
}
#else
osDelay(pdMS_TO_TICKS(us/1000u));
#endif
obj->ch = ch;
if (soft_i2c_if_init(ch)) {
obj->initialized = 0;
return -2;
}
obj->initialized = 1;
return 0;
}
/**
* @brief 设置 SDA 输出
*/
static void soft_i2c_sda_out(soft_i2c_t *i2c, uint8_t val)
{
gpio_if_write(i2c->config.sda_gpio_ch, val);
int soft_i2c_write(soft_i2c_t *obj, uint16_t dev_addr,
const uint8_t *data, uint16_t size, uint32_t timeout) {
if (obj == NULL || obj->initialized == 0)
return -1;
return soft_i2c_if_write(obj->ch, dev_addr, data, size, timeout);
}
/**
* @brief 读取 SDA 输入(释放为输入模式)
* @note 软件 I2C 需要在读取前释放 SDA 线
*/
static uint8_t soft_i2c_sda_in(soft_i2c_t *i2c)
{
/* 读取时 SDA 应为输入模式 */
/* 注意:当前 gpio_if 没有输入/输出模式切换 */
/* 假设 GPIO 已配置为合适的模式 */
return (uint8_t)gpio_if_read(i2c->config.sda_gpio_ch);
int soft_i2c_read(soft_i2c_t *obj, uint16_t dev_addr,
uint8_t *data, uint16_t size, uint32_t timeout) {
if (obj == NULL || obj->initialized == 0)
return -1;
return soft_i2c_if_read(obj->ch, dev_addr, data, size, timeout);
}
/**
* @brief 设置 SCL 输出
*/
static void soft_i2c_scl_out(soft_i2c_t *i2c, uint8_t val)
{
gpio_if_write(i2c->config.scl_gpio_ch, val);
int soft_i2c_mem_write(soft_i2c_t *obj, uint16_t dev_addr,
uint16_t mem_addr, uint16_t mem_addr_size,
const uint8_t *data, uint16_t size, uint32_t timeout) {
if (obj == NULL || obj->initialized == 0)
return -1;
return soft_i2c_if_mem_write(obj->ch, dev_addr, mem_addr, mem_addr_size, data, size, timeout);
}
/**
* @brief 读取 SCL 输入(未使用,保留用于时钟拉伸支持)
*/
#if 0
static uint8_t soft_i2c_scl_in(soft_i2c_t *i2c)
{
return (uint8_t)gpio_if_read(i2c->config.scl_gpio_ch);
}
#endif
/**
* @brief 产生半个 I2C 周期延时
*/
static inline void soft_i2c_half_cycle(soft_i2c_t *i2c)
{
soft_i2c_delay_us(i2c->config.delay_us);
}
/* ============================================================================
* 核心 API 实现
* ============================================================================ */
int soft_i2c_init(soft_i2c_t *i2c, const soft_i2c_config_t *config)
{
if (!i2c || !config) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 复制配置 */
i2c->config = *config;
i2c->bus_busy = false;
i2c->last_error = 0;
/* 初始化 GPIO */
/* 注意:GPIO 模式应在外部配置好(开漏输出 + 上拉) */
if (gpio_if_init(config->sda_gpio_ch) != 0) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
if (gpio_if_init(config->scl_gpio_ch) != 0) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 初始状态:SDA 和 SCL 为高(空闲状态)*/
soft_i2c_sda_out(i2c, 1);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
return SOFT_I2C_OK;
}
int soft_i2c_start(soft_i2c_t *i2c)
{
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 检查总线是否忙 */
if (i2c->bus_busy) {
/* 可选:总线忙处理 */
}
/* SCL 高期间,SDA 由高变低 -> 起始条件 */
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_sda_out(i2c, 0);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
i2c->bus_busy = true;
return SOFT_I2C_OK;
}
int soft_i2c_stop(soft_i2c_t *i2c)
{
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* SCL 高期间,SDA 由低变高 -> 停止条件 */
soft_i2c_sda_out(i2c, 0);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
i2c->bus_busy = false;
return SOFT_I2C_OK;
}
int soft_i2c_write_byte(soft_i2c_t *i2c, uint8_t data)
{
uint8_t i;
uint8_t ack;
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 发送 8 位数据,MSB 在前 */
for (i = 0; i < 8; i++) {
/* SCL 低期间改变 SDA */
soft_i2c_scl_out(i2c, 0);
/* 输出数据位 */
if (data & 0x80) {
soft_i2c_sda_out(i2c, 1);
} else {
soft_i2c_sda_out(i2c, 0);
}
data <<= 1;
soft_i2c_half_cycle(i2c);
/* SCL 高期间数据有效 */
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 0);
}
/* 释放 SDA,准备读取 ACK */
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
/* 读取 ACK/NACK */
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
ack = soft_i2c_sda_in(i2c);
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
if (ack) {
return SOFT_I2C_ERR_NACK;
}
return SOFT_I2C_OK;
}
int soft_i2c_read_byte(soft_i2c_t *i2c, bool ack, uint8_t *data)
{
uint8_t i;
uint8_t result = 0;
if (!i2c || !data) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 释放 SDA(作为输入)*/
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
/* 读取 8 位数据,MSB 在前 */
for (i = 0; i < 8; i++) {
/* SCL 高期间读取数据 */
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
result <<= 1;
if (soft_i2c_sda_in(i2c)) {
result |= 0x01;
}
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
}
*data = result;
/* 发送 ACK/NACK */
if (ack) {
soft_i2c_sda_out(i2c, 0); /* ACK */
} else {
soft_i2c_sda_out(i2c, 1); /* NACK */
}
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
return SOFT_I2C_OK;
}
int soft_i2c_send_addr(soft_i2c_t *i2c, uint8_t addr, bool write)
{
uint8_t addr_byte;
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 7 位地址 + 读写位 */
addr_byte = (addr << 1) | (write ? 0x00 : 0x01);
return soft_i2c_write_byte(i2c, addr_byte);
}
/* ============================================================================
* 高级 API 实现
* ============================================================================ */
int soft_i2c_mem_write(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, const uint8_t *data, uint16_t size,
uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data || (mem_addr_size != 1 && mem_addr_size != 2)) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址(写)*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送寄存器/内存地址 */
if (mem_addr_size == 2) {
ret = soft_i2c_write_byte(i2c, (uint8_t)(mem_addr >> 8));
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_write_byte(i2c, (uint8_t)mem_addr);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送数据 */
for (i = 0; i < size; i++) {
ret = soft_i2c_write_byte(i2c, data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
}
int soft_i2c_mem_read(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, uint8_t *data, uint16_t size,
uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data || (mem_addr_size != 1 && mem_addr_size != 2)) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 第一阶段:写入寄存器地址 */
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址(写)*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送寄存器/内存地址 */
if (mem_addr_size == 2) {
ret = soft_i2c_write_byte(i2c, (uint8_t)(mem_addr >> 8));
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_write_byte(i2c, (uint8_t)mem_addr);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 重复起始条件(Repeated Start)*/
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 第二阶段:读取数据 */
/* 发送设备地址(读)*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, false);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 读取数据 */
for (i = 0; i < size; i++) {
/* 最后一个字节发送 NACK,否则发送 ACK */
ret = soft_i2c_read_byte(i2c, (i < (size - 1)), &data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
}
int soft_i2c_write(soft_i2c_t *i2c, uint16_t dev_addr, const uint8_t *data,
uint16_t size, uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址(写)*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送数据 */
for (i = 0; i < size; i++) {
ret = soft_i2c_write_byte(i2c, data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
}
int soft_i2c_read(soft_i2c_t *i2c, uint16_t dev_addr, uint8_t *data,
uint16_t size, uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址(读)*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, false);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 读取数据 */
for (i = 0; i < size; i++) {
ret = soft_i2c_read_byte(i2c, (i < (size - 1)), &data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
int soft_i2c_mem_read(soft_i2c_t *obj, uint16_t dev_addr,
uint16_t mem_addr, uint16_t mem_addr_size,
uint8_t *data, uint16_t size, uint32_t timeout) {
if (obj == NULL || obj->initialized == 0)
return -1;
return soft_i2c_if_mem_read(obj->ch, dev_addr, mem_addr, mem_addr_size, data, size, timeout);
}
+14 -168
View File
@@ -1,189 +1,35 @@
#ifndef __SOFT_I2C_H__
#define __SOFT_I2C_H__
/**
* @file soft_i2c.h
* @brief 软件 I2C(位模拟)驱动程序
* @note 基于 GPIO 位时序实现的 I2C 总线驱动
* - 支持多实例(多个独立的 soft I2C 总线)
* - 标准模式 (100kHz) / 快速模式 (400kHz)
* - 7 位设备地址
*/
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif
/* ============================================================================
* 状态码和错误定义
* ============================================================================ */
/**
* @brief Soft I2C 状态码
*/
typedef enum {
SOFT_I2C_OK = 0, /**< 成功 */
SOFT_I2C_ERR_TIMEOUT = -1, /**< 超时错误 */
SOFT_I2C_ERR_NACK = -2, /**< NACK 错误(设备无应答) */
SOFT_I2C_ERR_INVALID_PARAM = -3, /**< 无效参数 */
SOFT_I2C_ERR_BUS_ERROR = -4, /**< 总线错误 */
SOFT_I2C_ERR_ARBITRATION = -5 /**< 仲裁丢失 */
} soft_i2c_status_t;
/* ============================================================================
* 配置结构体
* ============================================================================ */
/**
* @brief Soft I2C 配置结构体
*/
typedef struct {
int sda_gpio_ch; /**< SDA GPIO 通道号 */
int scl_gpio_ch; /**< SCL GPIO 通道号 */
uint32_t delay_us; /**< 半周期延时(微秒),决定总线速率 */
/**< 例:5us = 100kHz, 2.5us = 200kHz, 1.25us = 400kHz */
bool open_drain; /**< 是否开漏模式(默认 true)*/
} soft_i2c_config_t;
/**
* @brief Soft I2C 主机结构体
*/
typedef struct {
soft_i2c_config_t config; /**< 配置参数 */
bool bus_busy; /**< 总线忙标志 */
uint8_t last_error; /**< 最后错误码 */
int ch;
int initialized;
} soft_i2c_t;
/* ============================================================================
* 核心 API 函数
* ============================================================================ */
int soft_i2c_init(soft_i2c_t *obj, int ch);
/**
* @brief 初始化 Soft I2C 主机
* @param i2c: Soft I2C 结构体指针
* @param config: 配置参数
* @retval 成功返回 SOFT_I2C_OK,失败返回错误码
*/
int soft_i2c_init(soft_i2c_t *i2c, const soft_i2c_config_t *config);
int soft_i2c_write(soft_i2c_t *obj, uint16_t dev_addr,
const uint8_t *data, uint16_t size, uint32_t timeout);
/**
* @brief 产生 I2C 起始条件
* @param i2c: Soft I2C 结构体指针
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_start(soft_i2c_t *i2c);
int soft_i2c_read(soft_i2c_t *obj, uint16_t dev_addr,
uint8_t *data, uint16_t size, uint32_t timeout);
/**
* @brief 产生 I2C 停止条件
* @param i2c: Soft I2C 结构体指针
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_stop(soft_i2c_t *i2c);
int soft_i2c_mem_write(soft_i2c_t *obj, uint16_t dev_addr,
uint16_t mem_addr, uint16_t mem_add_size,
const uint8_t *data, uint16_t size, uint32_t timeout);
/**
* @brief 发送一个字节
* @param i2c: Soft I2C 结构体指针
* @param data: 要发送的数据
* @retval 成功返回 SOFT_I2C_OK,NACK 返回 SOFT_I2C_ERR_NACK
*/
int soft_i2c_write_byte(soft_i2c_t *i2c, uint8_t data);
/**
* @brief 读取一个字节
* @param i2c: Soft I2C 结构体指针
* @param ack: 是否发送 ACK (true=发送 ACK, false=发送 NACK)
* @param data: 读取到的数据(输出)
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_read_byte(soft_i2c_t *i2c, bool ack, uint8_t *data);
/**
* @brief 发送设备地址(含读写位)
* @param i2c: Soft I2C 结构体指针
* @param addr: 7 位设备地址
* @param write: 读写方向(true=写,false=读)
* @retval 成功返回 SOFT_I2C_OK,NACK 返回 SOFT_I2C_ERR_NACK
*/
int soft_i2c_send_addr(soft_i2c_t *i2c, uint8_t addr, bool write);
/* ============================================================================
* 高级 API 函数(类似硬件 I2C 的操作)
* ============================================================================ */
/**
* @brief I2C 内存写入(带寄存器地址)
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param mem_addr: 寄存器/内存地址
* @param mem_addr_size: 寄存器地址字节数(1 或 2)
* @param data: 要写入的数据
* @param size: 数据长度
* @param timeout: 超时时间(未使用,预留)
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_mem_write(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, const uint8_t *data, uint16_t size,
uint32_t timeout);
/**
* @brief I2C 内存读取(带寄存器地址)
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param mem_addr: 寄存器/内存地址
* @param mem_addr_size: 寄存器地址字节数(1 或 2)
* @param data: 读取的数据缓冲区
* @param size: 数据长度
* @param timeout: 超时时间(未使用,预留)
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_mem_read(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, uint8_t *data, uint16_t size,
uint32_t timeout);
/**
* @brief I2C 写入(不带寄存器地址)
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param data: 要写入的数据
* @param size: 数据长度
* @param timeout: 超时时间(未使用,预留)
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_write(soft_i2c_t *i2c, uint16_t dev_addr, const uint8_t *data,
uint16_t size, uint32_t timeout);
/**
* @brief I2C 读取(不带寄存器地址)
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param data: 读取的数据缓冲区
* @param size: 数据长度
* @param timeout: 超时时间(未使用,预留)
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_read(soft_i2c_t *i2c, uint16_t dev_addr, uint8_t *data,
uint16_t size, uint32_t timeout);
/* ============================================================================
* 辅助函数
* ============================================================================ */
/**
* @brief 产生 I2C 时钟脉冲
* @param i2c: Soft I2C 结构体指针
*/
void soft_i2c_generate_clock(soft_i2c_t *i2c);
/**
* @brief 延时函数(微秒级)
* @param us: 延时微秒数
* @note 可替换为实际的延时实现
*/
void soft_i2c_delay_us(uint32_t us);
int soft_i2c_mem_read(soft_i2c_t *obj, uint16_t dev_addr,
uint16_t mem_addr, uint16_t mem_add_size,
uint8_t *data, uint16_t size, uint32_t timeout);
#ifdef __cplusplus
}
-106
View File
@@ -1,106 +0,0 @@
/**
* @file soft_i2c_example.c
* @brief Soft I2C 使用示例
* @note 演示如何使用 soft_i2c 模块
*/
#include "soft_i2c.h"
#include "soft_i2c_hal.h"
#include "i2c_if.h"
#include <stdint.h>
/* ============================================================================
* 示例 1: 直接使用 soft_i2c API
* ============================================================================ */
void soft_i2c_example_direct(void)
{
soft_i2c_t i2c;
soft_i2c_config_t config = {
.sda_gpio_ch = 0, /* 根据实际硬件修改 */
.scl_gpio_ch = 1,
.delay_us = 5, /* 100kHz */
.open_drain = true
};
uint8_t data[2];
/* 初始化 */
soft_i2c_init(&i2c, &config);
/* 示例:读取 AT24C02 EEPROM (地址 0x50) 寄存器 0x00 的 2 个字节 */
soft_i2c_mem_read(&i2c, 0x50, 0x00, 1, data, 2, 0);
/* 示例:写入 2 个字节 */
soft_i2c_mem_write(&i2c, 0x50, 0x00, 1, data, 2, 0);
}
/* ============================================================================
* 示例 2: 使用 HAL 适配器(通过 i2c_if 接口)
* ============================================================================ */
void soft_i2c_example_hal(void)
{
uint8_t data[16];
/* 初始化 soft_i2c HAL,从通道 2 开始 */
/* 调用后,通道 2 和 3 对应 soft_i2c 总线 */
soft_i2c_hal_init(2);
/* 现在可以使用标准 i2c_if 接口 */
/* 读取第一个 soft_i2c 总线上的设备 */
i2c_if_mem_read(2, 0x50, 0x00, 1, data, 16, 100);
/* 读取第二个 soft_i2c 总线上的设备 */
i2c_if_mem_read(3, 0x68, 0x00, 1, data, 16, 100);
}
/* ============================================================================
* 示例 3: MPU6050 读取示例
* ============================================================================ */
void soft_i2c_example_mpu6050(soft_i2c_t *i2c)
{
uint8_t who_am_i;
uint8_t data[14];
/* 读取 WHO_AM_I 寄存器 (0x75) */
soft_i2c_mem_read(i2c, 0x68, 0x75, 1, &who_am_i, 1, 0);
/* 读取加速度计和陀螺仪数据 (0x3B-0x48) */
soft_i2c_mem_read(i2c, 0x68, 0x3B, 1, data, 14, 0);
/* ACCEL_XOUT_H */
int16_t accel_x = (int16_t)((data[0] << 8) | data[1]);
(void)accel_x;
}
/* ============================================================================
* 示例 4: OLED 显示写入示例 (SSD1306)
* ============================================================================ */
void soft_i2c_example_oled_command(soft_i2c_t *i2c, uint8_t cmd)
{
uint8_t buf[2];
/* SSD1306 命令模式:第一字节 0x00 = 命令 */
buf[0] = 0x00;
buf[1] = cmd;
soft_i2c_write(i2c, 0x3C, buf, 2, 0);
}
void soft_i2c_example_oled_data(soft_i2c_t *i2c, const uint8_t *data, uint16_t size)
{
uint8_t static_buf[129]; /* 静态缓冲区,最大 128 字节数据 + 1 字节命令 */
uint16_t i;
if (size > 128) return; /* 限制最大长度 */
/* SSD1306 数据模式:第一字节 0x40 = 数据 */
static_buf[0] = 0x40;
for (i = 0; i < size; i++) {
static_buf[i + 1] = data[i];
}
soft_i2c_write(i2c, 0x3C, static_buf, size + 1, 0);
}
-172
View File
@@ -1,172 +0,0 @@
/**
* @file soft_i2c_hal.c
* @brief Soft I2C HAL 适配器实现
* @note 将 soft_i2c 模块适配到现有的 i2c_ops_t 接口
*
* 使用示例:
* @code
* // 初始化 2 个 soft_i2c 总线(通道 2 和 3)
* soft_i2c_hal_init(2);
*
* // 通过标准接口访问
* i2c_if_read(2, 0x50, buffer, 16, 100); // 读取第一个 soft_i2c 总线
* @endcode
*/
#include "soft_i2c_hal.h"
#include "soft_i2c.h"
#include "i2c_if.h"
#include "gpio_if.h"
#include <stddef.h>
/* ============================================================================
* 配置区域
* ============================================================================ */
/**
* @brief 最大支持的 soft_i2c 总线数量
*/
#ifndef SOFT_I2C_MAX_CHANNELS
#define SOFT_I2C_MAX_CHANNELS 2
#endif
/**
* @brief Soft I2C 总线配置表
* @note 根据实际硬件连接修改引脚配置
*/
static const soft_i2c_config_t soft_i2c_configs[SOFT_I2C_MAX_CHANNELS] = {
{
.sda_gpio_ch = 0, /* SDA1 - 修改为实际 GPIO 通道 */
.scl_gpio_ch = 1, /* SCL1 */
.delay_us = 5, /* 5us = 约 100kHz */
.open_drain = true
},
{
.sda_gpio_ch = 2, /* SDA2 - 修改为实际 GPIO 通道 */
.scl_gpio_ch = 3, /* SCL2 */
.delay_us = 2, /* 2us = 约 250kHz */
.open_drain = true
}
};
/**
* @brief Soft I2C 主机实例
*/
static soft_i2c_t soft_i2c_hosts[SOFT_I2C_MAX_CHANNELS];
/**
* @brief 通道偏移量(soft_i2c 在 i2c_if 中的起始通道号)
*/
static int g_channel_offset = -1;
/* ============================================================================
* i2c_ops_t 接口实现
* ============================================================================ */
static int soft_i2c_hal_init_impl(int ch)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_init(&soft_i2c_hosts[local_ch], &soft_i2c_configs[local_ch]);
}
static int soft_i2c_hal_mem_write(int ch, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, const uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_mem_write(&soft_i2c_hosts[local_ch], dev_addr, mem_addr,
mem_addr_size, data, size, timeout);
}
static int soft_i2c_hal_mem_read(int ch, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_mem_read(&soft_i2c_hosts[local_ch], dev_addr, mem_addr,
mem_addr_size, data, size, timeout);
}
static int soft_i2c_hal_write(int ch, uint16_t dev_addr, const uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_write(&soft_i2c_hosts[local_ch], dev_addr, data, size, timeout);
}
static int soft_i2c_hal_read(int ch, uint16_t dev_addr, uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_read(&soft_i2c_hosts[local_ch], dev_addr, data, size, timeout);
}
/**
* @brief soft_i2c 的 i2c_ops_t 实现
*/
static const i2c_ops_t soft_i2c_ops = {
.init = soft_i2c_hal_init_impl,
.mem_write = soft_i2c_hal_mem_write,
.mem_read = soft_i2c_hal_mem_read,
.write = soft_i2c_hal_write,
.read = soft_i2c_hal_read
};
/* ============================================================================
* 公共 API 实现
* ============================================================================ */
int soft_i2c_hal_init(int channel_offset)
{
int i;
if (g_channel_offset >= 0) {
/* 已初始化 */
return -1;
}
g_channel_offset = channel_offset;
/* 初始化所有 soft_i2c 通道 */
for (i = 0; i < SOFT_I2C_MAX_CHANNELS; i++) {
int ret = soft_i2c_init(&soft_i2c_hosts[i], &soft_i2c_configs[i]);
if (ret != SOFT_I2C_OK) {
/* 初始化失败,但仍继续 */
}
}
/* 注册到 i2c_if */
i2c_register_ops(&soft_i2c_ops);
return 0;
}
int soft_i2c_hal_get_channel_count(void)
{
return SOFT_I2C_MAX_CHANNELS;
}
-34
View File
@@ -1,34 +0,0 @@
/**
* @file soft_i2c_hal.h
* @brief Soft I2C HAL 适配器头文件
* @note 将 soft_i2c 适配到现有的 i2c_ops_t 接口
*/
#ifndef __SOFT_I2C_HAL_H__
#define __SOFT_I2C_HAL_H__
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化 soft_i2c HAL 层并注册到 i2c_if
* @param channel_offset: 通道偏移量(soft_i2c 从该通道号开始)
* @retval 成功返回 0,失败返回 -1
* @note 调用后,soft_i2c 可通过 i2c_if_* 接口访问
* 例如:i2c_if_read(channel_offset, dev_addr, data, size, timeout)
*/
int soft_i2c_hal_init(int channel_offset);
/**
* @brief 获取 soft_i2c 通道数量
*/
int soft_i2c_hal_get_channel_count(void);
#ifdef __cplusplus
}
#endif
#endif /* __SOFT_I2C_HAL_H__ */
+30 -8
View File
@@ -14,6 +14,8 @@
#include <stdio.h>
#include <string.h>
#define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0)
#define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0)
/**
* @brief 初始化电机结构体
*/
@@ -37,7 +39,7 @@ int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch)
motor->last_comm_time = 0;
motor->error_count = 0;
motor->success_count = 0;
motor->flags = MF4010_FLAG_NONE;
motor->flags = MF4010_FLAG_ENABLED;
return MF4010_OK;
}
@@ -84,7 +86,7 @@ int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg)
if (delta > 18000) delta = 18000; // 最多 +180 度
if (delta < -18000) delta = -18000; // 最多 -180 度
cmd_position_add(cmd, delta);
cmd_incr_pos(cmd, delta);
motor->target_angle = angle_0_01deg;
return mf4010v2_send_command(motor, cmd);
@@ -129,12 +131,27 @@ void mf4010v2_stop(mf4010v2_t* motor)
void mf4010v2_close(mf4010v2_t* motor) {
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_CLOSE;
mf4010v2_send_command(motor, cmd);
motor->flags &= ~MF4010_FLAG_ENABLED;
int ret = mf4010v2_send_command(motor, cmd);
if (ret == MF4010_ERR_CAN_SEND) {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return;
} else if (ret == MF4010_ERR_CAN_RECV) {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return;
} else {
if (0 == memcmp(cmd, motor->last_response, sizeof(cmd)/sizeof(cmd[0]))) {
ERASE_FLAG(motor->flags, MF4010_FLAG_ENABLED);
return ;
} else {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return ;
}
}
}
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) {
if (!motor) return;
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8] = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
mf4010v2_send_command(motor, cmd);
pid->angle_kp = motor->last_response[2];
@@ -143,7 +160,7 @@ int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) {
pid->speed_ki = motor->last_response[5];
pid->iq_kp = motor->last_response[6];
pid->iq_ki = motor->last_response[7];
return 0;
return MF4010_OK;
}
/* ============================================================================
* 命令生成函数实现
@@ -238,8 +255,8 @@ void cmd_incr_pos(uint8_t* buf, int32_t angle_increment)
void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment) {
buf[0] = 0xA8;
buf[1] = 0x00;
buf[2] = (uint8_t)(maxSpeed & 0xFF);
buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF);
buf[2] = (uint8_t)(max_speed & 0xFF);
buf[3] = (uint8_t)((max_speed >> 8) & 0xFF);
buf[4] = (uint8_t)(angle_increment & 0xFF);
buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF);
buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF);
@@ -275,6 +292,11 @@ bool mf4010v2_is_running(mf4010v2_t* motor)
return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING);
}
bool mf4010v2_is_fault(mf4010v2_t* motor)
{
return (motor != NULL) && (motor->flags & MF4010_FLAG_FAULT);
}
uint16_t mf4010v2_get_error_count(mf4010v2_t* motor)
{
return (motor != NULL) ? motor->error_count : 0;
+6
View File
@@ -254,6 +254,12 @@ bool mf4010v2_is_connected(mf4010v2_t* motor);
*/
bool mf4010v2_is_running(mf4010v2_t* motor);
/**
* @brief 检查电机是否错误
*/
bool mf4010v2_is_fault(mf4010v2_t* motor);
/**
* @brief 获取电机错误计数
*/
Submodule
+1
Submodule release added at cf7d5a0541.