HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

新增因克斯EC电机(哨兵大yaw轴)驱动支持 目前支持速度伺服控制模式 解析反馈报文类型1

This commit is contained in:
宋家齐 2023-12-11 21:08:08 +08:00
parent 4f8100efbc
commit 9e51196992
12 changed files with 508 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -54,7 +54,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D
modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa
modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor
application application/chassis application/cmd application/gimbal application/shoot
Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)

162
application/gimbal/gimbal.c
View File

@ -1,6 +1,7 @@
#include "gimbal.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "ECmotor/ECA8210.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
@ -11,11 +12,15 @@
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor, *pitch_motor;
static ECMotorInstance *big_yaw_motor;
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
sin_input_generate_t sinInputGenerate;
void GimbalInit()
{
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
@ -73,40 +78,51 @@ void GimbalInit()
// .IntegralLimit = 100,
// .MaxOut = 500,
// },
// .speed_PID = {
// .Kp = 50, // 50
// .Ki = 350, // 350
// .Kd = 0, // 0
// .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
// .IntegralLimit = 2500,
// .MaxOut = 20000,
// },
.speed_PID = {
.Kp = 5.13, // 50
.Ki = 88.26, // 350
.Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500,
.MaxOut = 30000,
},
// .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
// .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
.current_PID = {
.Kp = 1, // 10
.Ki = 0,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 30000,
}
},
.controller_setting_init_config = {
.outer_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP,
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP|CURRENT_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020,
.motor_control_type = CURRENT_CONTROL
};
Motor_Init_Config_s big_yaw_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 1,
},
.controller_setting_init_config = {
.outer_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = ECA8210
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
pitch_motor = DJIMotorInit(&pitch_config);
// yaw_motor = DJIMotorInit(&yaw_config);
// pitch_motor = DJIMotorInit(&pitch_config);
big_yaw_motor = ECMotorInit(&big_yaw_config);
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
sin_input_frequency_init(&sinInputGenerate);
}
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
@ -154,21 +170,30 @@ void GimbalTask()
// 在合适的地方添加pitch重力补偿前馈力矩
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
// ...
static uint32_t cnt;
static float time;
DJIMotorEnable(pitch_motor);
float deltaT=DWT_GetDeltaT(&cnt);
time += deltaT;
//DJIMotorEnable(pitch_motor);
ECMotorEnable(big_yaw_motor);
float input = step_input_generate(&sinInputGenerate);
//DJIMotorSetRef(pitch_motor,input);
ECMotorSetRef(big_yaw_motor,30);
//ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0);
float theta = pitch_motor->measure.angle_single_round - 6200 * ECD_ANGLE_COEF_DJI;
//float input = arm_sin_f32(2*PI*10*time);
float gravity_feed = 3800*arm_cos_f32(theta/180*PI);
DJIMotorSetRef(pitch_motor,gravity_feed);
//DJIMotorSetRef(pitch_motor,gravity_feed);
float vofa_send_data[4];
// vofa_send_data[0]=yaw_motor->motor_controller.speed_PID.Ref;
// vofa_send_data[1]=yaw_motor->motor_controller.speed_PID.Measure;
// vofa_send_data[2]=yaw_motor->motor_controller.angle_PID.Ref;
vofa_send_data[0]=big_yaw_motor->measure.speed_rads;
vofa_send_data[1]=big_yaw_motor->measure.angle_single_round;
vofa_send_data[2]=big_yaw_motor->measure.real_current;
vofa_send_data[3]=big_yaw_motor->measure.temperature;
// vofa_send_data[3]=yaw_motor->motor_controller.angle_PID.Measure;
// vofa_justfloat_output(vofa_send_data,16,&huart1);
vofa_justfloat_output(vofa_send_data,16,&huart1);
vofa_send_data[0] = pitch_motor->motor_controller.pid_ref;
@ -185,16 +210,71 @@ void GimbalTask()
}
void sin_input_generate(float frequency, int count)
{
static uint32_t cnt;
static float time;
while(time>=count*(1/frequency))
{
float deltaT=DWT_GetDeltaT(&cnt);
time += deltaT;
//void sin_input_generate(float frequency, int count)
//{
// static uint32_t cnt;
// static float time;
// while(time>=count*(1/frequency))
// {
// float deltaT=DWT_GetDeltaT(&cnt);
// time += deltaT;
//
// float input = arm_sin_f32(2*PI*frequency*time);
// DJIMotorSetRef(yaw_motor,input);
// }
//}
float input = arm_sin_f32(2*PI*frequency*time);
DJIMotorSetRef(yaw_motor,input);
void sin_input_frequency_init(sin_input_generate_t* InputGenerate)
{
for(int i=0;i<43;i++)
{
InputGenerate->frequency[i] = 1.0 + 0.5*i;
}
for(int i=0;i<9;i++)
{
InputGenerate->frequency[i+43] = 24.0 + 2.0*i;
}
for(int i=0;i<8;i++)
{
InputGenerate->frequency[i+43+9] = 50 + 10*i;
}
InputGenerate->frequency[60] = 200;
InputGenerate->frequency[61] = 250;
InputGenerate->frequency[62] = 333;
InputGenerate->frequency[63] = 500;
}
float sin_input_generate(sin_input_generate_t* InputGenerate)
{
InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt);
InputGenerate->time += InputGenerate->DeltaT;
if(InputGenerate->time >= 20*(1/InputGenerate->frequency[InputGenerate->frequency_index]))
{
InputGenerate->time = 0;
InputGenerate->frequency_index += 1;
}
if(InputGenerate->frequency_index >= 64)
{
InputGenerate->input = 0;
}
else
InputGenerate->input = arm_sin_f32(2*PI*InputGenerate->frequency[InputGenerate->frequency_index]*InputGenerate->time);
//float input = arm_sin_f32(2*PI*frequency*time);
return InputGenerate->input;
}
float step_input_generate(sin_input_generate_t* InputGenerate)
{
static int8_t forward_flag = 1;
InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt);
InputGenerate->time += InputGenerate->DeltaT;
if(InputGenerate->time >= 3)
{
if(forward_flag ==1) forward_flag = -1;
else if (forward_flag == -1) forward_flag = 1;
InputGenerate->time = 0;
}
return 60*forward_flag;
}

16
application/gimbal/gimbal.h
View File

@ -1,6 +1,6 @@
#ifndef GIMBAL_H
#define GIMBAL_H
#include "robot_def.h"
/**
* @brief 初始化云台,会被RobotInit()调用
*
@ -13,4 +13,18 @@ void GimbalInit();
*/
void GimbalTask();
typedef struct
{
uint32_t cnt;//计时用
uint8_t frequency_index;//运行到哪个频率
float time;
float frequency[64];
float DeltaT;
float input;
} sin_input_generate_t;
void sin_input_frequency_init(sin_input_generate_t* InputGenerate);
float sin_input_generate(sin_input_generate_t* InputGenerate);
float step_input_generate(sin_input_generate_t* InputGenerate);
#endif // GIMBAL_H

4
application/robot.c
View File

@ -37,7 +37,7 @@ void RobotInit()
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
ChassisInit();
//ChassisInit();
#endif
OSTaskInit(); // 创建基础任务
@ -56,7 +56,7 @@ void RobotTask()
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
ChassisTask();
//ChassisTask();
#endif
}

4
application/robot_task.h
View File

@ -119,9 +119,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument)
robot_start = DWT_GetTimeline_ms();
RobotTask();
robot_dt = DWT_GetTimeline_ms() - robot_start;
if (robot_dt > 5)
if (robot_dt > 1)
LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
osDelay(5);
osDelay(1);
}
}

4
modules/motor/DJImotor/dji_motor.c
View File

@ -306,7 +306,9 @@ void DJIMotorControl()
pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
//现在3508和6020电调都内置电流闭环 无需PID计算
//pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
pid_ref = pid_ref;
}

179
modules/motor/ECmotor/ECA8210.c Normal file
View File

@ -0,0 +1,179 @@
//
// Created by SJQ on 2023/12/11.
//
#include "ECA8210.h"
ECMotor_Report_t ECMotor_Report;
ECMotor_SpdCMD_t ECMotor_SpdCMD;
ECMotor_PosCMD_t ECMotor_PosCMD;
static uint8_t idx;
static ECMotorInstance *ecmotor_instance[EC_MOTOR_MX_CNT] = {NULL};
static void swap ( uint8_t *a ,uint8_t *b )
{
uint8_t temp = *a;
*a = *b;
*b = temp;
}
/**
* @brief 电机反馈报文解析
*
* @param _instance 发生中断的caninstance
*/
static void ECMotorDecode(CANInstance *_instance)
{
ECMotorInstance *motor = (ECMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
ECMotor_Measure_t *measure = &motor->measure;
uint8_t *rx_buff = _instance->rx_buff;
swap(rx_buff+1,rx_buff+2);// 输出端位置 高位在前
memcpy(&ECMotor_Report,rx_buff,sizeof(ECMotor_Report));
//高位在前 这两个12位数据需要做特殊处理
ECMotor_Report.speed_raw = (uint16_t)(rx_buff[3]<<4|(rx_buff[4]>>4));
ECMotor_Report.current_raw = (uint16_t)(rx_buff[4]<<8|(rx_buff[5]));
DaemonReload(motor->daemon); // 喂狗
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697;
measure->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789;
measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648;
measure->temperature = (ECMotor_Report.temperature_raw - 50) /2;
}
static void ECMotorLostCallback(void *motor_ptr)
{
ECMotorInstance *motor = (ECMotorInstance *)motor_ptr;
LOGWARNING("[ECMotor] motor lost, id: %d", motor->motor_can_ins->tx_id);
}
ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config)
{
ECMotorInstance *motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance));
motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance));
memset(motor, 0, sizeof(ECMotorInstance));
motor->motor_settings = config->controller_setting_init_config;
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
config->can_init_config.id = motor;
config->can_init_config.can_module_callback = ECMotorDecode;
config->can_init_config.rx_id = config->can_init_config.tx_id;
config->can_init_config.tx_id = config->can_init_config.tx_id;
motor->motor_can_ins = CANRegister(&config->can_init_config);
ECMotorEnable(motor);
DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
ecmotor_instance[idx++] = motor;
Daemon_Init_Config_s daemon_config = {
.callback = ECMotorLostCallback,
.owner_id = motor,
.reload_count = 5, // 50ms
};
motor->daemon = DaemonRegister(&daemon_config);
return motor;
}
void ECMotorControl()
{
float pid_measure, pid_ref;
int16_t set;
ECMotorInstance *motor;
ECMotor_Measure_t *measure;
Motor_Control_Setting_s *setting;
for (size_t i = 0; i < idx; ++i)
{
motor = ecmotor_instance[i];
measure = &motor->measure;
setting = &motor->motor_settings;
pid_ref = motor->pid_ref;
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
}
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_rads;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
}
if (setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
ECMotor_SpdCMD.mode = 0b010;
ECMotor_SpdCMD.reserve = 0b000;
ECMotor_SpdCMD.ack_type = 0b01;
ECMotor_SpdCMD.spd_target = set;
ECMotor_SpdCMD.max_current = 10;
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
//memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
CANSetDLC(motor->motor_can_ins,7);
memcpy(motor->motor_can_ins->tx_buff,&ECMotor_SpdCMD,sizeof(ECMotor_SpdCMD));
//发送数据高位在前！！ 交换一下顺序
swap(motor->motor_can_ins->tx_buff+1,motor->motor_can_ins->tx_buff+4);
swap(motor->motor_can_ins->tx_buff+2,motor->motor_can_ins->tx_buff+3);
swap(motor->motor_can_ins->tx_buff+5,motor->motor_can_ins->tx_buff+6);
//memcpy(motor->motor_can_ins->tx_buff+1,&ECMotor_SpdCMD.spd_target,sizeof(float32_t));
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD));
}
CANTransmit(motor->motor_can_ins,0.2);
}
}
void ECMotorStop(ECMotorInstance *motor)
{
motor->stop_flag = MOTOR_STOP;
}
void ECMotorEnable(ECMotorInstance *motor)
{
motor->stop_flag = MOTOR_ENALBED;
}
void ECMotorSetRef(ECMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
}
uint8_t ECMotorIsOnline(ECMotorInstance *motor)
{
return DaemonIsOnline(motor->daemon);
}

130
modules/motor/ECmotor/ECA8210.h Normal file
View File

@ -0,0 +1,130 @@
//
// Created by SJQ on 2023/12/11.
//
#ifndef BASIC_FRAMEWORK_ECA8210_H
#define BASIC_FRAMEWORK_ECA8210_H
#include "stdint.h"
#include "bsp_can.h"
#include "controller.h"
#include "motor_def.h"
#include "daemon.h"
#define EC_MOTOR_MX_CNT 4
#pragma pack(1)
typedef struct
{
uint8_t bug_info:5; //错误信息
uint8_t report_type:3; //报文类别
uint16_t angle_raw:16; //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad
uint16_t speed_raw:12; //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s
uint16_t current_raw:12; //实际电流：0~4095 对应-30~30A
uint8_t temperature_raw:8;//电机温度：反馈的数值数据类型为无符号 8 位，数值等于实际温度*2+50
}ECMotor_Report_t;
#pragma pack()
typedef struct
{
uint8_t mode:3; //控制模式
float32_t pos_target; //错误信息
uint16_t spd_target:15; //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad
uint16_t max_current:12; //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s
uint8_t ack_type:2; //实际电流：0~4095 对应-30~30A
}ECMotor_PosCMD_t;
#pragma pack(1)
typedef struct
{
uint8_t ack_type:2;
uint8_t reserve:3;
uint8_t mode:3; //控制模式
float32_t spd_target; //输出端速度指令 单位RPM
uint16_t max_current; //电流限幅 电机电流阈值数值解释：0~65536 对应 0~6553.6A，比例为10
}ECMotor_SpdCMD_t;
#pragma pack()
typedef struct // EC-A8120
{
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 当前编码器值
float angle_single_round; // 单圈角度
float speed_rads; // speed rad/s
float real_current; // 实际电流
uint8_t temperature; // 温度,C°
float total_angle; // 总角度
int32_t total_round; // 总圈数
float feed_dt;
uint32_t feed_dwt_cnt;
} ECMotor_Measure_t;
typedef struct
{
ECMotor_Measure_t measure;
Motor_Control_Setting_s motor_settings;
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr; // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入
float *current_feedforward_ptr; // 电流前馈指针
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance *motor_can_ins;
DaemonInstance *daemon;
} ECMotorInstance;
/**
* @brief 初始化LK电机
*
* @param config 电机配置
* @return LKMotorInstance* 返回实例指针
*/
ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config);
/**
* @brief 设置参考值
* @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置
*
* @param motor 要设置的电机
* @param ref 设定值
*/
void ECMotorSetRef(ECMotorInstance *motor, float ref);
/**
* @brief 为所有EC电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文)
*
*/
void ECMotorControl();
/**
* @brief 停止EC电机,之后电机不会响应任何指令
*
* @param motor
*/
void ECMotorStop(ECMotorInstance *motor);
/**
* @brief 启动EC电机
*
* @param motor
*/
void ECMotorEnable(ECMotorInstance *motor);
uint8_t ECMotorIsOnline(ECMotorInstance *motor);
#endif //BASIC_FRAMEWORK_ECA8210_H

1
modules/motor/motor_def.h
View File

@ -106,6 +106,7 @@ typedef enum
M2006,
LK9025,
HT04,
ECA8210,
} Motor_Type_e;
/* 电机控制方式枚举 */

3
modules/motor/motor_task.c
View File

@ -1,5 +1,6 @@
#include "motor_task.h"
#include "LK9025.h"
#include "ECA8210.h"
#include "HT04.h"
#include "dji_motor.h"
#include "step_motor.h"
@ -14,7 +15,7 @@ void MotorControlTask()
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
LKMotorControl();
ECMotorControl();
// legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换

51
modules/vofa/vofa.c
View File

@ -33,3 +33,54 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart
//HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
}
#define BYTE0(dwTemp) (*(char*)(&dwTemp))
#define BYTE1(dwTemp) (*((char*)(&dwTemp)+1))
#define BYTE2(dwTemp) (*((char*)(&dwTemp)+2))
#define BYTE3(dwTemp) (*((char*)(&dwTemp)+3))
uint8_t DataSendBuf[100];
//匿名上位机
void ANODT_SendF1(int32_t Angle,int32_t speed_rpm,int32_t Angle_target,int32_t speed_target)//F1灵活格式帧
{
uint8_t cnt=0;
DataSendBuf[cnt++]=0xAA; //帧头
DataSendBuf[cnt++]=0xFF; //目标地址
DataSendBuf[cnt++]=0xF1; //功能码
DataSendBuf[cnt++]=16; //数据长度
DataSendBuf[cnt++]=BYTE0(Angle);
DataSendBuf[cnt++]=BYTE1(Angle);
DataSendBuf[cnt++]=BYTE2(Angle);
DataSendBuf[cnt++]=BYTE3(Angle);
DataSendBuf[cnt++]=BYTE0(speed_rpm);
DataSendBuf[cnt++]=BYTE1(speed_rpm);
DataSendBuf[cnt++]=BYTE2(speed_rpm);
DataSendBuf[cnt++]=BYTE3(speed_rpm);
DataSendBuf[cnt++]=BYTE0(Angle_target);
DataSendBuf[cnt++]=BYTE1(Angle_target);
DataSendBuf[cnt++]=BYTE2(Angle_target);
DataSendBuf[cnt++]=BYTE3(Angle_target);
DataSendBuf[cnt++]=BYTE0(speed_target);
DataSendBuf[cnt++]=BYTE1(speed_target);
DataSendBuf[cnt++]=BYTE2(speed_target);
DataSendBuf[cnt++]=BYTE3(speed_target);
uint8_t sc=0; //和校验
uint8_t ac=0; //附加校验
for(uint8_t i=0;i<DataSendBuf[3]+4;i++)
{
sc+=DataSendBuf[i];
ac+=sc;
}
DataSendBuf[cnt++]=sc;
DataSendBuf[cnt++]=ac;
// for(uint8_t i=0;i<cnt;i++)
// HAL_UART_Transmit(&huart6,&DataSendBuf[i],1,100);
CDC_Transmit_FS(DataSendBuf, cnt);
}

2
modules/vofa/vofa.h
View File

@ -17,5 +17,5 @@ typedef union
uint8_t uint8_t[4];
} send_float;
void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart);
void ANODT_SendF1(int32_t Angle,int32_t speed_rpm,int32_t Angle_target,int32_t speed_target);
#endif // !1#define