// /*
// 底盘模组
// */

// /*
// 底盘模组
// */

// #include "cmsis_os2.h"
// #include <stdlib.h>
// #include "bsp/mm.h"
// #include "bsp/can.h"
// #include "component/ahrs.h"
// #include "device/motor_rm.h"
// #include "device/motor.h"
// #include "module/chassis.h"

// /**
//  * @brief 底盘小陀螺模式相关参数
//  */
// #define CHASSIS_ROTOR_WZ_MIN 0.6f       //小陀螺最小速度
// #define CHASSIS_ROTOR_WZ_MAX 0.8f				//小陀螺最大速度
// #define M_7OVER72PI (M_2PI * 7.0f / 72.0f)   //角度偏移量（用在跟随云台35°）
// #define CHASSIS_ROTOR_OMEGA 0.001f        //角速度变化频率



// /**
//  * @brief 设置底盘模式
//  * @param c 底盘结构体指针
//  * @param mode 目标控制模式
//  * @param now 当前时间戳(ms)
//  * @return CHASSIS_OK:成功   CHASSIS_ERR_NULL:空
//  */
// static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, uint32_t now) {
//     if (!c) 
// 			return CHASSIS_ERR_NULL;
//     if (mode == c->mode)
// 			return CHASSIS_OK;
// //随机种子，小陀螺模式随机设置旋转方向
//     if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
//         srand(now);
//         c->wz_multi = (rand() % 2) ? -1 : 1;
//     }
// //重置PID和滤波
//     for (uint8_t i = 0; i < c->num_wheel; i++) {
//         PID_Reset(&c->pid.motor[i]);
//         LowPassFilter2p_Reset(&c->filter.in[i], 0.0f);
//         LowPassFilter2p_Reset(&c->filter.out[i], 0.0f);
//     }

//     c->mode = mode;
//     return CHASSIS_OK;
// }

// /**
//  * @brief 小陀螺模式动态角速度
//  * @param min 最小速度
//  * @param max 最大速度
//  * @param now 当前时间戳(ms)
//  * @return 角速度值
//  */
// static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
//     float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
//     return (wz_vary > max) ? max : wz_vary;
// }

// /**
//  * @brief 底盘模式初始化
//  * @param c 底盘结构体指针
//  * @param param  底盘参数结构体指针
//  * @param mech_zero 机械零点欧拉角
//  * @param target_freq 控制频率(Hz)
//  * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空 CHASSIS_ERR_TYPE:不支持的模式
//  */
// int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
//                     float target_freq) {
//     if (!c) return CHASSIS_ERR_NULL;
											
// 		//初始化CAN通信									
// 		BSP_CAN_Init();
//     c->param = param;
//     c->mode = CHASSIS_MODE_RELAX;
// //根据底盘不同设置模式轮子与混合器
//     Mixer_Mode_t mixer_mode;
//     switch (param->type) {
//         case CHASSIS_TYPE_MECANUM://麦轮
// 					c->num_wheel = 4; 
// 				mixer_mode = MIXER_MECANUM;
// 				break;
//         case CHASSIS_TYPE_PARLFIX4:
// 					c->num_wheel = 4; 
// 				mixer_mode = MIXER_PARLFIX4;
// 				break;
//         case CHASSIS_TYPE_PARLFIX2:
// 					c->num_wheel = 2; 
// 				mixer_mode = MIXER_PARLFIX2; 
// 				break;
//         case CHASSIS_TYPE_OMNI_CROSS:
// 					c->num_wheel = 4;
// 				mixer_mode = MIXER_OMNICROSS;
// 				break;
//         case CHASSIS_TYPE_OMNI_PLUS: //全向轮（老步兵类型）
// 					c->num_wheel = 4;
// 				mixer_mode = MIXER_OMNIPLUS;
// 				break;
//         case CHASSIS_TYPE_SINGLE:
// 					c->num_wheel = 1; 
// 				mixer_mode = MIXER_SINGLE;
// 				break;
//         default: 
// 					return CHASSIS_ERR_TYPE;
//     }
// 		//初始化时间戳
// 				c->last_wakeup = 0;
// 				c->dt = 0.0f;
// 		//初始化PID和滤波
//     for (uint8_t i = 0; i < c->num_wheel; i++) {
//         PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, &param->pid.motor_pid_param);
//         LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in);
//         LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out);
// 			//清零电机反馈
// 				c->feedback.motor[i].rotor_speed = 0;
//         c->feedback.motor[i].torque_current = 0;
//         c->feedback.motor[i].rotor_abs_angle = 0;
//         c->feedback.motor[i].temp = 0;
//     }
//      //初始化PID和混合器
//     PID_Init(&c->pid.follow, KPID_MODE_NO_D, target_freq, &param->pid.follow_pid_param);
//     Mixer_Init(&c->mixer, mixer_mode);
//      //清零运动向量和输出
//     c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
// 		for (uint8_t i = 0; i < c->num_wheel; i++) { 
// 				c->out.motor[i] = 0.0f;
// 		}
// 		//注册大疆电机
// 		for (int i = 0; i < c->num_wheel; i++) {
// 			MOTOR_RM_Register(&(c->param->motor_param[i]));
			 
// 		}
//     return CHASSIS_OK;
// }

// /**
//  * @brief 更新电机反馈(IMU+电机状态)
//  * @param c 底盘结构体指针
//  * @param feedback 底盘反馈指针结构体
//  * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
//  */
// int8_t Chassis_UpdateFeedback(Chassis_t *c) {


// 		//更新所有电机反馈
//     for (uint8_t i = 0; i < c->num_wheel; i++) {
// 			MOTOR_RM_Update(&(c->param->motor_param[i])); 
// 			MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i]));
// 			c->motors[i] = rm_motor;
// 			MOTOR_RM_t *rm = c->motors[i];
//          if (rm_motor != NULL) {
//             c->feedback.motor[i] = rm_motor->feedback;
//              }else 
// 					{ 
// 					return CHASSIS_ERR_NULL; 
// 					} 
// 		}
//     return CHASSIS_OK;
// }

// /**
//  * @brief 底盘电机控制
//  * @param c 底盘结构体指针
//  * @param c_cmd 控制命令
//  * @param now 当前时间戳(ms)
//  * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
//  */
// int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
//     if (!c || !c_cmd) return CHASSIS_ERR_NULL;
// 		//计算控制周期
//     c->dt = (float)(now - c->last_wakeup) / 1000.0f; 
//     c->last_wakeup = now;
// 		if (!isfinite(c->dt) || c->dt <= 0.0f) {
// 				c->dt = 0.001f;            
// 		}
// 		if (c->dt < 0.0005f) c->dt = 0.0005f;   
// 		if (c->dt > 0.050f)  c->dt = 0.050f;    
//     //设置模式
//     Chassis_SetMode(c, c_cmd->mode, now);
//     //不同模式下对应解算（运动向量）
//     switch (c->mode) {
//         case CHASSIS_MODE_BREAK:
//             c->move_vec.vx = c->move_vec.vy = 0.0f;
//             break;
//         case CHASSIS_MODE_INDEPENDENT:
//             c->move_vec.vx = c_cmd->ctrl_vec.vx;
//             c->move_vec.vy = c_cmd->ctrl_vec.vy;
//             break;
//         default: {      //遥控器坐标->机体坐标系
//             float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
//             float cosb = cosf(beta);
//             float sinb = sinf(beta);
//             c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy;
//             c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy;
//             break;
//         }
//     }
// 		//根据模式计算底盘角速度
//     switch (c->mode) {
//         case CHASSIS_MODE_RELAX:
//         case CHASSIS_MODE_BREAK:
//         case CHASSIS_MODE_INDEPENDENT:
//             c->move_vec.wz = 0.0f;
//             break;
//         case CHASSIS_MODE_OPEN:
// 						c->move_vec.wz = c_cmd->ctrl_vec.wz;
// 						break;
// 				//云台跟随
//         case CHASSIS_MODE_FOLLOW_GIMBAL:
//             c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
//             break;
// 				//云台跟随（偏移）
//         case CHASSIS_MODE_FOLLOW_GIMBAL_35:
//             c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
//             break;
// 				//小陀螺
//         case CHASSIS_MODE_ROTOR:
//             c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
//             break;
//     }
//         //运动学逆解算，运动向量分解为电机转速
//     Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f);


//     for (uint8_t i = 0; i < c->num_wheel; i++) {
// 				float rf = c->setpoint.motor_rpm[i];///目标转速
//         float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed);
// 				float out_current;
//         switch (c->mode) {
//             case CHASSIS_MODE_BREAK:
//             case CHASSIS_MODE_FOLLOW_GIMBAL:
//             case CHASSIS_MODE_FOLLOW_GIMBAL_35:
//             case CHASSIS_MODE_ROTOR:
//             case CHASSIS_MODE_INDEPENDENT:
//                 out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt);
//                 break;
//             case CHASSIS_MODE_OPEN:
//                 out_current = c->setpoint.motor_rpm[i] / 7000.0f;
//                 break;
//             case CHASSIS_MODE_RELAX:
//                 out_current = 0.0f;
//                 break;
//         }

//        //低通滤波和限幅
//          c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current);
// 				 Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current);
//     }

			
//     return CHASSIS_OK;
// }

// /**
//  * @brief 电机输出
//  * @param c 底盘结构体指针
//  */
// void Chassis_Output(Chassis_t *c) {
//     if (!c) 
// 			return;

//     //每个电机目标输出
//     for (uint8_t i = 0; i < c->num_wheel; i++) {
//         MOTOR_RM_t *rm = c->motors[i];
//         if (!rm) continue;
//         MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
//     }

//     //调用ctrl
//     for (uint8_t i = 0; i < c->num_wheel; i++) {
//         MOTOR_RM_t *rm = c->motors[i];
//         if (rm) {
//             MOTOR_RM_Ctrl(&rm->param);
//         }
//     }
// }

// /**
//  * @brief 重置底盘输出
//  * @param c  底盘结构体指针
//  */
// void Chassis_ResetOutput(Chassis_t *c) {
//     if (!c) return;
//     for (uint8_t i = 0; i < c->num_wheel; i++) {
//         MOTOR_RM_t *m = c->motors[i];
//         if (m) {
//             MOTOR_RM_Relax(&(m->param));
//         }
//     }
// }