diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index a889936..30d57c7 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -28,7 +28,7 @@ #include "ins_task.h" #include "motor_task.h" #include "led_task.h" -#include "monitor.h" +#include "daemon.h" #include "robot.h" /* USER CODE END Includes */ @@ -60,8 +60,11 @@ void StartINSTASK(void const * argument); void StartMOTORTASK(void const * argument); +void StartDAEMONTASK(void const* argument); + void StartROBOTTASK(void const* argument); + /* USER CODE END FunctionPrototypes */ void StartDefaultTask(void const * argument); @@ -122,6 +125,9 @@ void MX_FREERTOS_Init(void) { osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256); defaultTaskHandle = osThreadCreate(osThread(motortask), NULL); + osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 512); + defaultTaskHandle = osThreadCreate(osThread(daemontask), NULL); + osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 2048); defaultTaskHandle = osThreadCreate(osThread(robottask), NULL); @@ -176,6 +182,16 @@ void StartMOTORTASK(void const * argument) } } +void StartDAEMONTASK(void const * argument) +{ + while (1) + { + //500Hz + DaemonTask(); + osDelay(10); + } +} + void StartROBOTTASK(void const * argument) { while (1) diff --git a/Makefile b/Makefile index 6b357e7..6b3fde3 100644 --- a/Makefile +++ b/Makefile @@ -132,7 +132,7 @@ modules/remote/remote_control.c \ modules/super_cap/super_cap.c \ modules/can_comm/can_comm.c \ modules/message_center/message_center.c \ -modules/monitor/monitor.c \ +modules/daemon/daemon.c \ modules/vofa/vofa.c \ application/gimbal/gimbal.c \ application/chassis/chassis.c \ @@ -230,7 +230,7 @@ C_INCLUDES = \ -Imodules/super_cap \ -Imodules/can_comm \ -Imodules/message_center \ --Imodules/monitor \ +-Imodules/daemon \ -Imodules/vofa \ -Imodules/ diff --git a/TODO.md b/TODO.md index de99703..5b72dd7 100644 --- a/TODO.md +++ b/TODO.md @@ -20,14 +20,14 @@ #### bsp_buzzer -需要和**monitor**模块配合。 +需要和**daemon**模块配合。 - [ ] 增加错误或异常提示音 - [ ] 增加功能提示音 #### bsp_led -需要和**monitor**模块配合。这同时会影响到module层的led_task，放在这一并解决。 +需要和**daemon**模块配合。这同时会影响到module层的led_task，放在这一并解决。 - [ ] 增加错误或异常流水灯 - [ ] 增加功能点灯 @@ -81,11 +81,11 @@ - [ ] 编写舵机模块 -#### ==monitor== +#### ==daemon== 应用和模块监视功能，用于提供掉线和异常检测与应对机制，和底层的buzzer、led、oled等配合提供快速的问题定位。 -- [ ] 编写monitor +- [ ] 编写daemon #### LKMotor diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index bac89d4..ba2e8f8 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -57,12 +57,9 @@ static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出 void ChassisInit() { - // 左前轮 - Motor_Init_Config_s left_foward_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 1, - }, + // 四个轮子的参数一样,改tx_id和反转标志位即可 + Motor_Init_Config_s chassis_motor_config = { + .can_init_config.can_handle=&hcan2, .controller_param_init_config = { .speed_PID = { @@ -76,80 +73,24 @@ void ChassisInit() .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, }, .motor_type = M3508}; - // 右前轮 - Motor_Init_Config_s right_foward_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 2, - }, - .controller_param_init_config = { - .speed_PID = { + + chassis_motor_config.can_init_config.tx_id=1; + chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + motor_lf = DJIMotorInit(&chassis_motor_config); - }, - .current_PID = { + chassis_motor_config.can_init_config.tx_id=2, + chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + motor_rf = DJIMotorInit(&chassis_motor_config); + + chassis_motor_config.can_init_config.tx_id=3, + chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + motor_lb = DJIMotorInit(&chassis_motor_config); - }, - }, - .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - .outer_loop_type = SPEED_LOOP, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, - }, - .motor_type = M3508}; - // 左后轮 - Motor_Init_Config_s left_back_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 3, - }, - .controller_param_init_config = { - .speed_PID = { - - }, - .current_PID = { - - }, - }, - .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - .outer_loop_type = SPEED_LOOP, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, - }, - .motor_type = M3508}; - // 右后轮 - Motor_Init_Config_s right_back_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 4, - }, - .controller_param_init_config = { - .speed_PID = { - - }, - .current_PID = { - - }, - }, - .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - .outer_loop_type = SPEED_LOOP, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, - }, - .motor_type = M3508}; - - motor_lf = DJIMotorInit(&left_foward_config); - motor_rf = DJIMotorInit(&right_foward_config); - motor_lb = DJIMotorInit(&left_back_config); - motor_rb = DJIMotorInit(&right_back_config); + chassis_motor_config.can_init_config.tx_id=4, + chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE; + motor_rb = DJIMotorInit(&chassis_motor_config); referee_data = RefereeInit(&huart6); diff --git a/modules/daemon/daemon.c b/modules/daemon/daemon.c new file mode 100644 index 0000000..2733c91 --- /dev/null +++ b/modules/daemon/daemon.c @@ -0,0 +1,43 @@ +#include "daemon.h" +#include "bsp_dwt.h" // 后续通过定时器来计时? + +static DaemonInstance *daemon_instances[DAEMON_MX_CNT] = {NULL}; +static uint8_t idx; + +DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config) +{ + + daemon_instances[idx] = (DaemonInstance *)malloc(sizeof(DaemonInstance)); + + daemon_instances[idx]->reload_count = config->reload_count; + daemon_instances[idx]->callback = config->callback; + daemon_instances[idx]->owner_id = config->id; + daemon_instances[idx]->temp_count = config->reload_count; + + return daemon_instances[idx++]; +} + +void DaemonReload(DaemonInstance *instance) +{ + instance->temp_count = instance->reload_count; +} + +uint8_t DaemonIsOnline(DaemonInstance *instance) +{ + return instance->temp_count>0; +} + +void DaemonTask() +{ + static DaemonInstance* pins; //提高可读性同时降低访存开销 + for (size_t i = 0; i < idx; i++) + { + pins=daemon_instances[i]; + if(pins->temp_count>0) + pins->temp_count--; + else if(pins->callback) + { // 每个module根据自身的offline callback进行调用 + pins->callback(pins->owner_id); // module将owner_id强制类型转换成自身类型 + } + } +} diff --git a/modules/daemon/daemon.h b/modules/daemon/daemon.h new file mode 100644 index 0000000..689b266 --- /dev/null +++ b/modules/daemon/daemon.h @@ -0,0 +1,60 @@ +#ifndef MONITOR_H +#define MONITOR_H + +#include "stdint-gcc.h" + +#define DAEMON_MX_CNT 64 + +/* 模块离线处理函数指针 */ +typedef void (*offline_callback)(void *); + +/* daemon结构体定义 */ +typedef struct daemon_ins +{ + uint16_t reload_count; // 重载值 + offline_callback callback; // 异常处理函数,当模块发生异常时会被调用 + + uint16_t temp_count; //当前值,减为零说明模块离线或异常 + void *owner_id; // daemon实例的地址,初始化的时候填入 + +} DaemonInstance; + +/* daemon初始化配置 */ +typedef struct +{ + uint16_t reload_count; // 实际上这是app唯一需要设置的值? + offline_callback callback; + void *id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型 +} Daemon_Init_Config_s; + +/** + * @brief 注册一个daemon实例 + * + * @param config 初始化配置 + * @return DaemonInstance* 返回实例指针 + */ +DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config); + +/** + * @brief 当模块收到新的数据或进行其他动作时,调用该函数重载temp_count,相当于"喂狗" + * + * @param instance daemon实例指针 + */ +void DaemonReload(DaemonInstance *instance); + +/** + * @brief 确认模块是否离线 + * + * @param instance daemon实例指针 + * @return uint8_t 若在线且工作正常,返回1;否则返回零. 后续根据异常类型和离线状态等进行优化. + */ +uint8_t DaemonIsOnline(DaemonInstance *instance); + +/** + * @brief 放入rtos中,会给每个daemon实例的temp_count按频率进行递减操作. + * 模块成功接受数据或成功操作则会重载temp_count的值为reload_count. + * + */ +void DaemonTask(); + +#endif // !MONITOR_H \ No newline at end of file diff --git a/modules/daemon/daemon.md b/modules/daemon/daemon.md new file mode 100644 index 0000000..429f977 --- /dev/null +++ b/modules/daemon/daemon.md @@ -0,0 +1,5 @@ +daemon + +

neozng1@hnu.edu.cn

+ +用于监测模块和应用运行情况的module(和官方代码中的deteck task作用相同) \ No newline at end of file diff --git a/modules/monitor/monitor.c b/modules/monitor/monitor.c deleted file mode 100644 index d3e4ad6..0000000 --- a/modules/monitor/monitor.c +++ /dev/null @@ -1,3 +0,0 @@ -#include "monitor.h" -#include "bsp_dwt.h" - diff --git a/modules/monitor/monitor.h b/modules/monitor/monitor.h deleted file mode 100644 index 5048c55..0000000 --- a/modules/monitor/monitor.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef MONITOR_H -#define MONITOR_H - - - -#endif // !MONITOR_H \ No newline at end of file diff --git a/modules/monitor/monitor.md b/modules/monitor/monitor.md deleted file mode 100644 index e69de29..0000000 diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index e958f7e..a9415d3 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -10,8 +10,10 @@ static DJIMotorInstance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; * @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送 * 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行 * + * C610(m2006)/C620(m3508):0x1ff,0x200; GM6020:0x1ff,0x2ff + * 反馈: GM6020: 0x204+id ; C610/C620: 0x200+id * can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF - * can2: [0]:0x1FF,[1]:0x200,[2]:0x2FF + * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF */ static CANInstance sender_assignment[6] = { diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 9238a50..0eb8634 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -18,12 +18,12 @@ static USARTInstance *rc_usart_instance; static void sbus_to_rc(volatile const uint8_t *sbus_buf) { memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据 - // 摇杆 - rc_ctrl[TEMP].rc.joystick[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //!< Channel 0 - rc_ctrl[TEMP].rc.joystick[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1 - rc_ctrl[TEMP].rc.joystick[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff; //!< Channel 2 - rc_ctrl[TEMP].rc.joystick[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3 - rc_ctrl[TEMP].rc.joystick[4] = sbus_buf[16] | (sbus_buf[17] << 8); // 拨轮 + // 摇杆,直接解算时减去偏置 + rc_ctrl[TEMP].rc.joystick[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff - RC_CH_VALUE_OFFSET; //!< Channel 0 + rc_ctrl[TEMP].rc.joystick[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff - RC_CH_VALUE_OFFSET; //!< Channel 1 + rc_ctrl[TEMP].rc.joystick[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff - RC_CH_VALUE_OFFSET; //!< Channel 2 + rc_ctrl[TEMP].rc.joystick[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff - RC_CH_VALUE_OFFSET; //!< Channel 3 + rc_ctrl[TEMP].rc.joystick[4] = sbus_buf[16] | (sbus_buf[17] << 8) - RC_CH_VALUE_OFFSET; // 左侧拨轮 // 开关,0左1右 rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right @@ -61,13 +61,6 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf) // rc_ctrl[TEMP].key_test[KEY_PRESS_WITH_CTRL] = rc_ctrl[TEMP].key_test[KEY_PRESS]; // if (rc_ctrl[TEMP].key_test[KEY_PRESS].shift) // rc_ctrl[TEMP].key_test[Key_Shift] = rc_ctrl[TEMP].key_test[KEY_PRESS]; - - // 减去偏置值 - rc_ctrl[TEMP].rc.joystick[0] -= RC_CH_VALUE_OFFSET; - rc_ctrl[TEMP].rc.joystick[1] -= RC_CH_VALUE_OFFSET; - rc_ctrl[TEMP].rc.joystick[2] -= RC_CH_VALUE_OFFSET; - rc_ctrl[TEMP].rc.joystick[3] -= RC_CH_VALUE_OFFSET; - rc_ctrl[TEMP].rc.joystick[4] -= RC_CH_VALUE_OFFSET; } /**