diff --git a/application/chassis/balance.c b/application/chassis/balance.c index a3f0074..3f2933e 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -28,10 +28,10 @@ #define STOOL_YAW_RATE_REF 3.5f #define STOOL_ACC_REF 4.0f #define STOOL_BRAKE_ACC_REF 8.0f -#define STOOL_PITCH_K 55.0f -#define STOOL_PITCH_W_K 7.0f -#define STOOL_VEL_K 10.0f -#define STOOL_STOP_DIST_K 8.0f +#define STOOL_PITCH_K 36.0f +#define STOOL_PITCH_W_K 10.0f +#define STOOL_VEL_K 6.0f +#define STOOL_STOP_DIST_K 3.0f #define STOOL_VEL_LPF_RC 0.03f #define STOOL_WHEEL_TORQUE_LIMIT 25.0f #define STOOL_RC_DEADBAND 40 @@ -353,9 +353,6 @@ static void ControlSwitch() if (DrivenMotorIsLost()) return; - if (!switch_is_up(rc_data[TEMP].rc.switch_right)) - return; - chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE; chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF; chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 16d1351..46c5731 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -330,8 +330,8 @@ static void EmergencyHandler() if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下]或[中] { - // 急停触发条件：遥控器离线 或者 右侧开关打到[下] 或者 robot_state 已经是 STOP - if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right) || robot_state == ROBOT_STOP) + // 急停触发条件：遥控器离线或者右侧开关打到[下] + if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right)) { robot_state = ROBOT_STOP; gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; @@ -343,7 +343,7 @@ static void EmergencyHandler() gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步 gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; } - // 恢复正常运行条件：遥控器在线 且 右侧开关状态为[上] + // 恢复正常运行条件：遥控器在线且右侧开关状态为[上]或[中] else if (switch_is_up(rc_data[TEMP].rc.switch_right) || switch_is_mid(rc_data[TEMP].rc.switch_right)) { robot_state = ROBOT_READY; diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 9e32a13..43591b6 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -11,6 +11,11 @@ static CANInstance *can_instance[CAN_MX_REGISTER_CNT] = {NULL}; static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自增 +static uint8_t CANBusIndex(CAN_HandleTypeDef *hcan) +{ + return hcan == &hcan1 ? 0u : 1u; +} + /* ----------------two static function called by CANRegister()-------------------- */ /** @@ -28,18 +33,26 @@ static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自 */ static void CANAddFilter(CANInstance *_instance) { - CAN_FilterTypeDef can_filter_conf; - static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用 + static uint8_t all_pass_filter_configured[DEVICE_CAN_CNT] = {0}; + uint8_t bus = CANBusIndex(_instance->can_handle); - can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST; // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤 - can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; // 使用16位id模式,即只有低16位有效 - can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; // 奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1 - can_filter_conf.SlaveStartFilterBank = 14; // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机) - can_filter_conf.FilterIdLow = _instance->rx_id << 5; // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0 - can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); // 根据can_handle判断是CAN1还是CAN2,然后自增 - can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; // 启用过滤器 + if (all_pass_filter_configured[bus]) + return; - HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf); + CAN_FilterTypeDef accept_all_filter = {0}; + accept_all_filter.FilterActivation = CAN_FILTER_ENABLE; + accept_all_filter.FilterMode = CAN_FILTERMODE_IDMASK; + accept_all_filter.FilterScale = CAN_FILTERSCALE_32BIT; + accept_all_filter.FilterIdHigh = 0x0000; + accept_all_filter.FilterIdLow = 0x0000; + accept_all_filter.FilterMaskIdHigh = 0x0000; + accept_all_filter.FilterMaskIdLow = 0x0000; + accept_all_filter.FilterBank = _instance->can_handle == &hcan1 ? 0 : 14; + accept_all_filter.FilterFIFOAssignment = CAN_RX_FIFO0; + accept_all_filter.SlaveStartFilterBank = 14; + HAL_CAN_ConfigFilter(_instance->can_handle, &accept_all_filter); + all_pass_filter_configured[bus] = 1; + return; } /** @@ -163,7 +176,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox) memcpy(can_instance[i]->rx_buff, can_rx_buff, rxconf.DLC); // 消息拷贝到对应实例 can_instance[i]->can_module_callback(can_instance[i]); // 触发回调进行数据解析和处理 } - return; + break; } } } diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c index 54f593e..7fd7d3b 100644 --- a/modules/algorithm/QuaternionEKF.c +++ b/modules/algorithm/QuaternionEKF.c @@ -199,7 +199,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, // 利用四元数反解欧拉角 QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f; - QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; + QEKF_INS.Pitch = -asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺) diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index 6f159db..f2f4d28 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -1,5 +1,6 @@ #include "LK9025.h" #include "stdlib.h" +#include "string.h" #include "general_def.h" #include "daemon.h" #include "bsp_dwt.h" @@ -7,33 +8,30 @@ static uint8_t idx; static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL}; -static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance) -// 后续考虑兼容单电机和多电机指令. -/** - * @brief 电机反馈报文解析 - * - * @param _instance 发生中断的caninstance - */ +static uint32_t LKMotorSingleCanID(uint32_t id) +{ + return id < 0x140 ? 0x140 + id : id; +} + static void LKMotorDecode(CANInstance *_instance) { - LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance + LKMotorInstance *motor = (LKMotorInstance *)_instance->id; LKMotor_Measure_t *measure = &motor->measure; uint8_t *rx_buff = _instance->rx_buff; - DaemonReload(motor->daemon); // 喂狗 + DaemonReload(motor->daemon); measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt); measure->last_ecd = measure->ecd; measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]); - measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd; - measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads + + measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads + DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4])); - measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current + - CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])); + measure->real_current = (int16_t)((1.0f - CURRENT_SMOOTH_COEF) * measure->real_current + + CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]))); measure->temperature = rx_buff[1]; @@ -41,7 +39,7 @@ static void LKMotorDecode(CANInstance *_instance) measure->total_round--; else if (measure->ecd - measure->last_ecd < -32768) measure->total_round++; - measure->total_angle = measure->total_round * 360 + measure->angle_single_round; + measure->total_angle = measure->total_round * 360.0f + measure->angle_single_round; } static void LKMotorLostCallback(void *motor_ptr) @@ -53,7 +51,6 @@ static void LKMotorLostCallback(void *motor_ptr) LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config) { LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance)); - motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance)); memset(motor, 0, sizeof(LKMotorInstance)); motor->motor_settings = config->controller_setting_init_config; @@ -65,16 +62,10 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config) config->can_init_config.id = motor; config->can_init_config.can_module_callback = LKMotorDecode; - config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id; - config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280 + config->can_init_config.tx_id = LKMotorSingleCanID(config->can_init_config.tx_id); + config->can_init_config.rx_id = config->can_init_config.tx_id; motor->motor_can_ins = CANRegister(&config->can_init_config); - if (idx == 0) // 用第一个电机的can instance发送数据 - { - sender_instance = motor->motor_can_ins; - sender_instance->tx_id = 0x280; // 修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用 - } - LKMotorEnable(motor); DWT_GetDeltaT(&motor->measure.feed_dwt_cnt); lkmotor_instance[idx++] = motor; @@ -82,14 +73,29 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config) Daemon_Init_Config_s daemon_config = { .callback = LKMotorLostCallback, .owner_id = motor, - .reload_count = 50, // 50ms + .reload_count = 50, }; motor->daemon = DaemonRegister(&daemon_config); return motor; } -/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */ +static void LKMotorSendTorque(LKMotorInstance *motor, int16_t set) +{ + uint16_t raw_set = (uint16_t)set; + uint8_t *tx_buff = motor->motor_can_ins->tx_buff; + + tx_buff[0] = 0xA1; + tx_buff[1] = 0x00; + tx_buff[2] = 0x00; + tx_buff[3] = 0x00; + tx_buff[4] = (uint8_t)(raw_set & 0xff); + tx_buff[5] = (uint8_t)(raw_set >> 8); + tx_buff[6] = 0x00; + tx_buff[7] = 0x00; + CANTransmit(motor->motor_can_ins, 0.2); +} + void LKMotorControl() { float pid_measure, pid_ref; @@ -105,48 +111,41 @@ void LKMotorControl() setting = &motor->motor_settings; pid_ref = motor->pid_ref; if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) - pid_ref *= -1; + pid_ref *= -1.0f; 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_measure = measure->total_angle; 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->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) { - if (setting->angle_feedback_source == OTHER_FEED) + if (setting->speed_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); + pid_ref = PIDCalculate(&motor->speed_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 = (int16_t)pid_ref; + if (pid_ref > I_MAX) + pid_ref = I_MAX; + else if (pid_ref < I_MIN) + pid_ref = I_MIN; - // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance - memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t)); - - if (motor->stop_flag == MOTOR_STOP) - { // 若该电机处于停止状态,直接将发送buff置零 - memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t)); - } + set = motor->stop_flag == MOTOR_STOP ? 0 : (int16_t)pid_ref; + LKMotorSendTorque(motor, set); } - - if (idx) // 如果有电机注册了 - CANTransmit(sender_instance, 0.2); } void LKMotorStop(LKMotorInstance *motor)