新增底盘电机功率限制

CMakeLists.txt

@@ -53,7 +53,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D
         bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb
         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/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/power_meter
         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)

application/chassis/chassis.c

@@ -17,6 +17,7 @@
 #include "super_cap.h"
 #include "message_center.h"
 #include "referee_task.h"
+#include "power_meter.h"
 
 #include "general_def.h"
 #include "bsp_dwt.h"
@@ -46,6 +47,7 @@ static referee_info_t* referee_data; // 用于获取裁判系统的数据
 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
 
 static SuperCapInstance *cap;                                       // 超级电容
+static PowerMeterInstance *power_meter;                             //功率计
 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
 
 /* 用于自旋变速策略的时间变量 */
@@ -55,6 +57,8 @@ static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left righ
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
 
+static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
+
 void ChassisInit()
 {
     // 四个轮子的参数一样,改tx_id和反转标志位即可
@@ -82,7 +86,9 @@ void ChassisInit()
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
             .outer_loop_type = SPEED_LOOP,
-            .close_loop_type = SPEED_LOOP,//| CURRENT_LOOP,
+            .close_loop_type = SPEED_LOOP| CURRENT_LOOP,
+
+            .power_limit_flag = POWER_LIMIT_ON, //开启功率限制
         },
         .motor_type = M3508,
     };
@@ -114,6 +120,15 @@ void ChassisInit()
         }};
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
 
+    PowerMeter_Init_Config_s power_conf = {
+            .can_config = {
+                    .can_handle = &hcan1,
+                    .rx_id = 0x212,
+            }
+    };
+
+    power_meter = PowerMeterInit(&power_conf);
+
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
@@ -165,21 +180,47 @@ static void OmniCalculate()
     vt_lf/=(WHEEL_BASE*1.414f);
 }
 
+
+//依据3508电机功率模型，预测电机输出功率
+static float EstimatePower(DJIMotorInstance* chassis_motor)
+{
+
+    float I_cmd = chassis_motor->motor_controller.current_PID.Output;
+    float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
+
+    float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w  + motor_power_K[2]*I_cmd*I_cmd;
+
+    return power;
+}
+float vofa_send_data[6];
 /**
  * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
  *
  */
 static void LimitChassisOutput()
 {
-    // 功率限制待添加
+    float P_cmd = motor_rf->motor_controller.motor_power_predict +
-    // referee_data->PowerHeatData.chassis_power;
+                  motor_rb->motor_controller.motor_power_predict +
-    // referee_data->PowerHeatData.chassis_power_buffer;
+                  motor_lb->motor_controller.motor_power_predict +
+                  motor_lf->motor_controller.motor_power_predict + 3.6f;
+    float P_max = 60 - 10;
+    float K = P_max/P_cmd;
+    vofa_send_data[2] = P_cmd;
+
+    motor_rf->motor_controller.motor_power_scale = K;
+    motor_rb->motor_controller.motor_power_scale = K;
+    motor_lf->motor_controller.motor_power_scale = K;
+    motor_lb->motor_controller.motor_power_scale = K;
+
+    {
+        DJIMotorSetRef(motor_lf, vt_lf);
+        DJIMotorSetRef(motor_rf, vt_rf);
+        DJIMotorSetRef(motor_lb, vt_lb);
+        DJIMotorSetRef(motor_rb, vt_rb);
+    }
+
+
 
-    // 完成功率限制后进行电机参考输入设定
-    DJIMotorSetRef(motor_lf, vt_lf);
-    DJIMotorSetRef(motor_rf, vt_rf);
-    DJIMotorSetRef(motor_lb, vt_lb);
-    DJIMotorSetRef(motor_rb, vt_rb);
 }
 
 /**
@@ -246,16 +287,28 @@ void ChassisTask()
     chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
     chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
 
+    motor_rf->motor_controller.motor_power_max = 10;
+
     // 根据控制模式进行正运动学解算,计算底盘输出
     //MecanumCalculate();
     OmniCalculate();
-
+    //vt_rf = 5000;
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
     LimitChassisOutput();
-//    float vofa_send_data[2];
+
-//    vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
+    //DJIMotorSetRef(motor_rf, 5000);
-//    vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
+
-//    vofa_justfloat_output(vofa_send_data,8,&huart1);
+
+    vofa_send_data[0] = motor_rf->motor_controller.speed_PID.Ref;
+    vofa_send_data[1] = motor_rf->motor_controller.speed_PID.Measure;
+
+    vofa_send_data[3] = PowerMeterGet(power_meter);
+    vofa_send_data[4] = 60;
+
+
+
+
+    vofa_justfloat_output(vofa_send_data,24,&huart1);
 
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();

application/cmd/robot_cmd.c

@@ -157,6 +157,9 @@ static void RemoteControlSet()
         shoot_cmd_send.load_mode = LOAD_STOP;
     // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
     shoot_cmd_send.shoot_rate = 8;
+    //检测到卡弹  拨弹盘反转
+    if(shoot_fetch_data.stalled_flag == 1)
+        shoot_cmd_send.load_mode = LOAD_REVERSE;
 }
 
 /**

application/gimbal/gimbal.c

@@ -9,10 +9,13 @@
 #include "bmi088.h"
 #include "vofa.h"
 
+#include "power_meter.h"
+
+
 static attitude_t *gimba_IMU_data; // 云台IMU数据
 static DJIMotorInstance *yaw_motor, *pitch_motor;
 
-static ECMotorInstance *big_yaw_motor;
+static DJIMotorInstance *big_yaw_motor;
 
 static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
 static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
@@ -110,14 +113,16 @@ void GimbalInit()
                     .close_loop_type = OPEN_LOOP,
                     .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
             },
-            .motor_type = ECA8210
+            .motor_type = M3508
     };
 
+
     // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
 //    yaw_motor = DJIMotorInit(&yaw_config);
 //    pitch_motor = DJIMotorInit(&pitch_config);
 
-    big_yaw_motor = ECMotorInit(&big_yaw_config);
+    //big_yaw_motor = ECMotorInit(&big_yaw_config);
+    //big_yaw_motor = DJIMotorInit(&big_yaw_config);
 
     gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
@@ -172,41 +177,43 @@ void GimbalTask()
     // ...
 
     //DJIMotorEnable(pitch_motor);
-    ECMotorEnable(big_yaw_motor);
+    //ECMotorEnable(big_yaw_motor);
-
-    float input = step_input_generate(&sinInputGenerate);
 
+    float input = sin_input_generate(&sinInputGenerate);
 
 
+    //DJIMotorEnable(big_yaw_motor);
     //DJIMotorSetRef(pitch_motor,input);
-    ECMotorSetRef(big_yaw_motor,30);
+   //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 gravity_feed = 3800*arm_cos_f32(theta/180*PI);
-    //DJIMotorSetRef(pitch_motor,gravity_feed);
+    //DJIMotorSetRef(big_yaw_motor,input+2000);
-        float vofa_send_data[4];
+//        float vofa_send_data[6];
-    vofa_send_data[0]=big_yaw_motor->measure.speed_rads;
+//    vofa_send_data[0]=big_yaw_motor->measure.speed_aps;
-    vofa_send_data[1]=big_yaw_motor->measure.angle_single_round;
+//    vofa_send_data[1]=big_yaw_motor->measure.angle_single_round;
-    vofa_send_data[2]=big_yaw_motor->measure.real_current;
+//    vofa_send_data[2]=big_yaw_motor->measure.real_current;
-    vofa_send_data[3]=big_yaw_motor->measure.temperature;
+//    vofa_send_data[3]=power_meter->power_msg.vol;
-//    vofa_send_data[3]=yaw_motor->motor_controller.angle_PID.Measure;
+//    vofa_send_data[4]=power_meter->power_msg.current;
-    vofa_justfloat_output(vofa_send_data,16,&huart1);
+//    vofa_send_data[5]=big_yaw_motor->motor_controller.pid_ref;
+//
+//    vofa_justfloat_output(vofa_send_data,24,&huart1);
 
 
-    vofa_send_data[0] = pitch_motor->motor_controller.pid_ref;
+//    vofa_send_data[0] = pitch_motor->motor_controller.pid_ref;
-    vofa_send_data[1] = pitch_motor->measure.real_current;
+//    vofa_send_data[1] = pitch_motor->measure.real_current;
-    vofa_send_data[2] = theta;
+//    vofa_send_data[2] = theta;
-    vofa_send_data[3] = gravity_feed;
+//    vofa_send_data[3] = gravity_feed;
-    vofa_justfloat_output(vofa_send_data,16,&huart1);
+//    vofa_justfloat_output(vofa_send_data,16,&huart1);
     // 设置反馈数据,主要是imu和yaw的ecd
-    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
+//    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
-    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
+//    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
-
+//
-    // 推送消息
+//    // 推送消息
-    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
+//    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
 }
 
 
@@ -260,6 +267,7 @@ float sin_input_generate(sin_input_generate_t* InputGenerate)
     else
         InputGenerate->input = arm_sin_f32(2*PI*InputGenerate->frequency[InputGenerate->frequency_index]*InputGenerate->time);
     //float input = arm_sin_f32(2*PI*frequency*time);
+    InputGenerate->input *= 2000;
 
     return InputGenerate->input;
 }

application/robot.c

@@ -31,13 +31,13 @@ void RobotInit()
 
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDInit();
-    //暂时注释云台和发射任务  调试底盘
+
     GimbalInit();
-//    ShootInit();
+    ShootInit();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-    //ChassisInit();
+    ChassisInit();
 #endif
 
     OSTaskInit(); // 创建基础任务
@@ -52,11 +52,11 @@ void RobotTask()
     RobotCMDTask();
     //暂时注释云台和发射任务  调试底盘
     GimbalTask();
-//    ShootTask();
+    ShootTask();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-    //ChassisTask();
+    ChassisTask();
 #endif
 
 }

application/robot_def.h

@@ -19,7 +19,7 @@
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
 #define ONE_BOARD // 单板控制整车
 // #define CHASSIS_BOARD //底盘板
-// #define GIMBAL_BOARD  //云台板
+//#define GIMBAL_BOARD  //云台板
 
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
 // #define VISION_USE_UART // 使用串口发送视觉数据
@@ -204,6 +204,7 @@ typedef struct
 {
     // code to go here
     // ...
+    uint8_t stalled_flag;  //堵转标志
 } Shoot_Upload_Data_s;
 
 #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)

application/shoot/shoot.c

@@ -16,7 +16,7 @@ static Subscriber_t *shoot_sub;
 static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
 
 // dwt定时,计算冷却用
-static float hibernate_time = 0, dead_time = 0;
+static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0;
 
 void ShootInit()
 {
@@ -48,7 +48,7 @@ void ShootInit()
             .speed_feedback_source = MOTOR_FEED,
 
             .outer_loop_type = SPEED_LOOP,
-            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
+            .close_loop_type = SPEED_LOOP,
             .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
         .motor_type = M3508};
@@ -74,12 +74,12 @@ void ShootInit()
                 .MaxOut = 200,
             },
             .speed_PID = {
-                .Kp = 0, // 10
+                    .Kp = 3, // 10
-                .Ki = 0, // 1
+                    .Ki = 0, // 1
-                .Kd = 0,
+                    .Kd = 0,
-                .Improve = PID_Integral_Limit,
+                    .Improve = PID_Integral_Limit,
-                .IntegralLimit = 5000,
+                    .IntegralLimit = 5000,
-                .MaxOut = 5000,
+                    .MaxOut = 10000,
             },
             .current_PID = {
                 .Kp = 0, // 0.7
@@ -103,6 +103,31 @@ void ShootInit()
     shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
     shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
 }
+//卡弹检测
+void stalled_detect()
+{
+    static float last_detect_time = 0,detect_time = 0;
+    static float last_total_angle = 0;
+    static uint8_t stalled_cnt = 0;
+
+    last_detect_time = detect_time;
+    detect_time = DWT_GetTimeline_ms();
+
+    if(detect_time - last_detect_time < 200)  // 200ms 检测一次
+        return;
+
+    if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE)
+    {
+        float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f;
+        float real_angle = loader->measure.total_angle - last_total_angle;
+        if(real_angle < reference_angle * 0.2f)
+        {
+            //stalled_cnt ++;
+            shoot_feedback_data.stalled_flag = 1;
+        }
+    }
+}
+
 
 /* 机器人发射机构控制核心任务 */
 void ShootTask()
@@ -126,8 +151,8 @@ void ShootTask()
 
     // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
     // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
-    // if (hibernate_time + dead_time > DWT_GetTimeline_ms())
+     if (hibernate_time + dead_time > DWT_GetTimeline_ms())
-    //     return;
+         return;
 
     // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
     switch (shoot_cmd_recv.load_mode)
@@ -155,12 +180,17 @@ void ShootTask()
     case LOAD_BURSTFIRE:
         DJIMotorOuterLoop(loader, SPEED_LOOP);
         DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
-        // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
+
+            // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
         break;
     // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
     // 也有可能需要从switch-case中独立出来
     case LOAD_REVERSE:
         DJIMotorOuterLoop(loader, SPEED_LOOP);
+        DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8);             // 固定速度反转
+        hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
+        dead_time = 500;                                                                    // 翻转500ms
+        shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
         // ...
         break;
     default:
@@ -207,6 +237,8 @@ void ShootTask()
     {
         //...
     }
+    //卡弹检测
+    stalled_detect();
 
     // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
     PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);

modules/motor/DJImotor/dji_motor.c

@@ -198,6 +198,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
     instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
     // 后续增加电机前馈控制器(速度和电流)
 
+
     // 电机分组,因为至多4个电机可以共用一帧CAN控制报文
     MotorSenderGrouping(instance, &config->can_init_config);
 
@@ -251,6 +252,16 @@ void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
 {
     motor->motor_controller.pid_ref = ref;
 }
+static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
+//依据3508电机功率模型，预测电机输出功率
+static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
+{
+    float I_cmd = chassis_motor->motor_controller.current_PID.Output;
+    float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
+
+    float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w  + motor_power_K[2]*I_cmd*I_cmd;
+    return power;
+}
 
 // 为所有电机实例计算三环PID,发送控制报文
 void DJIMotorControl()
@@ -274,7 +285,6 @@ void DJIMotorControl()
         pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
         if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
             pid_ref *= -1; // 设置反转
-
         // pid_ref会顺次通过被启用的闭环充当数据的载体
         // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
         if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
@@ -308,6 +318,7 @@ void DJIMotorControl()
         {
             //现在3508和6020电调都内置电流闭环  无需PID计算
             //pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
+            motor_controller->current_PID.Output = pid_ref;
             pid_ref = pid_ref;
         }
 
@@ -316,6 +327,35 @@ void DJIMotorControl()
         if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
             pid_ref *= -1;
 
+        //功率限制
+        if(motor_setting->power_limit_flag == POWER_LIMIT_ON)
+        {
+            float I_cmd = pid_ref;
+            float w = measure->speed_aps /6 ;//aps to rpm
+            motor_controller->motor_power_predict = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w  + motor_power_K[2]*I_cmd*I_cmd;
+            //这里K应该使用所有底盘电机一起计算 (在底盘任务中)
+            //float K = motor_controller->motor_power_max / motor_controller->motor_power_predict;
+            float K = motor_controller->motor_power_scale;
+            if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率 或做负功的电机 不做限制
+            {
+
+            }
+            else if(K<1)
+            {
+                float P_cmd = K * motor_controller->motor_power_predict; //对输出功率进行缩放
+
+                float a = motor_power_K[2] ;
+                float b = motor_power_K[0] * w;
+                float c = motor_power_K[1] * w * w - P_cmd;
+
+                if(pid_ref < 0)
+                    pid_ref = (-b - sqrtf(b*b-4*a*c))/(2*a);
+                else
+                    pid_ref = (-b + sqrtf(b*b-4*a*c))/(2*a);
+            }
+            //if( motor_controller->motor_power_predict < )
+        }
+
         // 获取最终输出
         set = (int16_t)pid_ref;
 

modules/motor/motor_def.h

@@ -68,6 +68,11 @@ typedef enum
     MOTOR_ENALBED = 1,
 } Motor_Working_Type_e;
 
+typedef enum
+{
+    NO_POWER_LIMIT = 0,
+    POWER_LIMIT_ON = 1,
+} Power_Limit_Type_e;
 /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
 typedef struct
 {
@@ -79,6 +84,8 @@ typedef struct
     Feedback_Source_e speed_feedback_source;       // 速度反馈类型
     Feedfoward_Type_e feedforward_flag;            // 前馈标志
 
+    Power_Limit_Type_e power_limit_flag;            //功率限制标志
+
 } Motor_Control_Setting_s;
 
 /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
@@ -95,6 +102,10 @@ typedef struct
     PIDInstance angle_PID;
 
     float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
+
+    float motor_power_max;     //每个电机分配的功率上限
+    float motor_power_predict; //根据模型预测的电机功率
+    float motor_power_scale;   //电机功率缩放比例
 } Motor_Controller_s;
 
 /* 电机类型枚举 */
@@ -143,6 +154,7 @@ typedef struct
     Motor_Type_e motor_type;
     CAN_Init_Config_s can_init_config;
     Motor_Control_Type_e motor_control_type;
+
 } Motor_Init_Config_s;
 
 #endif // !MOTOR_DEF_H

modules/power_meter/power_meter.c

@@ -0,0 +1,38 @@
+//
+// Created by SJQ on 2023/12/19.
+//
+
+#include "power_meter.h"
+#include "memory.h"
+#include "stdlib.h"
+
+static PowerMeterInstance *power_meter_instance = NULL; // 可以由app保存此指针
+
+static void PowerMeterRxCallback(CANInstance *_instance)
+{
+    uint8_t *rxbuff;
+    PowerMeter_Msg_s *Msg;
+    rxbuff = _instance->rx_buff;
+    Msg = &power_meter_instance->power_msg;
+    Msg->vol = (uint16_t)(rxbuff[1] << 8 | rxbuff[0]);
+    Msg->current = (uint16_t)(rxbuff[3] << 8 | rxbuff[2]);
+
+}
+
+PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config)
+{
+    power_meter_instance = (PowerMeterInstance *)malloc(sizeof(PowerMeterInstance));
+    memset(power_meter_instance, 0, sizeof(PowerMeterInstance));
+
+    PowerMeter_config->can_config.can_module_callback = PowerMeterRxCallback;
+    power_meter_instance->can_ins = CANRegister(&PowerMeter_config->can_config);
+    return power_meter_instance;
+}
+
+float PowerMeterGet(PowerMeterInstance *instance)
+{
+    float power = instance->power_msg.current*instance->power_msg.vol/1e4;
+    return power;
+}
+
+#include "power_meter.h"

modules/power_meter/power_meter.h

@@ -0,0 +1,39 @@
+//
+// Created by SJQ on 2023/12/19.
+//
+
+#ifndef BASIC_FRAMEWORK_POWER_METER_H
+#define BASIC_FRAMEWORK_POWER_METER_H
+#include "bsp_can.h"
+
+#pragma pack(1)
+typedef struct
+{
+    uint16_t vol; // 电压
+    uint16_t current; // 电流
+} PowerMeter_Msg_s;
+#pragma pack()
+
+/* 超级电容实例 */
+typedef struct
+{
+    CANInstance *can_ins; // CAN实例
+    PowerMeter_Msg_s power_msg; // 功率计反馈信息
+} PowerMeterInstance;
+
+/* 超级电容初始化配置 */
+typedef struct
+{
+    CAN_Init_Config_s can_config;
+} PowerMeter_Init_Config_s;
+
+/**
+ * @brief 初始化功率计
+ *
+ * @param PowerMeter_config 功率计初始化配置
+ * @return PowerMeterInstance* 功率计实例指针
+ */
+PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config);
+float PowerMeterGet(PowerMeterInstance *instance);
+
+#endif //BASIC_FRAMEWORK_POWER_METER_H