diff --git a/.vscode/settings.json b/.vscode/settings.json index 61a1af5..a80f8c2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -14,7 +14,10 @@ "string.h": "c", "stdlib.h": "c", "crc8.h": "c", - "main.h": "c" + "main.h": "c", + "can.h": "c", + "bsp_can.h": "c", + "dji_motor.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", "C_Cpp.intelliSenseEngineFallback": "enabled", diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index 02f63e2..4158a05 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -40,6 +40,7 @@ #include "motor_task.h" #include "referee.h" #include "ins_task.h" +#include "can_comm.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -71,14 +72,21 @@ void MX_FREERTOS_Init(void); /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ - +typedef struct +{ + float a; + float b; + float c; +}sdf; +volatile sdf* rx; /* USER CODE END 0 */ /** * @brief The application entry point. * @retval int */ -int main(void){ +int main(void) +{ /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ @@ -125,9 +133,17 @@ int main(void){ .tx_id = 6}, .controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL}, .controller_param_init_config = {.angle_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}}}; - dji_motor_instance *djimotor = DJIMotorInit(config); + dji_motor_instance *djimotor = DJIMotorInit(&config); + CANComm_Init_Config_s cconfig = { + .can_config = {.can_handle=&hcan1,.tx_id=0x02,.rx_id=0x03}, + .send_data_len = 4, + .recv_data_len = 12}; + CANCommInstance* in = CANCommInit(&cconfig); + volatile float tx = 32; + RefereeInit(&huart6); + /* USER CODE END 2 */ /* Call init function for freertos objects (in freertos.c) */ @@ -139,15 +155,20 @@ int main(void){ /* We should never get here as control is now taken by the scheduler */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ - INS_Init(); + // INS_Init(); + while (1) { /* USER CODE END WHILE */ - DJIMotorSetRef(djimotor, 10); - MotorControlTask(); - HAL_Delay(1); - INS_Task(); + + // DJIMotorSetRef(djimotor, 10); + // MotorControlTask(); + HAL_Delay(10); + CANCommSend(in, (uint8_t*)&tx); + tx+=1; + rx=(sdf*)CANCommGet(in); + // INS_Task(); /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ diff --git a/Makefile b/Makefile index 53ebedb..1417df1 100644 --- a/Makefile +++ b/Makefile @@ -210,6 +210,10 @@ C_INCLUDES = \ -IHAL_N_Middlewares/Middlewares/Third_Party/SEGGER/RTT \ -IHAL_N_Middlewares/Middlewares/Third_Party/SEGGER/Config \ -IHAL_N_Middlewares/Middlewares/ST/ARM/DSP/Inc \ +-Iapplication/chassis \ +-Iapplication/shoot \ +-Iapplication/gimbal \ +-Iapplication/cmd \ -Iapplication \ -Ibsp \ -Imodules/algorithm \ @@ -219,7 +223,8 @@ C_INCLUDES = \ -Imodules/motor \ -Imodules/referee \ -Imodules/remote \ --Imodules/super_cap +-Imodules/super_cap \ +-Imodules/can_comm # compile gcc flags ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections diff --git a/application/cmd/chassis_cmd.c b/application/cmd/chassis_cmd.c index ea34c7e..c05c298 100644 --- a/application/cmd/chassis_cmd.c +++ b/application/cmd/chassis_cmd.c @@ -1,96 +1,96 @@ -#include -#include -void chassis_board_com_lost(void* obj) { - //暂时没有效用 -} -chassis_board_cmd::chassis_board_cmd() : sender([&] { - can_send::can_send_config config; - config.device = &BSP_CanTypeDef::can_devices[1]; - config.can_identifier = 0x003; - return config; - }()), - recver([&] { - can_recv::can_recv_config config; - config.device = &BSP_CanTypeDef::can_devices[1]; - config.can_identifier = 0x004; - config.lost_callback = chassis_board_com_lost; - return config; - }()), - board_buzzer([&] { - buzzer::buzzer_config config; - //底盘音乐2 - config.music = &buzzer::buzzer_musics[2]; - config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT]; - return config; - }()) { - robot_mode = robot_stop; - robot_ready = 0; - chassis_upload_data = NULL; - board_recv = recver.recv_data; - memset(&chassis_control, 0, sizeof(cmd_chassis)); - memset(&board_send, 0, sizeof(chassis_board_send)); -} +// #include +// #include +// void chassis_board_com_lost(void* obj) { +// //暂时没有效用 +// } +// chassis_board_cmd::chassis_board_cmd() : sender([&] { +// can_send::can_send_config config; +// config.device = &BSP_CanTypeDef::can_devices[1]; +// config.can_identifier = 0x003; +// return config; +// }()), +// recver([&] { +// can_recv::can_recv_config config; +// config.device = &BSP_CanTypeDef::can_devices[1]; +// config.can_identifier = 0x004; +// config.lost_callback = chassis_board_com_lost; +// return config; +// }()), +// board_buzzer([&] { +// buzzer::buzzer_config config; +// //底盘音乐2 +// config.music = &buzzer::buzzer_musics[2]; +// config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT]; +// return config; +// }()) { +// robot_mode = robot_stop; +// robot_ready = 0; +// chassis_upload_data = NULL; +// board_recv = recver.recv_data; +// memset(&chassis_control, 0, sizeof(cmd_chassis)); +// memset(&board_send, 0, sizeof(chassis_board_send)); +// } -void chassis_board_cmd::update() { - //初始化为RUN - robot_mode = robot_run; +// void chassis_board_cmd::update() { +// //初始化为RUN +// robot_mode = robot_run; - //判断板间通信在线 - if (!recver.is_online()) { - robot_mode = robot_stop; //板间通信掉线,机器人停止 - } +// //判断板间通信在线 +// if (!recver.is_online()) { +// robot_mode = robot_stop; //板间通信掉线,机器人停止 +// } - //接收底盘回传信息,判断底盘IMU在线且初始化完成 - static subscriber chassis_upload_suber("upload_chassis"); - if (!chassis_upload_suber.empty()) { - chassis_upload_data = chassis_upload_suber.pop(); - if (chassis_upload_data->chassis_status == module_lost) { //底盘模块掉线 - robot_mode = robot_stop; - } - } +// //接收底盘回传信息,判断底盘IMU在线且初始化完成 +// static subscriber chassis_upload_suber("upload_chassis"); +// if (!chassis_upload_suber.empty()) { +// chassis_upload_data = chassis_upload_suber.pop(); +// if (chassis_upload_data->chassis_status == module_lost) { //底盘模块掉线 +// robot_mode = robot_stop; +// } +// } - if (chassis_upload_data == NULL) { //底盘模块初始化尚未完成,第一次回传数据未收到 - robot_mode = robot_stop; - } else { - board_send.gyro_yaw = chassis_upload_data->chassis_imu->euler[2]; - } +// if (chassis_upload_data == NULL) { //底盘模块初始化尚未完成,第一次回传数据未收到 +// robot_mode = robot_stop; +// } else { +// board_send.gyro_yaw = chassis_upload_data->chassis_imu->euler[2]; +// } - //判断裁判系统是否在线,并处理掉线情况(未实现) - board_send.heat_limit_remain = 30; //读取裁判系统 - board_send.bullet_speed_max = 30; //读取裁判系统 +// //判断裁判系统是否在线,并处理掉线情况(未实现) +// board_send.heat_limit_remain = 30; //读取裁判系统 +// board_send.bullet_speed_max = 30; //读取裁判系统 - //判断除了云台板stop之外,都已经上线,说明底盘板初始化完成,进入ready状态 - if (robot_mode == robot_run) { - if (!robot_ready) { - robot_ready = 1; - board_buzzer.start(); //播放音乐 - } - board_send.chassis_board_status = module_run; - } else { - board_send.chassis_board_status = module_lost; - } +// //判断除了云台板stop之外,都已经上线,说明底盘板初始化完成,进入ready状态 +// if (robot_mode == robot_run) { +// if (!robot_ready) { +// robot_ready = 1; +// board_buzzer.start(); //播放音乐 +// } +// board_send.chassis_board_status = module_run; +// } else { +// board_send.chassis_board_status = module_lost; +// } - //云台板进入stop模式 - if (board_recv->robot_mode == robot_stop) { - robot_mode = robot_stop; - } +// //云台板进入stop模式 +// if (board_recv->robot_mode == robot_stop) { +// robot_mode = robot_stop; +// } - if (robot_mode == robot_stop) { - // STOP模式 - chassis_control.mode = chassis_stop; - } else { - // RUN模式 - // 底盘控制指令 - chassis_control.mode = board_recv->chassis_mode; - chassis_control.speed = board_recv->chassis_speed; - chassis_control.power.power_buffer = 0; //应该由裁判系统获得,未实现 - chassis_control.power.power_limit = 50; //应该由裁判系统获得,未实现 - } +// if (robot_mode == robot_stop) { +// // STOP模式 +// chassis_control.mode = chassis_stop; +// } else { +// // RUN模式 +// // 底盘控制指令 +// chassis_control.mode = board_recv->chassis_mode; +// chassis_control.speed = board_recv->chassis_speed; +// chassis_control.power.power_buffer = 0; //应该由裁判系统获得,未实现 +// chassis_control.power.power_limit = 50; //应该由裁判系统获得,未实现 +// } - //发布指令 - static publisher chassis_puber("cmd_chassis"); - chassis_puber.push(&chassis_control); +// //发布指令 +// static publisher chassis_puber("cmd_chassis"); +// chassis_puber.push(&chassis_control); - //板间通信 - sender.send(board_send); -} \ No newline at end of file +// //板间通信 +// sender.send(board_send); +// } \ No newline at end of file diff --git a/application/cmd/chassis_cmd.h b/application/cmd/chassis_cmd.h index c79e676..b874790 100644 --- a/application/cmd/chassis_cmd.h +++ b/application/cmd/chassis_cmd.h @@ -1,25 +1,25 @@ -#ifndef _CHASSIS_BOARD_CMD_H_ -#define _CHASSIS_BOARD_CMD_H_ -#include -#include -#include -#include -#include -class chassis_board_cmd { - private: - Robot_mode robot_mode; //机器人状态 - cmd_chassis chassis_control; //发送给底盘的控制量 - upload_chassis* chassis_upload_data; //底盘模块回传数据 - chassis_board_send board_send; //发送给云台主控的数据 - can_send sender; //板间通信发送 - can_recv recver; //板间通信接收 - gimbal_board_send* board_recv; //板间通信接收数据指针 - void* referee; //裁判系统(尚未完成) - buzzer board_buzzer; //蜂鸣器 - uint8_t robot_ready; //底盘板是否准备好 - public: - chassis_board_cmd(); - void update(); -}; +// #ifndef _CHASSIS_BOARD_CMD_H_ +// #define _CHASSIS_BOARD_CMD_H_ +// #include +// #include +// #include +// #include +// #include +// class chassis_board_cmd { +// private: +// Robot_mode robot_mode; //机器人状态 +// cmd_chassis chassis_control; //发送给底盘的控制量 +// upload_chassis* chassis_upload_data; //底盘模块回传数据 +// chassis_board_send board_send; //发送给云台主控的数据 +// can_send sender; //板间通信发送 +// can_recv recver; //板间通信接收 +// gimbal_board_send* board_recv; //板间通信接收数据指针 +// void* referee; //裁判系统(尚未完成) +// buzzer board_buzzer; //蜂鸣器 +// uint8_t robot_ready; //底盘板是否准备好 +// public: +// chassis_board_cmd(); +// void update(); +// }; -#endif \ No newline at end of file +// #endif \ No newline at end of file diff --git a/application/cmd/gimbal_cmd.c b/application/cmd/gimbal_cmd.c index adffaf6..0d82a89 100644 --- a/application/cmd/gimbal_cmd.c +++ b/application/cmd/gimbal_cmd.c @@ -1,336 +1,336 @@ -#include "gimbal_board_cmd.h" +// #include "gimbal_board_cmd.h" -#include "bsp_can.h" -#include "bsp_log.h" -#include "bsp_uart.h" -#include "common.h" -#include "pub_sub.h" -#include "string.h" -#define INIT_FORWARD 3152 // 云台朝向底盘正前时云台yaw编码器值 +// #include "bsp_can.h" +// #include "bsp_log.h" +// #include "bsp_uart.h" +// #include "common.h" +// #include "pub_sub.h" +// #include "string.h" +// #define INIT_FORWARD 3152 // 云台朝向底盘正前时云台yaw编码器值 -void gimbal_board_com_lost(void* obj) { - //暂时没有效用 -} +// void gimbal_board_com_lost(void* obj) { +// //暂时没有效用 +// } -//此处涉及到lamdba表达式的应用 -gimbal_board_cmd::gimbal_board_cmd() : sender([&] { - //板间通信:发 - can_send::can_send_config config; - config.device = &BSP_CanTypeDef::can_devices[1]; - config.can_identifier = 0x004; - return config; - }()), - recver([&] { - //板间通信:收 - can_recv::can_recv_config config; - config.device = &BSP_CanTypeDef::can_devices[1]; - config.can_identifier = 0x003; - config.notify_func = NULL; - config.lost_callback = gimbal_board_com_lost; - return config; - }()), - pc([&] { - //小电脑通信配置 - canpc::canpc_config config; - config.device = &BSP_CanTypeDef::can_devices[0]; - config.recv_identifer = 0x001; - return config; - }()), - remote([&] { - dt7Remote::dt7_config config; - config.uart_device = &BSP_UART_Typedef::uart_ports[UART_REMOTE_PORT]; - return config; - }()), - board_buzzer([&] { - buzzer::buzzer_config config; - config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT]; - config.music = &buzzer::buzzer_musics[1]; - return config; - }()) { - robot_mode = robot_stop; - robot_ready = 0; - autoaim_mode = auto_aim_off; - gimbal_upload_data = NULL; - //指针指向接收的实际数据 - board_recv = recver.recv_data; - memset(&gimbal_control, 0, sizeof(cmd_gimbal)); - memset(&shoot_control, 0, sizeof(cmd_shoot)); - memset(&board_send, 0, sizeof(gimbal_board_send)); -} +// //此处涉及到lamdba表达式的应用 +// gimbal_board_cmd::gimbal_board_cmd() : sender([&] { +// //板间通信:发 +// can_send::can_send_config config; +// config.device = &BSP_CanTypeDef::can_devices[1]; +// config.can_identifier = 0x004; +// return config; +// }()), +// recver([&] { +// //板间通信:收 +// can_recv::can_recv_config config; +// config.device = &BSP_CanTypeDef::can_devices[1]; +// config.can_identifier = 0x003; +// config.notify_func = NULL; +// config.lost_callback = gimbal_board_com_lost; +// return config; +// }()), +// pc([&] { +// //小电脑通信配置 +// canpc::canpc_config config; +// config.device = &BSP_CanTypeDef::can_devices[0]; +// config.recv_identifer = 0x001; +// return config; +// }()), +// remote([&] { +// dt7Remote::dt7_config config; +// config.uart_device = &BSP_UART_Typedef::uart_ports[UART_REMOTE_PORT]; +// return config; +// }()), +// board_buzzer([&] { +// buzzer::buzzer_config config; +// config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT]; +// config.music = &buzzer::buzzer_musics[1]; +// return config; +// }()) { +// robot_mode = robot_stop; +// robot_ready = 0; +// autoaim_mode = auto_aim_off; +// gimbal_upload_data = NULL; +// //指针指向接收的实际数据 +// board_recv = recver.recv_data; +// memset(&gimbal_control, 0, sizeof(cmd_gimbal)); +// memset(&shoot_control, 0, sizeof(cmd_shoot)); +// memset(&board_send, 0, sizeof(gimbal_board_send)); +// } -float get_offset_angle(short init_forward, short now_encoder) { - short tmp = 0; - if (init_forward < 4096) { - if (now_encoder > init_forward && now_encoder <= 4096 + init_forward) { - tmp = now_encoder - init_forward; - } else if (now_encoder > 4096 + init_forward) { - tmp = -8192 + now_encoder - init_forward; - } else { - tmp = now_encoder - init_forward; - } - } else { - if (now_encoder > init_forward) { - tmp = now_encoder - init_forward; - } else if (now_encoder <= init_forward && now_encoder >= init_forward - 4096) { - tmp = now_encoder - init_forward; - } else { - tmp = now_encoder + 8192 - init_forward; - } - } - return tmp * 360.0 / 8192.0; -} +// float get_offset_angle(short init_forward, short now_encoder) { +// short tmp = 0; +// if (init_forward < 4096) { +// if (now_encoder > init_forward && now_encoder <= 4096 + init_forward) { +// tmp = now_encoder - init_forward; +// } else if (now_encoder > 4096 + init_forward) { +// tmp = -8192 + now_encoder - init_forward; +// } else { +// tmp = now_encoder - init_forward; +// } +// } else { +// if (now_encoder > init_forward) { +// tmp = now_encoder - init_forward; +// } else if (now_encoder <= init_forward && now_encoder >= init_forward - 4096) { +// tmp = now_encoder - init_forward; +// } else { +// tmp = now_encoder + 8192 - init_forward; +// } +// } +// return tmp * 360.0 / 8192.0; +// } -void gimbal_board_cmd::update() { - //第一步,判断机器人工作模式 +// void gimbal_board_cmd::update() { +// //第一步,判断机器人工作模式 - //初始化为RUN - robot_mode = robot_run; +// //初始化为RUN +// robot_mode = robot_run; - //判断板间通信在线 - if (!recver.is_online()) { - robot_mode = robot_stop; //板间通信掉线,机器人停止 - } +// //判断板间通信在线 +// if (!recver.is_online()) { +// robot_mode = robot_stop; //板间通信掉线,机器人停止 +// } - //接收云台回传信息,判断云台IMU在线且初始化完成 - static subscriber gimbal_upload_suber("upload_gimbal"); - if (!gimbal_upload_suber.empty()) { - gimbal_upload_data = gimbal_upload_suber.pop(); - if (gimbal_upload_data->gimbal_status == module_lost) { //云台模块掉线 - robot_mode = robot_stop; - } - //计算云台和底盘的夹角 offset_angle - board_send.chassis_speed.offset_angle = get_offset_angle(INIT_FORWARD, *gimbal_upload_data->yaw_encoder); - pc.pc_send_data.euler[0] = gimbal_upload_data->gimbal_imu->euler[0]; - pc.pc_send_data.euler[1] = gimbal_upload_data->gimbal_imu->euler[1]; - pc.pc_send_data.euler[2] = gimbal_upload_data->gimbal_imu->euler[2]; - pc.pc_send_data.auto_mode_flag = auto_aim_normal; - } +// //接收云台回传信息,判断云台IMU在线且初始化完成 +// static subscriber gimbal_upload_suber("upload_gimbal"); +// if (!gimbal_upload_suber.empty()) { +// gimbal_upload_data = gimbal_upload_suber.pop(); +// if (gimbal_upload_data->gimbal_status == module_lost) { //云台模块掉线 +// robot_mode = robot_stop; +// } +// //计算云台和底盘的夹角 offset_angle +// board_send.chassis_speed.offset_angle = get_offset_angle(INIT_FORWARD, *gimbal_upload_data->yaw_encoder); +// pc.pc_send_data.euler[0] = gimbal_upload_data->gimbal_imu->euler[0]; +// pc.pc_send_data.euler[1] = gimbal_upload_data->gimbal_imu->euler[1]; +// pc.pc_send_data.euler[2] = gimbal_upload_data->gimbal_imu->euler[2]; +// pc.pc_send_data.auto_mode_flag = auto_aim_normal; +// } - if (gimbal_upload_data == NULL) { //云台模块初始化尚未完成,第一次回传数据未收到 - robot_mode = robot_stop; - } +// if (gimbal_upload_data == NULL) { //云台模块初始化尚未完成,第一次回传数据未收到 +// robot_mode = robot_stop; +// } - //如果除遥控器之外都已经上线,说明机器人初始化完成,进入ready状态 - if (robot_mode == robot_run) { - if (!robot_ready) { - robot_ready = 1; - board_buzzer.start(); //蜂鸣器播放初始化提示音 - } - } +// //如果除遥控器之外都已经上线,说明机器人初始化完成,进入ready状态 +// if (robot_mode == robot_run) { +// if (!robot_ready) { +// robot_ready = 1; +// board_buzzer.start(); //蜂鸣器播放初始化提示音 +// } +// } - //遥控器判断 - if (!remote.monitor_item.is_online()) { - //遥控器掉线 - robot_mode = robot_stop; - } else if (remote.data.input_mode == dt7Remote::RC_Stop) { - //遥控器处于stop档位 - robot_mode = robot_stop; - } +// //遥控器判断 +// if (!remote.monitor_item.is_online()) { +// //遥控器掉线 +// robot_mode = robot_stop; +// } else if (remote.data.input_mode == dt7Remote::RC_Stop) { +// //遥控器处于stop档位 +// robot_mode = robot_stop; +// } - //第二步,机器人控制主要逻辑 - // STOP模式 - if (robot_mode == robot_stop) { - stop_mode_update(); - } - // 运行模式 - if (robot_mode == robot_run) { - //遥控器控制模式 - if (remote.data.input_mode == dt7Remote::RC_Remote) { - remote_mode_update(); - } else if (remote.data.input_mode == dt7Remote::RC_MouseKey) { - mouse_key_mode_update(); - } - } - //发布命令,板间通信 - send_cmd_and_data(); -} +// //第二步,机器人控制主要逻辑 +// // STOP模式 +// if (robot_mode == robot_stop) { +// stop_mode_update(); +// } +// // 运行模式 +// if (robot_mode == robot_run) { +// //遥控器控制模式 +// if (remote.data.input_mode == dt7Remote::RC_Remote) { +// remote_mode_update(); +// } else if (remote.data.input_mode == dt7Remote::RC_MouseKey) { +// mouse_key_mode_update(); +// } +// } +// //发布命令,板间通信 +// send_cmd_and_data(); +// } -void gimbal_board_cmd::stop_mode_update() { - board_send.robot_mode = robot_stop; - board_send.chassis_mode = chassis_stop; - gimbal_control.gimbal_mode = gimbal_stop; - shoot_control.shoot_mode = shoot_stop; - shoot_control.bullet_mode = bullet_holdon; -} +// void gimbal_board_cmd::stop_mode_update() { +// board_send.robot_mode = robot_stop; +// board_send.chassis_mode = chassis_stop; +// gimbal_control.gimbal_mode = gimbal_stop; +// shoot_control.shoot_mode = shoot_stop; +// shoot_control.bullet_mode = bullet_holdon; +// } -void gimbal_board_cmd::remote_mode_update() { - //云台控制参数 - gimbal_control.gimbal_mode = gimbal_run; - gimbal_control.yaw -= 0.04f * ((float)remote.data.rc.ch2 - CHx_BIAS); - gimbal_control.pitch = -1.0f * ((float)remote.data.rc.ch3 - CHx_BIAS); - gimbal_control.rotate_feedforward = 0; //目前没有使用小陀螺前馈 +// void gimbal_board_cmd::remote_mode_update() { +// //云台控制参数 +// gimbal_control.gimbal_mode = gimbal_run; +// gimbal_control.yaw -= 0.04f * ((float)remote.data.rc.ch2 - CHx_BIAS); +// gimbal_control.pitch = -1.0f * ((float)remote.data.rc.ch3 - CHx_BIAS); +// gimbal_control.rotate_feedforward = 0; //目前没有使用小陀螺前馈 - //底盘控制参数 - if (remote.data.rc.s1 == 1) { - //小陀螺模式 - board_send.chassis_mode = chassis_rotate_run; - board_send.chassis_speed.vy = 16.0 * 0.6 * (float)(remote.data.rc.ch1 - CHx_BIAS); - board_send.chassis_speed.vx = 16.0 * 0.6 * (float)(remote.data.rc.ch0 - CHx_BIAS); - } else { - //底盘跟随模式 - board_send.chassis_mode = chassis_run_follow_offset; - board_send.chassis_speed.vy = 16.0 * (float)(remote.data.rc.ch1 - CHx_BIAS); - board_send.chassis_speed.vx = 16.0 * (float)(remote.data.rc.ch0 - CHx_BIAS); - } +// //底盘控制参数 +// if (remote.data.rc.s1 == 1) { +// //小陀螺模式 +// board_send.chassis_mode = chassis_rotate_run; +// board_send.chassis_speed.vy = 16.0 * 0.6 * (float)(remote.data.rc.ch1 - CHx_BIAS); +// board_send.chassis_speed.vx = 16.0 * 0.6 * (float)(remote.data.rc.ch0 - CHx_BIAS); +// } else { +// //底盘跟随模式 +// board_send.chassis_mode = chassis_run_follow_offset; +// board_send.chassis_speed.vy = 16.0 * (float)(remote.data.rc.ch1 - CHx_BIAS); +// board_send.chassis_speed.vx = 16.0 * (float)(remote.data.rc.ch0 - CHx_BIAS); +// } - //发射机构控制 - if (remote.data.rc.s1 == 2) { - shoot_control.bullet_mode = bullet_holdon; //在发射停止位关闭播弹轮 - shoot_control.shoot_mode = shoot_stop; //在发射停止位关闭摩擦轮 - //弹仓盖控制 - if (remote.data.rc.ch4 > CHx_BIAS + 400) shoot_control.mag_mode = magazine_open; - if (remote.data.rc.ch4 < CHx_BIAS - 400) shoot_control.mag_mode = magazine_close; - } else { - //发弹控制 - shoot_control.shoot_mode = shoot_run; //开摩擦轮 - shoot_control.bullet_mode = bullet_continuous; //连续发射 - shoot_control.fire_rate = 0.01f * (float)(remote.data.rc.ch4 - CHx_BIAS); //射频 - shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余 - shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限 - } -} +// //发射机构控制 +// if (remote.data.rc.s1 == 2) { +// shoot_control.bullet_mode = bullet_holdon; //在发射停止位关闭播弹轮 +// shoot_control.shoot_mode = shoot_stop; //在发射停止位关闭摩擦轮 +// //弹仓盖控制 +// if (remote.data.rc.ch4 > CHx_BIAS + 400) shoot_control.mag_mode = magazine_open; +// if (remote.data.rc.ch4 < CHx_BIAS - 400) shoot_control.mag_mode = magazine_close; +// } else { +// //发弹控制 +// shoot_control.shoot_mode = shoot_run; //开摩擦轮 +// shoot_control.bullet_mode = bullet_continuous; //连续发射 +// shoot_control.fire_rate = 0.01f * (float)(remote.data.rc.ch4 - CHx_BIAS); //射频 +// shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余 +// shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限 +// } +// } -void gimbal_board_cmd::mouse_key_mode_update() { - // r:小陀螺模式 - if (remote.data.key_single_press_cnt.r != remote.last_data.key_single_press_cnt.r) { - if (board_send.chassis_mode != chassis_rotate_run) { - board_send.chassis_mode = chassis_rotate_run; //小陀螺模式 - gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 - } else { - board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 - gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 - } - } - // v:云台底盘独立模式 - if (remote.data.key_single_press_cnt.v != remote.last_data.key_single_press_cnt.v) { - if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_run) { - board_send.chassis_mode = chassis_run; //底盘独立模式 - gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 - } else { - board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 - gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 - } - } - // x:云台跟随底盘模式 - if (remote.data.key_single_press_cnt.x != remote.last_data.key_single_press_cnt.x) { - if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_middle) { - board_send.chassis_mode = chassis_run; //底盘独立模式 - gimbal_control.gimbal_mode = gimbal_run; //云台跟随底盘模式 - } else { - board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 - gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 - } - } - // z:爬坡模式(待添加) - // f:正常自瞄模式 - if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { - if (autoaim_mode != auto_aim_normal) { - autoaim_mode = auto_aim_normal; - } else { - autoaim_mode = auto_aim_off; - } - } - // g:小符 - if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { - if (autoaim_mode != auto_aim_buff_small) { - autoaim_mode = auto_aim_buff_small; - } else { - autoaim_mode = auto_aim_off; - } - } - // b:大符 - if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { - if (autoaim_mode != auto_aim_buff_big) { - autoaim_mode = auto_aim_buff_big; - } else { - autoaim_mode = auto_aim_off; - } - } - pc.pc_send_data.auto_mode_flag = autoaim_mode; - // c:开关弹仓 - if (remote.data.key_single_press_cnt.c != remote.last_data.key_single_press_cnt.c) { - if (shoot_control.mag_mode == magazine_open) { - shoot_control.mag_mode = magazine_close; - } else { - shoot_control.mag_mode = magazine_open; - } - } - //底盘控制参数 - // wasd - if (remote.data.key_down.w) board_send.chassis_speed.vy = 8000; - if (remote.data.key_down.s) board_send.chassis_speed.vy = -8000; - if (remote.data.key_down.d) board_send.chassis_speed.vx = 8000; - if (remote.data.key_down.a) board_send.chassis_speed.vx = -8000; +// void gimbal_board_cmd::mouse_key_mode_update() { +// // r:小陀螺模式 +// if (remote.data.key_single_press_cnt.r != remote.last_data.key_single_press_cnt.r) { +// if (board_send.chassis_mode != chassis_rotate_run) { +// board_send.chassis_mode = chassis_rotate_run; //小陀螺模式 +// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 +// } else { +// board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 +// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 +// } +// } +// // v:云台底盘独立模式 +// if (remote.data.key_single_press_cnt.v != remote.last_data.key_single_press_cnt.v) { +// if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_run) { +// board_send.chassis_mode = chassis_run; //底盘独立模式 +// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 +// } else { +// board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 +// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 +// } +// } +// // x:云台跟随底盘模式 +// if (remote.data.key_single_press_cnt.x != remote.last_data.key_single_press_cnt.x) { +// if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_middle) { +// board_send.chassis_mode = chassis_run; //底盘独立模式 +// gimbal_control.gimbal_mode = gimbal_run; //云台跟随底盘模式 +// } else { +// board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 +// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 +// } +// } +// // z:爬坡模式(待添加) +// // f:正常自瞄模式 +// if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { +// if (autoaim_mode != auto_aim_normal) { +// autoaim_mode = auto_aim_normal; +// } else { +// autoaim_mode = auto_aim_off; +// } +// } +// // g:小符 +// if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { +// if (autoaim_mode != auto_aim_buff_small) { +// autoaim_mode = auto_aim_buff_small; +// } else { +// autoaim_mode = auto_aim_off; +// } +// } +// // b:大符 +// if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { +// if (autoaim_mode != auto_aim_buff_big) { +// autoaim_mode = auto_aim_buff_big; +// } else { +// autoaim_mode = auto_aim_off; +// } +// } +// pc.pc_send_data.auto_mode_flag = autoaim_mode; +// // c:开关弹仓 +// if (remote.data.key_single_press_cnt.c != remote.last_data.key_single_press_cnt.c) { +// if (shoot_control.mag_mode == magazine_open) { +// shoot_control.mag_mode = magazine_close; +// } else { +// shoot_control.mag_mode = magazine_open; +// } +// } +// //底盘控制参数 +// // wasd +// if (remote.data.key_down.w) board_send.chassis_speed.vy = 8000; +// if (remote.data.key_down.s) board_send.chassis_speed.vy = -8000; +// if (remote.data.key_down.d) board_send.chassis_speed.vx = 8000; +// if (remote.data.key_down.a) board_send.chassis_speed.vx = -8000; - //按住ctrl减速 - if (remote.data.key_down.ctrl) { - board_send.chassis_speed.vx /= 3.0; - board_send.chassis_speed.vy /= 3.0; - } +// //按住ctrl减速 +// if (remote.data.key_down.ctrl) { +// board_send.chassis_speed.vx /= 3.0; +// board_send.chassis_speed.vy /= 3.0; +// } - //按住shift加速 - if (remote.data.key_down.shift) { - board_send.chassis_speed.vx *= 3.0; - board_send.chassis_speed.vy *= 3.0; - } +// //按住shift加速 +// if (remote.data.key_down.shift) { +// board_send.chassis_speed.vx *= 3.0; +// board_send.chassis_speed.vy *= 3.0; +// } - // q和e - if (remote.data.key_down.q) { - if (board_send.chassis_mode == chassis_run) { - board_send.chassis_speed.rotate = -90; - } else if (board_send.chassis_mode == chassis_run_follow_offset) { - gimbal_control.yaw -= 15; //通过控制云台yaw轴带动底盘 - } - } +// // q和e +// if (remote.data.key_down.q) { +// if (board_send.chassis_mode == chassis_run) { +// board_send.chassis_speed.rotate = -90; +// } else if (board_send.chassis_mode == chassis_run_follow_offset) { +// gimbal_control.yaw -= 15; //通过控制云台yaw轴带动底盘 +// } +// } - if (remote.data.key_down.e) { - if (board_send.chassis_mode == chassis_run) { - board_send.chassis_speed.rotate = 90; - } else if (board_send.chassis_mode == chassis_run_follow_offset) { - gimbal_control.yaw += 15; //通过控制云台yaw轴带动底盘 - } - } +// if (remote.data.key_down.e) { +// if (board_send.chassis_mode == chassis_run) { +// board_send.chassis_speed.rotate = 90; +// } else if (board_send.chassis_mode == chassis_run_follow_offset) { +// gimbal_control.yaw += 15; //通过控制云台yaw轴带动底盘 +// } +// } - //云台控制参数 - if (gimbal_control.gimbal_mode == gimbal_run) { - if (autoaim_mode == auto_aim_off) { - gimbal_control.yaw -= 0.5f * (0.7f * (remote.data.mouse.x) + 0.3f * (remote.last_data.mouse.x)); - gimbal_control.pitch -= -0.1f * ((float)remote.data.mouse.y); - } else { - //计算真实yaw值 - float yaw_target = pc.pc_recv_data->yaw * 8192.0 / 2 / pi + gimbal_upload_data->gimbal_imu->round * 8192.0; - if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] > pi) yaw_target -= 8192; - if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] < -pi) yaw_target += 8192; - gimbal_control.yaw = yaw_target; - gimbal_control.pitch = pc.pc_recv_data->roll * 8192.0 / 2 / pi; // 根据当前情况决定,pitch轴反馈为陀螺仪roll - } - } else if (gimbal_control.gimbal_mode == gimbal_middle) { - //云台跟随底盘模式,待完善 - } +// //云台控制参数 +// if (gimbal_control.gimbal_mode == gimbal_run) { +// if (autoaim_mode == auto_aim_off) { +// gimbal_control.yaw -= 0.5f * (0.7f * (remote.data.mouse.x) + 0.3f * (remote.last_data.mouse.x)); +// gimbal_control.pitch -= -0.1f * ((float)remote.data.mouse.y); +// } else { +// //计算真实yaw值 +// float yaw_target = pc.pc_recv_data->yaw * 8192.0 / 2 / pi + gimbal_upload_data->gimbal_imu->round * 8192.0; +// if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] > pi) yaw_target -= 8192; +// if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] < -pi) yaw_target += 8192; +// gimbal_control.yaw = yaw_target; +// gimbal_control.pitch = pc.pc_recv_data->roll * 8192.0 / 2 / pi; // 根据当前情况决定,pitch轴反馈为陀螺仪roll +// } +// } else if (gimbal_control.gimbal_mode == gimbal_middle) { +// //云台跟随底盘模式,待完善 +// } - //发射机构控制参数 - if (remote.data.rc.s1 == 2) { - shoot_control.bullet_mode = bullet_holdon; - shoot_control.shoot_mode = shoot_stop; - } else { - //发弹控制,单发,双发, 射频和小电脑控制待完善 - shoot_control.shoot_mode = shoot_run; //开摩擦轮 - shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余 - shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限 - shoot_control.fire_rate = 3; //固定射频 - if (remote.data.mouse.press_l) { - shoot_control.bullet_mode = bullet_continuous; - } else { - shoot_control.bullet_mode = bullet_holdon; - } - } -} +// //发射机构控制参数 +// if (remote.data.rc.s1 == 2) { +// shoot_control.bullet_mode = bullet_holdon; +// shoot_control.shoot_mode = shoot_stop; +// } else { +// //发弹控制,单发,双发, 射频和小电脑控制待完善 +// shoot_control.shoot_mode = shoot_run; //开摩擦轮 +// shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余 +// shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限 +// shoot_control.fire_rate = 3; //固定射频 +// if (remote.data.mouse.press_l) { +// shoot_control.bullet_mode = bullet_continuous; +// } else { +// shoot_control.bullet_mode = bullet_holdon; +// } +// } +// } -void gimbal_board_cmd::send_cmd_and_data() { - static publisher gimbal_puber("cmd_gimbal"); - static publisher shoot_puber("cmd_shoot"); - gimbal_puber.push(&gimbal_control); - shoot_puber.push(&shoot_control); - sender.send(board_send); - pc.send(pc.pc_send_data); -} \ No newline at end of file +// void gimbal_board_cmd::send_cmd_and_data() { +// static publisher gimbal_puber("cmd_gimbal"); +// static publisher shoot_puber("cmd_shoot"); +// gimbal_puber.push(&gimbal_control); +// shoot_puber.push(&shoot_control); +// sender.send(board_send); +// pc.send(pc.pc_send_data); +// } \ No newline at end of file diff --git a/application/cmd/gimbal_cmd.h b/application/cmd/gimbal_cmd.h index 90ad4e8..eff9bc9 100644 --- a/application/cmd/gimbal_cmd.h +++ b/application/cmd/gimbal_cmd.h @@ -1,35 +1,35 @@ -#ifndef _GIMBAL_ROBOT_CMD_H -#define _GIMBAL_ROBOT_CMD_H -#include -#include -#include -#include -#include -class gimbal_board_cmd { - private: - gimbal_board_send board_send; //需要发送给下板的板间通信数据 - cmd_gimbal gimbal_control; //发送给云台的控制量 - cmd_shoot shoot_control; //发送给发射机构的控制量 +// #ifndef _GIMBAL_ROBOT_CMD_H +// #define _GIMBAL_ROBOT_CMD_H +// #include +// #include +// #include +// #include +// #include +// class gimbal_board_cmd { +// private: +// gimbal_board_send board_send; //需要发送给下板的板间通信数据 +// cmd_gimbal gimbal_control; //发送给云台的控制量 +// cmd_shoot shoot_control; //发送给发射机构的控制量 - can_send sender; //板间通信发送 - can_recv recver; //板间通信接收 - chassis_board_send* board_recv; //板间通信接收数据指针 - canpc pc; //小电脑视觉数据 - dt7Remote remote; //遥控器 - Robot_mode robot_mode; //机器人模式 - uint8_t robot_ready; //机器人准备好标志位 - AutoAim_mode autoaim_mode; //机器人自瞄模式 - buzzer board_buzzer; //蜂鸣器 - upload_gimbal* gimbal_upload_data; //云台模块回传的数据 +// can_send sender; //板间通信发送 +// can_recv recver; //板间通信接收 +// chassis_board_send* board_recv; //板间通信接收数据指针 +// canpc pc; //小电脑视觉数据 +// dt7Remote remote; //遥控器 +// Robot_mode robot_mode; //机器人模式 +// uint8_t robot_ready; //机器人准备好标志位 +// AutoAim_mode autoaim_mode; //机器人自瞄模式 +// buzzer board_buzzer; //蜂鸣器 +// upload_gimbal* gimbal_upload_data; //云台模块回传的数据 - void stop_mode_update(); //机器人停止模式更新函数 - void remote_mode_update(); //机器人遥控器模式更新函数 - void mouse_key_mode_update(); //机器人键鼠模式更新函数 - void send_cmd_and_data(); //发布指令和板间通信 +// void stop_mode_update(); //机器人停止模式更新函数 +// void remote_mode_update(); //机器人遥控器模式更新函数 +// void mouse_key_mode_update(); //机器人键鼠模式更新函数 +// void send_cmd_and_data(); //发布指令和板间通信 - public: - gimbal_board_cmd(); - void update(); -}; +// public: +// gimbal_board_cmd(); +// void update(); +// }; -#endif \ No newline at end of file +// #endif \ No newline at end of file diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index 57e882d..684046c 100644 --- a/bsp/bsp_can.c +++ b/bsp/bsp_can.c @@ -57,7 +57,7 @@ static void CANServiceInit() /* -----------------------two extern callable function -----------------------*/ -void CANRegister(can_instance *ins, can_instance_config_s config) +void CANRegister(can_instance *ins, can_instance_config_s* config) { static uint8_t idx; if (!idx) @@ -66,15 +66,15 @@ void CANRegister(can_instance *ins, can_instance_config_s config) } instance[idx] = ins; - instance[idx]->txconf.StdId = config.tx_id; + instance[idx]->txconf.StdId = config->tx_id; instance[idx]->txconf.IDE = CAN_ID_STD; instance[idx]->txconf.RTR = CAN_RTR_DATA; instance[idx]->txconf.DLC = 0x08; - instance[idx]->can_handle = config.can_handle; - instance[idx]->tx_id = config.tx_id; - instance[idx]->rx_id = config.rx_id; - instance[idx]->can_module_callback = config.can_module_callback; + instance[idx]->can_handle = config->can_handle; + instance[idx]->tx_id = config->tx_id; + instance[idx]->rx_id = config->rx_id; + instance[idx]->can_module_callback = config->can_module_callback; CANAddFilter(instance[idx++]); } diff --git a/bsp/bsp_can.h b/bsp/bsp_can.h index 7ca8948..06d033c 100644 --- a/bsp/bsp_can.h +++ b/bsp/bsp_can.h @@ -9,7 +9,9 @@ #define MX_CAN_FILTER_CNT (2 * 14) // temporarily useless #define DEVICE_CAN_CNT 2 // CAN1,CAN2 + /* can instance typedef, every module registered to CAN should have this variable */ +#pragma pack(1) typedef struct _ { CAN_HandleTypeDef *can_handle; @@ -22,6 +24,7 @@ typedef struct _ uint8_t rx_len; void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones } can_instance; +#pragma pack() /* this structure is used as initialization*/ typedef struct @@ -32,8 +35,8 @@ typedef struct void (*can_module_callback)(can_instance *); } can_instance_config_s; -/* module callback,which resolve protocol when new mesg arrives*/ -typedef void (*can_callback)(can_instance *); +/* module callback,which resolve protocol when new mesg arrives */ +typedef void (*can_callback)(can_instance*); /** * @brief transmit mesg through CAN device @@ -48,14 +51,14 @@ void CANTransmit(can_instance *_instance); * @param config init config * @return can_instance* can instance owned by module */ -void CANRegister(can_instance *instance, can_instance_config_s config); +void CANRegister(can_instance *instance, can_instance_config_s *config); /** * @brief 修改CAN发送报文的数据帧长度;注意最大长度为8,在没有进行修改的时候,默认长度为8 - * + * * @param _instance 要修改长度的can实例 * @param length 设定长度 */ -void CANSetDLC(can_instance *_instance,uint8_t length); +void CANSetDLC(can_instance *_instance, uint8_t length); #endif diff --git a/modules/can_comm/can_comm.c b/modules/can_comm/can_comm.c index 6a3e69b..37eaa05 100644 --- a/modules/can_comm/can_comm.c +++ b/modules/can_comm/can_comm.c @@ -1,16 +1,28 @@ #include "can_comm.h" #include "memory.h" #include "stdlib.h" -#include "crc8.h" +#include "crc8.h" /* can_comm用于保存每个实例的指针数组,用于回调函数区分实例 */ static CANCommInstance *can_comm_instance[MX_CAN_COMM_COUNT] = {NULL}; -static uint8_t idx; //配合can_comm_instance的初始化使用,标识当前初始化的是哪一个实例 +static uint8_t idx; // 配合can_comm_instance的初始化使用,标识当前初始化的是哪一个实例 /** - * @brief - * - * @param _instance + * @brief 重置CAN comm的接收状态和buffer + * + * @param ins 需要重置的实例 + */ +static void CANCommResetRx(CANCommInstance *ins) +{ + memset(ins->raw_recvbuf, 0, ins->cur_recv_len); + ins->recv_state = 0; + ins->cur_recv_len = 0; +} + +/** + * @brief + * + * @param _instance */ static void CANCommRxCallback(can_instance *_instance) { @@ -18,75 +30,84 @@ static void CANCommRxCallback(can_instance *_instance) { if (&can_comm_instance[i]->can_ins == _instance) // 遍历,找到对应的接收CAN COMM实例 { - if(_instance->rx_buff[0]==CAN_COMM_HEADER && can_comm_instance[i]->recv_state==0) //尚未开始接收且新的一包里有帧头 + /* 接收状态判断 */ + if (_instance->rx_buff[0] == CAN_COMM_HEADER && can_comm_instance[i]->recv_state == 0) // 尚未开始接收且新的一包里有帧头 { - if(_instance->rx_buff[1]==can_comm_instance[i]->recv_data_len) // 接收长度等于设定接收长度 + if (_instance->rx_buff[1] == can_comm_instance[i]->recv_data_len) // 接收长度等于设定接收长度 { - can_comm_instance[i]->recv_state=1; + can_comm_instance[i]->recv_state = 1; } else - return ; //直接跳过即可 + return; // 直接跳过即可 } - if(can_comm_instance[i]->recv_state) //已经开始接收 - { // 直接拷贝到当前的接收buffer后面 - memcpy(can_comm_instance[i]->raw_recvbuf+can_comm_instance[i]->cur_recv_len,_instance->rx_buff,8); - can_comm_instance[i]->cur_recv_len+=8; + if (can_comm_instance[i]->recv_state) // 已经收到过帧头 + { // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误 + if (can_comm_instance[i]->cur_recv_len + _instance->rx_len > can_comm_instance[i]->recv_buf_len) + { + CANCommResetRx(can_comm_instance[i]); + return; // 重置状态然后返回 + } + + // 直接拷贝到当前的接收buffer后面 + memcpy(can_comm_instance[i]->raw_recvbuf + can_comm_instance[i]->cur_recv_len, _instance->rx_buff, _instance->rx_len); + can_comm_instance[i]->cur_recv_len += _instance->rx_len; + // 当前已经收满 - if(can_comm_instance[i]->cur_recv_len>=can_comm_instance[i]->recv_buf_len) - { // buff里本该是tail的位置不等于CAN_COMM_TAIL - if(can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len-1]!=CAN_COMM_TAIL) - { - memset(can_comm_instance[i]->raw_recvbuf,0,can_comm_instance[i]->recv_buf_len); - can_comm_instance[i]->recv_state=0; - can_comm_instance[i]->cur_recv_len=0; - return ; // 重置状态然后返回 + if (can_comm_instance[i]->cur_recv_len == can_comm_instance[i]->recv_buf_len) + { // buff里本该是tail的位置不等于CAN_COMM_TAIL + if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 1] != CAN_COMM_TAIL) + { + CANCommResetRx(can_comm_instance[i]); + return; // 重置状态然后返回 } else // tail正确, 对数据进行crc8校验 { - if(can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len-2] == - crc_8(can_comm_instance[i]->raw_recvbuf+2,can_comm_instance[i]->recv_data_len)) - { // 通过校验,复制数据到unpack_data中 - memcpy(can_comm_instance[i]->raw_recvbuf+2,can_comm_instance[i]->unpacked_recv_data,can_comm_instance[i]->recv_data_len); - can_comm_instance[i]->update_flag=1; //数据更新flag置为1 + if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 2] == + crc_8(can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len)) + { // 通过校验,复制数据到unpack_data中 + memcpy(can_comm_instance[i]->unpacked_recv_data,can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len); + can_comm_instance[i]->update_flag = 1; // 数据更新flag置为1 } - memset(can_comm_instance[i]->raw_recvbuf,0,can_comm_instance[i]->recv_buf_len); //整个buff置零 - can_comm_instance[i]->recv_state=0; - can_comm_instance[i]->cur_recv_len=0; - return ; // 重置状态然后返回 + CANCommResetRx(can_comm_instance[i]); + return; // 重置状态然后返回 } } } - return ; + return; // 访问完一个can comm直接退出,一次中断只会也只可能会处理一个实例的回调 } } } -CANCommInstance *CANCommInit(CANComm_Init_Config_s config) +CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config) { can_comm_instance[idx] = (CANCommInstance *)malloc(sizeof(CANCommInstance)); memset(can_comm_instance[idx], 0, sizeof(CANCommInstance)); - can_comm_instance[idx]->recv_data_len = config.recv_data_len; - can_comm_instance[idx]->send_data_len = config.send_data_len; - can_comm_instance[idx]->send_buf_len = config.send_data_len + CAN_COMM_OFFSET_BYTES; + can_comm_instance[idx]->recv_data_len = comm_config->recv_data_len; + can_comm_instance[idx]->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES; + can_comm_instance[idx]->send_data_len = comm_config->send_data_len; + can_comm_instance[idx]->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES; can_comm_instance[idx]->raw_sendbuf[0] = CAN_COMM_HEADER; - can_comm_instance[idx]->raw_sendbuf[1] = config.send_data_len; - can_comm_instance[idx]->raw_sendbuf[config.send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL; + can_comm_instance[idx]->raw_sendbuf[1] = comm_config->send_data_len; + can_comm_instance[idx]->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL; - config.can_config.can_module_callback = CANCommRxCallback; - CANRegister(&can_comm_instance[idx]->can_ins, config.can_config); + comm_config->can_config.can_module_callback = CANCommRxCallback; + CANRegister(&can_comm_instance[idx]->can_ins, &comm_config->can_config); return can_comm_instance[idx++]; } void CANCommSend(CANCommInstance *instance, uint8_t *data) { static uint8_t crc8; + static uint8_t send_len; memcpy(instance->raw_sendbuf + 2, data, instance->send_data_len); crc8 = crc_8(data, instance->send_data_len); instance->raw_sendbuf[2 + instance->send_data_len] = crc8; for (size_t i = 0; i < instance->send_buf_len; i += 8) - { - memcpy(instance->can_ins.tx_buff, instance->raw_sendbuf[i], 8); + { // 如果是最后一包,send len将会小于8,要修改CAN的txconf中的DLC位,调用bsp_can提供的接口即可 + send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i; + CANSetDLC(&instance->can_ins, send_len); + memcpy(instance->can_ins.tx_buff, instance->raw_sendbuf+i, send_len); CANTransmit(&instance->can_ins); } } diff --git a/modules/can_comm/can_comm.h b/modules/can_comm/can_comm.h index 429dcc7..90d7d9d 100644 --- a/modules/can_comm/can_comm.h +++ b/modules/can_comm/can_comm.h @@ -4,72 +4,74 @@ * @brief 用于多机CAN通信的收发模块 * @version 0.1 * @date 2022-11-27 - * + * * @copyright Copyright (c) 2022 HNUYueLu EC all rights reserved - * + * */ #ifndef CAN_COMM_H #define CAN_COMM_H - #include "bsp_can.h" -#define MX_CAN_COMM_COUNT 4 //注意均衡负载,一条总线上不要挂载过多的外设 +#define MX_CAN_COMM_COUNT 4 // 注意均衡负载,一条总线上不要挂载过多的外设 -#define CAN_COMM_MAX_BUFFSIZE 68 // 最大发送/接收字节数 +#define CAN_COMM_MAX_BUFFSIZE 60 // 最大发送/接收字节数,如果不够可以增加此数值 #define CAN_COMM_HEADER 's' -#define CAN_COMM_TAIL 'e' +#define CAN_COMM_TAIL 'e' #define CAN_COMM_OFFSET_BYTES 4 // 's'+datalen+'e'+crc8 -typedef struct +#pragma pack(1) +/* CAN comm 结构体, 拥有CAN comm的app应该包含一个CAN comm指针 */ +typedef struct { can_instance can_ins; /* 发送部分 */ uint8_t send_data_len; uint8_t send_buf_len; - uint8_t raw_sendbuf[CAN_COMM_MAX_BUFFSIZE+CAN_COMM_OFFSET_BYTES]; //额外4个bytes保存帧头帧尾和校验和 + uint8_t raw_sendbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和 /* 接收部分 */ uint8_t recv_data_len; uint8_t recv_buf_len; - uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE+CAN_COMM_OFFSET_BYTES]; //额外4个bytes保存帧头帧尾和校验和 - uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; + uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和 + uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; // 解包后的数据,调用CANCommGet()后cast成对应的类型通过指针读取即可 /* 接收和更新标志位*/ uint8_t recv_state; uint8_t cur_recv_len; uint8_t update_flag; } CANCommInstance; +#pragma pack() +/* CAN comm 初始化结构体 */ typedef struct { + can_instance_config_s can_config; uint8_t send_data_len; uint8_t recv_data_len; - can_instance_config_s can_config; -}CANComm_Init_Config_s; +} CANComm_Init_Config_s; /** - * @brief - * - * @param config - * @return CANCommInstance* + * @brief + * + * @param config + * @return CANCommInstance* */ -CANCommInstance* CANCommInit(CANComm_Init_Config_s config); +CANCommInstance *CANCommInit(CANComm_Init_Config_s* comm_config); /** * @brief 发送数据 - * + * * @param instance can comm实例 * @param data 注意此地址的有效数据长度需要和初始化时传入的datalen相同 */ -void CANCommSend(CANCommInstance* instance,uint8_t* data); +void CANCommSend(CANCommInstance *instance, uint8_t *data); /** * @brief 获取CAN COMM接收的数据,需要自己使用强制类型转换将返回的void指针转换成指定类型 - * + * * @return void* 返回的数据指针 * @attention 注意如果希望直接通过转换指针访问数据,如果数据是union或struct,要检查是否使用了pack(n) * CAN COMM接收到的数据可以看作是pack(1)之后的,是连续存放的 */ -void* CANCommGet(CANCommInstance* instance); - +void *CANCommGet(CANCommInstance *instance); #endif // !CAN_COMM_H \ No newline at end of file diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index db4a7e8..5a02ec9 100644 --- a/modules/motor/HT04.c +++ b/modules/motor/HT04.c @@ -40,7 +40,7 @@ joint_instance *HTMotorInit(can_instance_config_s config) { static uint8_t idx; joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance)); - CANRegister(joint_motor_info[idx++]->motor_can_instace, config); + CANRegister(joint_motor_info[idx++]->motor_can_instace, &config); return joint_motor_info[idx++]; } diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index 7baa307..cccbbe6 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -23,7 +23,7 @@ driven_instance *LKMotroInit(can_instance_config_s config) static uint8_t idx; driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance)); config.can_module_callback = DecodeDriven; - CANRegister(driven_motor_info[idx]->motor_can_instance, config); + CANRegister(driven_motor_info[idx]->motor_can_instance, &config); return driven_motor_info[idx++]; } diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 77c6339..41fd4e8 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -120,8 +120,10 @@ static void MotorSenderGrouping(can_instance_config_s *config) */ static void DecodeDJIMotor(can_instance *_instance) { + // 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取 static uint8_t *rxbuff; - static dji_motor_measure *measure; + static dji_motor_measure *measure; + for (size_t i = 0; i < DJI_MOTOR_CNT; i++) { if (&dji_motor_info[i]->motor_can_instance == _instance) @@ -131,6 +133,7 @@ static void DecodeDJIMotor(can_instance *_instance) // resolve data and apply filter to current and speed measure->last_ecd = measure->ecd; measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); + // 增加滤波 measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]); measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); measure->temperate = rxbuff[6]; @@ -146,26 +149,26 @@ static void DecodeDJIMotor(can_instance *_instance) } // 电机初始化,返回一个电机实例 -dji_motor_instance *DJIMotorInit(Motor_Init_Config_s config) +dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config) { dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance)); memset(dji_motor_info[idx], 0, sizeof(dji_motor_instance)); // motor basic setting - dji_motor_info[idx]->motor_type = config.motor_type; - dji_motor_info[idx]->motor_settings = config.controller_setting_init_config; + dji_motor_info[idx]->motor_type = config->motor_type; + dji_motor_info[idx]->motor_settings = config->controller_setting_init_config; // motor controller init - PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &config.controller_param_init_config.current_PID); - PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &config.controller_param_init_config.speed_PID); - PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config.controller_param_init_config.angle_PID); - dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config.controller_param_init_config.other_angle_feedback_ptr; - dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config.controller_param_init_config.other_speed_feedback_ptr; + PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &config->controller_param_init_config.current_PID); + PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID); + PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID); + dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; + dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; // group motors, because 4 motors share the same CAN control message - MotorSenderGrouping(&config.can_init_config); + MotorSenderGrouping(&config->can_init_config); // register motor to CAN bus - config.can_init_config.can_module_callback = DecodeDJIMotor; // set callback - CANRegister(&dji_motor_info[idx]->motor_can_instance, config.can_init_config); + config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback + CANRegister(&dji_motor_info[idx]->motor_can_instance, &config->can_init_config); return dji_motor_info[idx++]; } diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index ddd515a..6894553 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -20,6 +20,8 @@ #include "motor_def.h" #define DJI_MOTOR_CNT 12 + +/* 滤波系数设置为1的时候即关闭滤波 */ #define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.8 #define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95 @@ -77,7 +79,7 @@ typedef struct * * @return dji_motor_instance* */ -dji_motor_instance *DJIMotorInit(Motor_Init_Config_s config); +dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config); /** * @brief 被application层的应用调用,给电机设定参考值. @@ -103,4 +105,5 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba */ void DJIMotorControl(); + #endif // !DJI_MOTOR_H diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index f2a9e8c..16835ed 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -17,6 +17,7 @@ #define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x))) + /** * @brief 闭环类型,如果需要多个闭环,则使用或运算 * 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP diff --git a/modules/referee/referee.h b/modules/referee/referee.h index ec05993..5c3b256 100644 --- a/modules/referee/referee.h +++ b/modules/referee/referee.h @@ -346,4 +346,5 @@ void RefereeInit(UART_HandleTypeDef *referee_usart_handle); void JudgeReadData(uint8_t *ReadFromUsart); float JudgeGetChassisPower(void); +#pragma pack() #endif // !REFEREE_H