robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add offline

Browse Source
This commit is contained in:
Robofish 2026-03-15 10:23:54 +08:00
parent 0ab60f81dd
commit 5ad9ef8f49
2 changed files with 36 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
User/module/balance_chassis.c
View File

@ -31,7 +31,6 @@ static void Chassis_UpdateChassisState(Chassis_t *c);
static void Chassis_ResetControllers(Chassis_t *c);
static void Chassis_SelectYawZeroPoint(Chassis_t *c);
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
static void Chassis_RecoverControl(Chassis_t *c);
/* Private functions -------------------------------------------------------- */
@ -543,6 +542,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->jump.launch_force = c->param->jump.launch_force;
c->jump.retract_leg_length = c->param->jump.retract_leg_length;
/* 初始化电机离线检测 */
c->motor_status.any_offline = false;
return CHASSIS_OK;
}
@ -559,10 +561,19 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
MOTOR_LK_UpdateAll();
/* 更新关节电机反馈 */
uint64_t now_us = BSP_TIME_Get_us();
bool any_offline = false;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
if (joint_motor != NULL) {
c->feedback.joint[i] = joint_motor->motor.feedback;
/* 检测关节电机离线：500ms 无更新 */
if (now_us - joint_motor->motor.header.last_online_time > 500000u) {
any_offline = true;
}
} else {
any_offline = true;
}
/* 机械零点调整 */
@ -580,9 +591,17 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
if (wheel_motor != NULL) {
c->feedback.wheel[i] = wheel_motor->motor.feedback;
/* 检测轮子电机离线：500ms 无更新 */
if (now_us - wheel_motor->motor.header.last_online_time > 500000u) {
any_offline = true;
}
} else {
any_offline = true;
}
}
c->motor_status.any_offline = any_offline;
/* 更新机体状态估计 */
Chassis_UpdateChassisState(c);
@ -615,6 +634,17 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;
c->lask_wakeup = BSP_TIME_Get_us();
/* 电机离线保护：有电机离线时强制 RELAX 并持续使能 */
if (c->motor_status.any_offline) {
if (c->mode != CHASSIS_MODE_RELAX) {
c->mode = CHASSIS_MODE_RELAX;
Chassis_ResetControllers(c);
}
Chassis_MotorRelax(c);
Chassis_MotorEnable(c);
return CHASSIS_OK;
}
/* 设置底盘模式 */
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
return CHASSIS_ERR_MODE;

5
User/module/balance_chassis.h
View File

@ -215,6 +215,11 @@ typedef struct {
Recover_State_t state[2]; /* 左右腿自起状态 */
} recover;
/* 电机离线检测 */
struct {
bool any_offline; /* 是否有电机离线 */
} motor_status;
/* PID计算的目标值 */
struct {
AHRS_Eulr_t chassis;