diff --git a/CMakeLists.txt b/CMakeLists.txt index 163f8fc..0136592 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ function(include_sub_directories_recursively root_dir) include_directories(${root_dir}) endif() - file(GLOB ALL_SUB RELATIVE ${root_dir} ${root_dir}/*) # 获得当前目录下的所有文件，让如ALL_SUB列表中 + file(GLOB ALL_SUB CONFIGURE_DEPENDS RELATIVE ${root_dir} ${root_dir}/*) # 获得当前目录下的所有文件，让如ALL_SUB列表中 foreach(sub ${ALL_SUB}) if (IS_DIRECTORY ${root_dir}/${sub}) include_sub_directories_recursively(${root_dir}/${sub}) # 对子目录递归调用，包含 @@ -68,7 +68,7 @@ include_sub_directories_recursively(${CMAKE_SOURCE_DIR}/application) include_sub_directories_recursively(${CMAKE_SOURCE_DIR}/Inc) # add source, only surfix .c -file(GLOB_RECURSE SOURCES +file(GLOB_RECURSE SOURCES CONFIGURE_DEPENDS "Drivers/*.c" "Src/*.c" "Middlewares/*.c" @@ -112,4 +112,4 @@ add_custom_command( COMMENT "Building hex & bin file..." COMMENT "EXCUTABLE SIZE:" COMMAND ${SIZE} ${PROJECT_NAME}.elf - ) \ No newline at end of file + ) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index f1d374f..4ceb956 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -17,6 +17,9 @@ #include "dji_motor.h" #include "LKMotor.h" #include "super_cap.h" +#include "chassis_power.h" +#include "power_meter.h" +#include "vofa.h" #include "message_center.h" #include "referee_task.h" #include "user_lib.h" @@ -25,6 +28,31 @@ #include "bsp_dwt.h" #include "referee_UI.h" #include "arm_math.h" +#include + +#ifndef CHASSIS_POWER_LIMIT_ENABLE +#define CHASSIS_POWER_LIMIT_ENABLE 0 +#endif + +#ifndef CHASSIS_POWER_USE_POWER_METER +#define CHASSIS_POWER_USE_POWER_METER 1 +#endif + +#ifndef CHASSIS_POWER_RLS_ENABLE +#define CHASSIS_POWER_RLS_ENABLE 0 +#endif + +#ifndef CHASSIS_POWER_VOFA_ENABLE +#define CHASSIS_POWER_VOFA_ENABLE 0 +#endif + +#ifndef CHASSIS_POWER_VOFA_DIV +#define CHASSIS_POWER_VOFA_DIV 10u +#endif + +ChassisPowerInstance chassis_power_control; +PowerMeterInstance *chassis_power_meter; +ChassisPower_Limit_Input_s chassis_power_debug_input; /* 根据robot_def.h中的macro自动计算的参数 */ #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距 @@ -181,6 +209,26 @@ void ChassisInit() // （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整 SuperCapSetPowerOffset(cap, 5); +#if CHASSIS_POWER_USE_POWER_METER + chassis_power_meter = PowerMeterInitCan(&hcan1, POWER_METER_DEFAULT_CAN_ID); +#endif + + ChassisPowerMotorModel_s wheel_model = ChassisPowerGetDefault3508Model(); + ChassisPower_Init_Config_s power_config = { + .wheel_model = wheel_model, + .default_power_limit = 60.0f, + .min_power_limit = 20.0f, + .buffer_low = 30.0f, + .buffer_high = 50.0f, + .buffer_gain = 0.25f, + .steering_power_reserve = 6.0f, + .min_scale = 0.20f, + .rls_lambda = 0.995f, + .rls_init_covariance = 1000.0f, + .rls_enable = CHASSIS_POWER_RLS_ENABLE, + }; + ChassisPowerInit(&chassis_power_control, &power_config); + // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 @@ -340,6 +388,123 @@ static void SteeringWheelCalculate() */ static void LimitChassisOutput() { +#if CHASSIS_POWER_LIMIT_ENABLE + float limited_rf = vt_rf; + float limited_lb = vt_lb; + const PowerMeterInstance *meter_data = 0; +#if CHASSIS_POWER_USE_POWER_METER + meter_data = PowerMeterGetData(chassis_power_meter); +#endif + + ChassisPower_Limit_Input_s power_input = { + .rf_speed_ref = vt_rf, + .lb_speed_ref = vt_lb, + .rf_speed = motor_rf->measure.speed_rads, + .lb_speed = motor_lb->measure.speed_rads, + .rf_current_cmd = motor_rf->speed_PID.Output, + .lb_current_cmd = motor_lb->speed_PID.Output, + .rf_output_cmd = motor_rf->current_PID.Output, + .lb_output_cmd = motor_lb->current_PID.Output, + .rf_feedback_current = motor_rf->measure.real_current, + .lb_feedback_current = motor_lb->measure.real_current, + .referee_limit = referee_data != 0 ? referee_data->GameRobotState.chassis_power_limit : 0.0f, + .referee_power = referee_data != 0 ? referee_data->PowerHeatData.chassis_power : 0.0f, + .power_buffer = referee_data != 0 ? referee_data->PowerHeatData.chassis_power_buffer : 0.0f, + .measured_power = meter_data != 0 ? meter_data->power : 0.0f, + .steering_power_estimate = 0.0f, + .power_meter_online = meter_data != 0 ? meter_data->online : 0u, + }; + chassis_power_debug_input = power_input; + + ChassisPowerUpdateMeasureState(&chassis_power_control, &power_input); + ChassisPowerLimitWheelRefs(&chassis_power_control, &power_input, &limited_rf, &limited_lb); + if (power_input.power_meter_online) { + ChassisPowerUpdateRLS(&chassis_power_control, &power_input, power_input.measured_power); + } + + vt_rf = limited_rf; + vt_lb = limited_lb; + +#if CHASSIS_POWER_VOFA_ENABLE + static uint16_t vofa_divider = 0u; + if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) { + vofa_divider = 0u; + const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control); + float vofa_data[] = { + power_state->estimated_total_power, + power_state->allowed_power, + power_state->scale, + power_input.measured_power, + meter_data != 0 ? meter_data->voltage : 0.0f, + meter_data != 0 ? meter_data->current : 0.0f, + power_state->rf_ref_raw, + power_state->rf_ref_limited, + power_state->lb_ref_raw, + power_state->lb_ref_limited, + motor_rf->measure.speed_rads, + motor_lb->measure.speed_rads, + motor_rf->speed_PID.Output, + motor_lb->speed_PID.Output, + motor_rf->measure.real_current, + motor_lb->measure.real_current, + }; + vofa_justfloat_output(vofa_data, sizeof(vofa_data) / sizeof(vofa_data[0]), &huart1); + } +#endif +#else + const PowerMeterInstance *meter_data = 0; +#if CHASSIS_POWER_USE_POWER_METER + meter_data = PowerMeterGetData(chassis_power_meter); +#endif + + ChassisPower_Limit_Input_s power_input = { + .rf_speed_ref = vt_rf, + .lb_speed_ref = vt_lb, + .rf_speed = motor_rf->measure.speed_rads, + .lb_speed = motor_lb->measure.speed_rads, + .rf_current_cmd = motor_rf->speed_PID.Output, + .lb_current_cmd = motor_lb->speed_PID.Output, + .rf_output_cmd = motor_rf->current_PID.Output, + .lb_output_cmd = motor_lb->current_PID.Output, + .rf_feedback_current = motor_rf->measure.real_current, + .lb_feedback_current = motor_lb->measure.real_current, + .referee_limit = referee_data != 0 ? referee_data->GameRobotState.chassis_power_limit : 0.0f, + .referee_power = referee_data != 0 ? referee_data->PowerHeatData.chassis_power : 0.0f, + .power_buffer = referee_data != 0 ? referee_data->PowerHeatData.chassis_power_buffer : 0.0f, + .measured_power = meter_data != 0 ? meter_data->power : 0.0f, + .steering_power_estimate = 0.0f, + .power_meter_online = meter_data != 0 ? meter_data->online : 0u, + }; + chassis_power_debug_input = power_input; + ChassisPowerUpdateMeasureState(&chassis_power_control, &power_input); + +#if CHASSIS_POWER_VOFA_ENABLE + static uint16_t vofa_divider = 0u; + if (++vofa_divider >= CHASSIS_POWER_VOFA_DIV) { + vofa_divider = 0u; + const ChassisPower_State_s *power_state = ChassisPowerGetState(&chassis_power_control); + float vofa_data[] = { + power_state->estimated_total_power, + power_state->allowed_power, + power_state->scale, + power_input.measured_power, + meter_data != 0 ? meter_data->voltage : 0.0f, + meter_data != 0 ? meter_data->current : 0.0f, + power_state->rf_ref_raw, + power_state->rf_ref_limited, + power_state->lb_ref_raw, + power_state->lb_ref_limited, + motor_rf->measure.speed_rads, + motor_lb->measure.speed_rads, + motor_rf->speed_PID.Output, + motor_lb->speed_PID.Output, + motor_rf->measure.real_current, + motor_lb->measure.real_current, + }; + vofa_justfloat_output(vofa_data, sizeof(vofa_data) / sizeof(vofa_data[0]), &huart1); + } +#endif +#endif // 功率限制待添加 // referee_data->PowerHeatData.chassis_power; // referee_data->PowerHeatData.chassis_power_buffer; @@ -416,7 +581,7 @@ void ChassisTask() { default: break; } - static float sin_theta, cos_theta; + float sin_theta = 0.0f, cos_theta = 1.0f; // ------------------ 【修改部分：加入相位滞后动态补偿】 ------------------ // 提取当前的云台-底盘夹角 float real_offset_angle = chassis_cmd_recv.offset_angle; @@ -477,4 +642,4 @@ void ChassisTask() { #ifdef CHASSIS_BOARD CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data); #endif // CHASSIS_BOARD -} \ No newline at end of file +} diff --git a/modules/algorithm/rls.c b/modules/algorithm/rls.c new file mode 100644 index 0000000..35130b2 --- /dev/null +++ b/modules/algorithm/rls.c @@ -0,0 +1,99 @@ +#include "rls.h" + +#include + +void RLSInit(RLSInstance *rls, uint8_t order, float lambda, float init_covariance, const float *initial_theta) +{ + if (rls == 0) { + return; + } + + memset(rls, 0, sizeof(RLSInstance)); + + if (order == 0u) { + return; + } + if (order > RLS_MAX_ORDER) { + order = RLS_MAX_ORDER; + } + if (lambda < 0.01f) { + lambda = 0.01f; + } + if (lambda > 1.0f) { + lambda = 1.0f; + } + if (init_covariance <= 0.0f) { + init_covariance = 1000.0f; + } + + rls->order = order; + rls->lambda = lambda; + rls->initialized = 1u; + + for (uint8_t i = 0u; i < order; i++) { + rls->theta[i] = initial_theta != 0 ? initial_theta[i] : 0.0f; + rls->covariance[i][i] = init_covariance; + } +} + +float RLSPredict(const RLSInstance *rls, const float *x) +{ + if (rls == 0 || x == 0 || rls->initialized == 0u) { + return 0.0f; + } + + float prediction = 0.0f; + for (uint8_t i = 0u; i < rls->order; i++) { + prediction += rls->theta[i] * x[i]; + } + return prediction; +} + +float RLSUpdate(RLSInstance *rls, const float *x, float y) +{ + float px[RLS_MAX_ORDER] = {0.0f}; + float xtp[RLS_MAX_ORDER] = {0.0f}; + + if (rls == 0 || x == 0 || rls->initialized == 0u) { + return 0.0f; + } + + const uint8_t order = rls->order; + const float prediction = RLSPredict(rls, x); + const float error = y - prediction; + + for (uint8_t i = 0u; i < order; i++) { + for (uint8_t j = 0u; j < order; j++) { + px[i] += rls->covariance[i][j] * x[j]; + xtp[i] += x[j] * rls->covariance[j][i]; + } + } + + float denominator = rls->lambda; + for (uint8_t i = 0u; i < order; i++) { + denominator += x[i] * px[i]; + } + if (denominator <= 1.0e-6f) { + denominator = 1.0e-6f; + } + + for (uint8_t i = 0u; i < order; i++) { + rls->gain[i] = px[i] / denominator; + rls->theta[i] += rls->gain[i] * error; + } + + for (uint8_t i = 0u; i < order; i++) { + for (uint8_t j = 0u; j < order; j++) { + rls->covariance[i][j] = (rls->covariance[i][j] - rls->gain[i] * xtp[j]) / rls->lambda; + } + } + + rls->last_prediction = prediction; + rls->last_error = error; + return prediction; +} + +const float *RLSGetTheta(const RLSInstance *rls) +{ + return rls != 0 ? rls->theta : 0; +} diff --git a/modules/algorithm/rls.h b/modules/algorithm/rls.h new file mode 100644 index 0000000..2a5d8f3 --- /dev/null +++ b/modules/algorithm/rls.h @@ -0,0 +1,25 @@ +#ifndef RLS_H +#define RLS_H + +#include + +#define RLS_MAX_ORDER 4u + +typedef struct +{ + uint8_t order; + uint8_t initialized; + float lambda; + float theta[RLS_MAX_ORDER]; + float covariance[RLS_MAX_ORDER][RLS_MAX_ORDER]; + float gain[RLS_MAX_ORDER]; + float last_prediction; + float last_error; +} RLSInstance; + +void RLSInit(RLSInstance *rls, uint8_t order, float lambda, float init_covariance, const float *initial_theta); +float RLSPredict(const RLSInstance *rls, const float *x); +float RLSUpdate(RLSInstance *rls, const float *x, float y); +const float *RLSGetTheta(const RLSInstance *rls); + +#endif diff --git a/modules/chassis_power/chassis_power.c b/modules/chassis_power/chassis_power.c new file mode 100644 index 0000000..78f88c2 --- /dev/null +++ b/modules/chassis_power/chassis_power.c @@ -0,0 +1,279 @@ +#include "chassis_power.h" + +#include +#include + +ChassisPower_State_s chassis_power_debug_state; + +static float ChassisPowerLimitValue(float value, float min, float max) +{ + if (value < min) { + return min; + } + if (value > max) { + return max; + } + return value; +} + +ChassisPowerMotorModel_s ChassisPowerGetDefault3508Model(void) +{ + const float it_to_ireal_m3508 = 20.0f / 16384.0f; + const float i_to_t_m3508 = 0.3f / (3591.0f / 187.0f); + const float it_to_t_m3508 = it_to_ireal_m3508 * i_to_t_m3508; + + ChassisPowerMotorModel_s model = { + .k_current_speed = it_to_t_m3508 / 9.55f, + .k_speed2 = 1.453e-7f, + .k_current2 = 2.53e-7f, + .k0 = 4.081f / 4.0f, + }; + return model; +} + +void ChassisPowerInit(ChassisPowerInstance *instance, const ChassisPower_Init_Config_s *config) +{ + if (instance == 0) { + return; + } + + memset(instance, 0, sizeof(ChassisPowerInstance)); + + instance->wheel_model = ChassisPowerGetDefault3508Model(); + instance->default_power_limit = 60.0f; + instance->min_power_limit = 20.0f; + instance->buffer_low = 30.0f; + instance->buffer_high = 50.0f; + instance->buffer_gain = 0.25f; + instance->steering_power_reserve = 6.0f; + instance->min_scale = 0.20f; + instance->rls_enable = 0u; + + float lambda = 0.995f; + float init_covariance = 1000.0f; + + if (config != 0) { + instance->wheel_model = config->wheel_model; + if (config->default_power_limit > 0.0f) { + instance->default_power_limit = config->default_power_limit; + } + if (config->min_power_limit > 0.0f) { + instance->min_power_limit = config->min_power_limit; + } + if (config->buffer_low > 0.0f) { + instance->buffer_low = config->buffer_low; + } + if (config->buffer_high > instance->buffer_low) { + instance->buffer_high = config->buffer_high; + } + if (config->buffer_gain > 0.0f) { + instance->buffer_gain = config->buffer_gain; + } + if (config->steering_power_reserve >= 0.0f) { + instance->steering_power_reserve = config->steering_power_reserve; + } + if (config->min_scale > 0.0f && config->min_scale <= 1.0f) { + instance->min_scale = config->min_scale; + } + if (config->rls_lambda > 0.0f) { + lambda = config->rls_lambda; + } + if (config->rls_init_covariance > 0.0f) { + init_covariance = config->rls_init_covariance; + } + instance->rls_enable = config->rls_enable; + } + + float theta[CHASSIS_POWER_RLS_ORDER] = { + instance->wheel_model.k_current_speed, + instance->wheel_model.k_speed2, + instance->wheel_model.k_current2, + instance->wheel_model.k0 * 2.0f, + }; + RLSInit(&instance->rls, CHASSIS_POWER_RLS_ORDER, lambda, init_covariance, theta); + memcpy(instance->state.rls_theta, theta, sizeof(theta)); +} + +float ChassisPowerEstimateMotor(const ChassisPowerMotorModel_s *model, float speed, float current_cmd) +{ + if (model == 0) { + return 0.0f; + } + + float power = model->k_current_speed * speed * current_cmd + + model->k_speed2 * speed * speed + + model->k_current2 * current_cmd * current_cmd + + model->k0; + + if (power < 0.0f) { + power = 0.0f; + } + return power; +} + +static float ChassisPowerGetAllowedPower(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input) +{ + float allowed = input->referee_limit > 1.0f ? input->referee_limit : instance->default_power_limit; + + if (input->referee_limit > 1.0f) { + if (input->power_buffer < instance->buffer_low) { + allowed -= (instance->buffer_low - input->power_buffer) * instance->buffer_gain; + } else if (input->power_buffer > instance->buffer_high) { + allowed += (input->power_buffer - instance->buffer_high) * instance->buffer_gain; + } + } + + return ChassisPowerLimitValue(allowed, instance->min_power_limit, instance->default_power_limit + 20.0f); +} + +void ChassisPowerUpdateMeasureState(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input) +{ + if (instance == 0 || input == 0) { + return; + } + + ChassisPower_State_s *state = &instance->state; + const float rf_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->rf_speed, input->rf_current_cmd); + const float lb_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->lb_speed, input->lb_current_cmd); + + state->rf_ref_raw = input->rf_speed_ref; + state->lb_ref_raw = input->lb_speed_ref; + state->rf_ref_limited = input->rf_speed_ref; + state->lb_ref_limited = input->lb_speed_ref; + state->rf_power = rf_power; + state->lb_power = lb_power; + state->estimated_wheel_power = rf_power + lb_power; + state->estimated_total_power = state->estimated_wheel_power; + state->allowed_power = ChassisPowerGetAllowedPower(instance, input); + state->wheel_budget = state->allowed_power; + state->scale = 1.0f; + state->measured_power = input->measured_power; + state->referee_power = input->referee_power; + state->power_buffer = input->power_buffer; + state->rf_speed = input->rf_speed; + state->lb_speed = input->lb_speed; + state->rf_current_cmd = input->rf_current_cmd; + state->lb_current_cmd = input->lb_current_cmd; + state->rf_output_cmd = input->rf_output_cmd; + state->lb_output_cmd = input->lb_output_cmd; + state->rf_feedback_current = input->rf_feedback_current; + state->lb_feedback_current = input->lb_feedback_current; + state->power_meter_online = input->power_meter_online; + state->limited = 0u; + + state->features[0] = input->rf_speed * input->rf_current_cmd + input->lb_speed * input->lb_current_cmd; + state->features[1] = input->rf_speed * input->rf_speed + input->lb_speed * input->lb_speed; + state->features[2] = input->rf_current_cmd * input->rf_current_cmd + input->lb_current_cmd * input->lb_current_cmd; + state->features[3] = 1.0f; + chassis_power_debug_state = *state; +} + +float ChassisPowerLimitWheelRefs(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float *rf_ref, float *lb_ref) +{ + if (instance == 0 || input == 0 || rf_ref == 0 || lb_ref == 0) { + return 1.0f; + } + + ChassisPower_State_s *state = &instance->state; + const float rf_raw = *rf_ref; + const float lb_raw = *lb_ref; + const float allowed = ChassisPowerGetAllowedPower(instance, input); + float steering_power = input->steering_power_estimate; + + if (steering_power <= 0.0f) { + steering_power = instance->steering_power_reserve; + } + + const float rf_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->rf_speed, input->rf_current_cmd); + const float lb_power = ChassisPowerEstimateMotor(&instance->wheel_model, input->lb_speed, input->lb_current_cmd); + const float wheel_power = rf_power + lb_power; + const float total_power = wheel_power + steering_power; + float wheel_budget = allowed - steering_power; + float scale = 1.0f; + + if (wheel_budget < 1.0f) { + wheel_budget = 1.0f; + } + + if (wheel_power > wheel_budget && wheel_power > 1.0e-3f) { + scale = sqrtf(wheel_budget / wheel_power); + scale = ChassisPowerLimitValue(scale, instance->min_scale, 1.0f); + *rf_ref = rf_raw * scale; + *lb_ref = lb_raw * scale; + state->limited = 1u; + } else { + state->limited = 0u; + } + + state->rf_ref_raw = rf_raw; + state->lb_ref_raw = lb_raw; + state->rf_ref_limited = *rf_ref; + state->lb_ref_limited = *lb_ref; + state->rf_power = rf_power; + state->lb_power = lb_power; + state->estimated_wheel_power = wheel_power; + state->estimated_total_power = total_power; + state->allowed_power = allowed; + state->wheel_budget = wheel_budget; + state->scale = scale; + state->measured_power = input->measured_power; + state->referee_power = input->referee_power; + state->power_buffer = input->power_buffer; + state->rf_speed = input->rf_speed; + state->lb_speed = input->lb_speed; + state->rf_current_cmd = input->rf_current_cmd; + state->lb_current_cmd = input->lb_current_cmd; + state->rf_output_cmd = input->rf_output_cmd; + state->lb_output_cmd = input->lb_output_cmd; + state->rf_feedback_current = input->rf_feedback_current; + state->lb_feedback_current = input->lb_feedback_current; + state->power_meter_online = input->power_meter_online; + + state->features[0] = input->rf_speed * input->rf_current_cmd + input->lb_speed * input->lb_current_cmd; + state->features[1] = input->rf_speed * input->rf_speed + input->lb_speed * input->lb_speed; + state->features[2] = input->rf_current_cmd * input->rf_current_cmd + input->lb_current_cmd * input->lb_current_cmd; + state->features[3] = 1.0f; + chassis_power_debug_state = *state; + + return scale; +} + +void ChassisPowerUpdateRLS(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float measured_wheel_power) +{ + if (instance == 0 || input == 0 || instance->rls_enable == 0u || measured_wheel_power <= 0.0f) { + return; + } + + float features[CHASSIS_POWER_RLS_ORDER] = { + input->rf_speed * input->rf_current_cmd + input->lb_speed * input->lb_current_cmd, + input->rf_speed * input->rf_speed + input->lb_speed * input->lb_speed, + input->rf_current_cmd * input->rf_current_cmd + input->lb_current_cmd * input->lb_current_cmd, + 1.0f, + }; + + RLSUpdate(&instance->rls, features, measured_wheel_power); + const float *theta = RLSGetTheta(&instance->rls); + if (theta == 0) { + return; + } + + instance->wheel_model.k_current_speed = theta[0]; + instance->wheel_model.k_speed2 = theta[1] > 0.0f ? theta[1] : 0.0f; + instance->wheel_model.k_current2 = theta[2] > 0.0f ? theta[2] : 0.0f; + instance->wheel_model.k0 = theta[3] > 0.0f ? theta[3] * 0.5f : 0.0f; + memcpy(instance->state.rls_theta, theta, sizeof(instance->state.rls_theta)); + chassis_power_debug_state = instance->state; +} + +void ChassisPowerSetWheelModel(ChassisPowerInstance *instance, const ChassisPowerMotorModel_s *model) +{ + if (instance == 0 || model == 0) { + return; + } + instance->wheel_model = *model; +} + +const ChassisPower_State_s *ChassisPowerGetState(const ChassisPowerInstance *instance) +{ + return instance != 0 ? &instance->state : 0; +} diff --git a/modules/chassis_power/chassis_power.h b/modules/chassis_power/chassis_power.h new file mode 100644 index 0000000..d00a5f6 --- /dev/null +++ b/modules/chassis_power/chassis_power.h @@ -0,0 +1,109 @@ +#ifndef CHASSIS_POWER_H +#define CHASSIS_POWER_H + +#include + +#include "rls.h" + +#define CHASSIS_POWER_RLS_ORDER 4u + +typedef struct +{ + float k_current_speed; + float k_speed2; + float k_current2; + float k0; +} ChassisPowerMotorModel_s; + +typedef struct +{ + ChassisPowerMotorModel_s wheel_model; + float default_power_limit; + float min_power_limit; + float buffer_low; + float buffer_high; + float buffer_gain; + float steering_power_reserve; + float min_scale; + float rls_lambda; + float rls_init_covariance; + uint8_t rls_enable; +} ChassisPower_Init_Config_s; + +typedef struct +{ + float rf_speed_ref; + float lb_speed_ref; + float rf_speed; + float lb_speed; + float rf_current_cmd; + float lb_current_cmd; + float rf_output_cmd; + float lb_output_cmd; + float rf_feedback_current; + float lb_feedback_current; + float referee_limit; + float referee_power; + float power_buffer; + float measured_power; + float steering_power_estimate; + uint8_t power_meter_online; +} ChassisPower_Limit_Input_s; + +typedef struct +{ + float rf_ref_raw; + float lb_ref_raw; + float rf_ref_limited; + float lb_ref_limited; + float rf_power; + float lb_power; + float estimated_wheel_power; + float estimated_total_power; + float allowed_power; + float wheel_budget; + float scale; + float measured_power; + float referee_power; + float power_buffer; + float rf_speed; + float lb_speed; + float rf_current_cmd; + float lb_current_cmd; + float rf_output_cmd; + float lb_output_cmd; + float rf_feedback_current; + float lb_feedback_current; + float features[CHASSIS_POWER_RLS_ORDER]; + float rls_theta[CHASSIS_POWER_RLS_ORDER]; + uint8_t limited; + uint8_t power_meter_online; +} ChassisPower_State_s; + +typedef struct +{ + ChassisPowerMotorModel_s wheel_model; + RLSInstance rls; + ChassisPower_State_s state; + float default_power_limit; + float min_power_limit; + float buffer_low; + float buffer_high; + float buffer_gain; + float steering_power_reserve; + float min_scale; + uint8_t rls_enable; +} ChassisPowerInstance; + +extern ChassisPower_State_s chassis_power_debug_state; + +ChassisPowerMotorModel_s ChassisPowerGetDefault3508Model(void); +void ChassisPowerInit(ChassisPowerInstance *instance, const ChassisPower_Init_Config_s *config); +float ChassisPowerEstimateMotor(const ChassisPowerMotorModel_s *model, float speed, float current_cmd); +void ChassisPowerUpdateMeasureState(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input); +float ChassisPowerLimitWheelRefs(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float *rf_ref, float *lb_ref); +void ChassisPowerUpdateRLS(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float measured_wheel_power); +void ChassisPowerSetWheelModel(ChassisPowerInstance *instance, const ChassisPowerMotorModel_s *model); +const ChassisPower_State_s *ChassisPowerGetState(const ChassisPowerInstance *instance); + +#endif diff --git a/modules/power_meter/power_meter.c b/modules/power_meter/power_meter.c new file mode 100644 index 0000000..5cdd813 --- /dev/null +++ b/modules/power_meter/power_meter.c @@ -0,0 +1,105 @@ +#include "power_meter.h" + +#include +#include + +#include "bsp_dwt.h" + +static void PowerMeterLostCallback(void *meter_ptr) +{ + PowerMeterInstance *meter = (PowerMeterInstance *)meter_ptr; + if (meter != 0) { + meter->online = 0u; + } +} + +static void PowerMeterRxCallback(CANInstance *can) +{ + if (can == 0 || can->id == 0 || can->rx_len < 4u) { + return; + } + + PowerMeterInstance *meter = (PowerMeterInstance *)can->id; + const uint8_t *data = can->rx_buff; + const float scale = meter->scale > 0.0f ? meter->scale : POWER_METER_DEFAULT_SCALE; + + meter->raw_current = (uint16_t)(((uint16_t)data[1] << 8) | data[0]); + meter->raw_voltage = (uint16_t)(((uint16_t)data[3] << 8) | data[2]); + meter->current = (float)meter->raw_current / scale; + meter->voltage = (float)meter->raw_voltage / scale; + meter->power = meter->voltage * meter->current; + meter->update_count++; + meter->last_update_time_ms = DWT_GetTimeline_ms(); + meter->online = 1u; + + if (meter->daemon != 0) { + DaemonReload(meter->daemon); + } +} + +PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *config) +{ + if (config == 0 || config->can_config.can_handle == 0) { + return 0; + } + + PowerMeterInstance *meter = (PowerMeterInstance *)malloc(sizeof(PowerMeterInstance)); + if (meter == 0) { + return 0; + } + memset(meter, 0, sizeof(PowerMeterInstance)); + + meter->scale = config->scale > 0.0f ? config->scale : POWER_METER_DEFAULT_SCALE; + if (config->can_config.rx_id == 0u) { + config->can_config.rx_id = POWER_METER_DEFAULT_CAN_ID; + } + if (config->can_config.tx_id == 0u) { + config->can_config.tx_id = config->can_config.rx_id; + } + + config->can_config.id = meter; + config->can_config.can_module_callback = PowerMeterRxCallback; + meter->can_ins = CANRegister(&config->can_config); + + Daemon_Init_Config_s daemon_config = { + .callback = PowerMeterLostCallback, + .owner_id = meter, + .reload_count = 5u, + .init_count = 100u, + }; + meter->daemon = DaemonRegister(&daemon_config); + + return meter; +} + +PowerMeterInstance *PowerMeterInitCan(CAN_HandleTypeDef *can_handle, uint32_t rx_id) +{ + PowerMeter_Init_Config_s config = { + .can_config = { + .can_handle = can_handle, + .tx_id = rx_id, + .rx_id = rx_id, + }, + .scale = POWER_METER_DEFAULT_SCALE, + }; + + return PowerMeterInit(&config); +} + +const PowerMeterInstance *PowerMeterGetData(PowerMeterInstance *meter) +{ + if (meter == 0) { + return 0; + } + + meter->online = PowerMeterIsOnline(meter); + return meter; +} + +uint8_t PowerMeterIsOnline(PowerMeterInstance *meter) +{ + if (meter == 0 || meter->daemon == 0) { + return 0u; + } + return DaemonIsOnline(meter->daemon); +} diff --git a/modules/power_meter/power_meter.h b/modules/power_meter/power_meter.h new file mode 100644 index 0000000..4e86df7 --- /dev/null +++ b/modules/power_meter/power_meter.h @@ -0,0 +1,38 @@ +#ifndef POWER_METER_H +#define POWER_METER_H + +#include + +#include "bsp_can.h" +#include "daemon.h" + +#define POWER_METER_DEFAULT_CAN_ID 0x213u +#define POWER_METER_DEFAULT_SCALE 100.0f + +typedef struct +{ + CAN_Init_Config_s can_config; + float scale; +} PowerMeter_Init_Config_s; + +typedef struct +{ + CANInstance *can_ins; + DaemonInstance *daemon; + float voltage; + float current; + float power; + float scale; + uint16_t raw_voltage; + uint16_t raw_current; + uint32_t update_count; + float last_update_time_ms; + uint8_t online; +} PowerMeterInstance; + +PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *config); +PowerMeterInstance *PowerMeterInitCan(CAN_HandleTypeDef *can_handle, uint32_t rx_id); +const PowerMeterInstance *PowerMeterGetData(PowerMeterInstance *meter); +uint8_t PowerMeterIsOnline(PowerMeterInstance *meter); + +#endif