构造函数

源代码

Side::Side(bool is_left, ExoData* exo_data)
: _hip((config_defs::joint_id)((uint8_t)(is_left ? config_defs::joint_id::left : config_defs::joint_id::right) | (uint8_t)config_defs::joint_id::hip), exo_data)        //We need to cast to uint8_t to do bitwise or, then we have to cast it back to joint_id
, _knee((config_defs::joint_id)((uint8_t)(is_left ? config_defs::joint_id::left : config_defs::joint_id::right) | (uint8_t)config_defs::joint_id::knee), exo_data)
, _ankle((config_defs::joint_id)((uint8_t)(is_left ? config_defs::joint_id::left : config_defs::joint_id::right) | (uint8_t)config_defs::joint_id::ankle), exo_data)
, _elbow((config_defs::joint_id)((uint8_t)(is_left ? config_defs::joint_id::left : config_defs::joint_id::right) | (uint8_t)config_defs::joint_id::elbow), exo_data)
, _heel_fsr(is_left ? logic_micro_pins::fsr_sense_left_heel_pin : logic_micro_pins::fsr_sense_right_heel_pin) //Check if it is the left and use the appropriate pin for the side.
, _toe_fsr(is_left ? logic_micro_pins::fsr_sense_left_toe_pin : logic_micro_pins::fsr_sense_right_toe_pin)
{_data = exo_data;_is_left = is_left;//This data object is set for the specific side so we don't have to keep checking._side_data = _is_left ? &(_data->left_side) : &(_data->right_side);#ifdef SIDE_DEBUGlogger::print(_is_left ? "Left " : "Right ");logger::println("Side :: Constructor : _data set");#endif_prev_heel_contact_state = true;        //Initialized to true so we don't get a strike the first time we read_prev_toe_contact_state = true;_prev_toe_contact_state_toe_off = true;_prev_toe_contact_state_toe_on = true;for (int i = 0; i<_num_steps_avg; i++){_step_times[i] = 0;}_ground_strike_timestamp = 0;_prev_ground_strike_timestamp = 0;//_expected_step_duration = 0;#ifdef SIDE_DEBUGlogger::print(_is_left ? "Left " : "Right ");logger::println("Side :: Constructor : Exit");#endif_heel_fsr.get_contact_thresholds(_side_data->heel_fsr_lower_threshold, _side_data->heel_fsr_upper_threshold);_toe_fsr.get_contact_thresholds(_side_data->toe_fsr_lower_threshold, _side_data->toe_fsr_upper_threshold);inclination_detector = new InclinationDetector();
};

功能总结：

• 分配关节对象，自动绑定侧别（左/右），不需要在后续逻辑反复判断左/右。

• FSR传感器（heel/toe）自动选pin。

• 初始化状态机相关变量、步态窗口数组，防止“误触发”。

• 关联倾角检测器，用于动态地形或坡度环境下的自适应。

run_side - 核心

void Side::run_side()
{#ifdef SIDE_DEBUGlogger::print("\nmicros : ");logger::println(micros());logger::print(_is_left ? "Left " : "Right ");logger::println("Side :: run_side : checking calibration");#endifcheck_calibration();#ifdef SIDE_DEBUGlogger::print(_is_left ? "Left " : "Right ");logger::println("Side :: run_side : reading data");#endif_check_thresholds();//Read all the data before we calculate and send the new motor commandsread_data();#ifdef SIDE_DEBUGlogger::print(_is_left ? "Left " : "Right ");logger::println("Side :: run_side : updating motor commands");#endif//Calculates the new motor commands and sends them.update_motor_cmds();}; 

• 检查&触发FSR或关节标定流程
• FSR的阈值设定（防止标定变化）
• 读取传感器/关节/步态状态
• 计算并下发新的电机指令

read_data()

源代码

void Side::read_data()
{//Check the FSRs_side_data->heel_fsr = _heel_fsr.read();_side_data->toe_fsr = _toe_fsr.read();//Check if a ground strike is detected_side_data->ground_strike = _check_ground_strike();//If a strike is detected, update the expected duration of the step if (_side_data->ground_strike){_side_data->expected_step_duration = _update_expected_duration();}//Check if the toe on or toe off is occuring_side_data->toe_off = _check_toe_off();_side_data->toe_on = _check_toe_on();//If toe off is detected, updated expected stance durationif (_side_data->toe_off == true){_side_data->expected_stance_duration = _update_expected_stance_duration();}//If toe on is detected, update expected swing durationif (_side_data->toe_on == true){_side_data->expected_swing_duration = _update_expected_swing_duration();}//Calculate Percent Gait, Percent Stance, and Percent Swing_side_data->percent_gait = _calc_percent_gait();_side_data->percent_stance = _calc_percent_stance();_side_data->percent_swing = _calc_percent_swing();//Get the contact thesholds for the Heel and Toe FSRs_heel_fsr.get_contact_thresholds(_side_data->heel_fsr_lower_threshold, _side_data->heel_fsr_upper_threshold);_toe_fsr.get_contact_thresholds(_side_data->toe_fsr_lower_threshold, _side_data->toe_fsr_upper_threshold);//Check the inclination_side_data->inclination = inclination_detector->check(_side_data->toe_stance, _side_data->do_calibration_refinement_toe_fsr, _side_data->ankle.joint_position);//Check the joint sensors if the joint is used.if (_side_data->hip.is_used){_hip.read_data();}if (_side_data->knee.is_used){_knee.read_data();}if (_side_data->ankle.is_used){_ankle.read_data();}if (_side_data->elbow.is_used){_elbow.read_data();}};

FSR压力传感器读取与赋值

_side_data->heel_fsr = _heel_fsr.read();
_side_data->toe_fsr  = _toe_fsr.read();

• 这里调用了heel/toe FSR（脚跟/脚趾压力传感器）的read()方法，获得当前压力或传感器值，并存入对应的SideData成员。

• 这是所有步态事件检测（落地、抬脚、踢地等）的基础感知信号。

步态事件检测：落地（ground_strike）

_side_data->ground_strike = _check_ground_strike();

• 用FSR的“接触/离地”状态，判断本步是否发生了“落地”。

• 这种事件通常只在脚由“空中”转为“地面”时才判定为真，有效避免重复误判。

• 检测到ground_strike之后（即完成一次步态周期），会进入下个步骤。

步态周期自适应：期望步长更新

if (_side_data->ground_strike)
{_side_data->expected_step_duration = _update_expected_duration();
}

• 如果检测到新的一次落地事件，则用当前步长（两次落地时间差），更新一个滑动窗口（见前文分析）。

• 用窗口平均自适应地估计本侧当前的步长（步态周期），用于百分比进度估算和节奏自调。

• 这样能让外骨骼“跟随使用者节奏”，而不是死板设定。

Toe-Off/Toe-On事件检测与站立/摆动窗口更新

_side_data->toe_off = _check_toe_off();
_side_data->toe_on  = _check_toe_on();if (_side_data->toe_off)_side_data->expected_stance_duration = _update_expected_stance_duration();
if (_side_data->toe_on)_side_data->expected_swing_duration  = _update_expected_swing_duration();

• 分别检测“脚趾离地”（摆动开始）和“脚趾着地”（摆动结束/站立开始）。

• 每检测到一次事件，就用最新时长更新“期望站立/摆动时长”，同样采用滑动窗口方式。

• 这样做的好处：

  • 能区分不同步态阶段（stance/swing）。

  • 自动适应不同用户、速度、模式下的站立/摆动比例（如老人慢步和跑步的占比差别）。

步态百分比进度估算

_side_data->percent_gait   = _calc_percent_gait();
_side_data->percent_stance = _calc_percent_stance();
_side_data->percent_swing  = _calc_percent_swing();

• 利用已知的周期时长、事件时间戳等，实时计算本侧“已走过的步态百分比”、“已站立的比例”、“已摆动的比例”。

• 这些百分比是时序同步、控制器触发、力矩规划、数据记录的核心指标，也是下游“高级控制算法/自适应助力”必备输入。

FSR阈值动态读取（可能受标定/漂移影响）

_heel_fsr.get_contact_thresholds(_side_data->heel_fsr_lower_threshold, _side_data->heel_fsr_upper_threshold);
_toe_fsr.get_contact_thresholds(_side_data->toe_fsr_lower_threshold, _side_data->toe_fsr_upper_threshold);

• 获取heel/toe当前的“接触判定阈值”——可能在动态标定或补偿漂移后被修改。

• 这样后续所有事件检测判据都能“自适应传感器特性/环境变化”。

坡度/倾角检测

_side_data->inclination = inclination_detector->check(_side_data->toe_stance, _side_data->do_calibration_refinement_toe_fsr, _side_data->ankle.joint_position);

• 这里利用关节角、FSR、标定标志等信息，调用InclinationDetector检测当前步态的“地面坡度/使用者踝关节姿态”。

• 这对于动态坡度补偿、上下坡时的力矩修正、平地与障碍判别等场景非常有用。

关节级传感器/数据读取

if (_side_data->hip.is_used)    { _hip.read_data(); }
if (_side_data->knee.is_used)   { _knee.read_data(); }
if (_side_data->ankle.is_used)  { _ankle.read_data(); }
if (_side_data->elbow.is_used)  { _elbow.read_data(); }

• 调用各关节对象的 read_data() 方法，采集角度/力矩/速度/电流等状态。

• 是否“参与”由 is_used 决定，可灵活屏蔽部分关节，适应不同外骨骼拓扑。

• 关节本身负责数据融合和内部状态更新，不影响Side的主流程。

check_calibration()

源代码

void Side::check_calibration()
{if (_side_data->is_used){//Make sure FSR calibration is done before refinement.if (_side_data->do_calibration_toe_fsr){_side_data->do_calibration_toe_fsr = _toe_fsr.calibrate(_side_data->do_calibration_toe_fsr);_data->set_status(status_defs::messages::fsr_calibration);}else if (_side_data->do_calibration_refinement_toe_fsr) {_side_data->do_calibration_refinement_toe_fsr = _toe_fsr.refine_calibration(_side_data->do_calibration_refinement_toe_fsr);_data->set_status(status_defs::messages::fsr_refinement);}if (_side_data->do_calibration_heel_fsr){_side_data->do_calibration_heel_fsr = _heel_fsr.calibrate(_side_data->do_calibration_heel_fsr);_data->set_status(status_defs::messages::fsr_calibration);}else if (_side_data->do_calibration_refinement_heel_fsr) {_side_data->do_calibration_refinement_heel_fsr = _heel_fsr.refine_calibration(_side_data->do_calibration_refinement_heel_fsr);_data->set_status(status_defs::messages::fsr_refinement);}//Check the joint sensors if the joint is used.if (_side_data->hip.is_used){_hip.check_calibration();}if (_side_data->knee.is_used){_knee.check_calibration();}if (_side_data->ankle.is_used){_ankle.check_calibration();}if (_side_data->elbow.is_used){_elbow.check_calibration();}}        
};

• 此函数是整个“单侧传感与控制链”的标定调度枢纽，上层能清晰了解当前哪个部分正在标定。

• 典型用法场景：UI/蓝牙/上位机发出一次标定指令→ 相关flag置位 → 控制循环自动推进流程 → 标定完成自动退回。

• 便于加装其他类型传感器或关节——只要新增对应的do_calibration_xxx和check_calibration()接口就可以。

• 整个流程不阻塞主控循环，适合穿戴场景的高鲁棒、快速上电自检。

整体结构：只对启用侧做标定

if (_side_data->is_used)
{// ...
}

• 只对被启用的side（即实际接入的左/右腿）执行标定流程，未启用的侧完全跳过，提高效率且防止误操作。

FSR（压力传感器）脚趾标定与精细化标定

if (_side_data->do_calibration_toe_fsr)
{_side_data->do_calibration_toe_fsr = _toe_fsr.calibrate(_side_data->do_calibration_toe_fsr);_data->set_status(status_defs::messages::fsr_calibration);
}
else if (_side_data->do_calibration_refinement_toe_fsr) 
{_side_data->do_calibration_refinement_toe_fsr = _toe_fsr.refine_calibration(_side_data->do_calibration_refinement_toe_fsr);_data->set_status(status_defs::messages::fsr_refinement);
}

• 先判断do_calibration_toe_fsr（脚趾FSR是否需要标定）。若为真，执行FSR初始标定流程（如采集多帧信号平均、设阈值等）。

• 标定完成后，calibrate返回false，flag被置0，防止重复执行。

• 标定时同步全局状态，让上位机/LED/UI等实时反映“当前正处于FSR标定中”。

• 只有初始标定完成后，才允许进入精细化（refine_calibration）。这个流程多用于二次补偿、更高精度调节。

• 这种分阶段、自动推进的设计，保障了标定流程不会混乱，也方便用户交互（比如按钮点一次就是一轮完整流程）。

FSR脚跟标定与精细化标定

if (_side_data->do_calibration_heel_fsr)
{_side_data->do_calibration_heel_fsr = _heel_fsr.calibrate(_side_data->do_calibration_heel_fsr);_data->set_status(status_defs::messages::fsr_calibration);
}
else if (_side_data->do_calibration_refinement_heel_fsr) 
{_side_data->do_calibration_refinement_heel_fsr = _heel_fsr.refine_calibration(_side_data->do_calibration_refinement_heel_fsr);_data->set_status(status_defs::messages::fsr_refinement);
}

• 脚跟FSR与脚趾FSR标定流程完全一样，各自独立，互不干扰。

• 支持先后/多轮分阶段标定，适合应对传感器偏移、环境变化等。

关节（Hip/Knee/Ankle/Elbow）逐个检查并执行标定

if (_side_data->hip.is_used)   { _hip.check_calibration(); }
if (_side_data->knee.is_used)  { _knee.check_calibration(); }
if (_side_data->ankle.is_used) { _ankle.check_calibration(); }
if (_side_data->elbow.is_used) { _elbow.check_calibration(); }

• 对每个关节（只要在本侧被启用）依次下发check_calibration()，这会递归调用到关节/电机/角度传感器等子系统的标定。

• 这种“递归式”设计保证了上层Side类只需关注高层流程，不用管每种关节的细节，强解耦。

• 新增关节只需实现自己的check_calibration即可，对系统无侵入。

_check_ground_strike()

源代码

bool Side::_check_ground_strike()
{_side_data->prev_heel_stance = _prev_heel_contact_state;  _side_data->prev_toe_stance = _prev_toe_contact_state;bool heel_contact_state = _heel_fsr.get_ground_contact();bool toe_contact_state = _toe_fsr.get_ground_contact();_side_data->heel_stance = heel_contact_state;_side_data->toe_stance = toe_contact_state;bool ground_strike = false;// logger::print("Side::_check_ground_strike : _prev_heel_contact_state - ");// logger::print(_prev_heel_contact_state);// logger::print("\n");// logger::print("\t_prev_toe_contact_state - ");// logger::print(_prev_toe_contact_state);// logger::print("\n");//Only check if in swing_side_data->toe_strike = toe_contact_state > _prev_toe_contact_state;if(!_prev_heel_contact_state & !_prev_toe_contact_state) //If we were previously in swing{//Check for rising edge on heel and toe, toe is to account for flat foot landingsif ((heel_contact_state > _prev_heel_contact_state) | (toe_contact_state > _prev_toe_contact_state))    //If either the heel or toe FSR is on the ground and it previously wasn't on the ground{ground_strike = true;_prev_ground_strike_timestamp = _ground_strike_timestamp;_ground_strike_timestamp = millis();}}_prev_heel_contact_state = heel_contact_state;_prev_toe_contact_state = toe_contact_state;return ground_strike;
};

_check_ground_strike() 是步态周期检测的核心，用于识别足底由“空中（摆动）”到“着地”这一关键事件（即“ground strike”/落地瞬间），它是外骨骼助力/数据记录的基本触发点。

保存上一次heel/toe接触状态

_side_data->prev_heel_stance = _prev_heel_contact_state;  
_side_data->prev_toe_stance = _prev_toe_contact_state;

把上一次采样时的“脚跟/脚趾是否触地”状态，存到side数据结构中，方便全局分析和后续计算（比如UI、数据同步、事件溯源）。

读取当前heel/toe的触地状态

bool heel_contact_state = _heel_fsr.get_ground_contact();
bool toe_contact_state = _toe_fsr.get_ground_contact();

• 实时读取脚跟、脚趾FSR（力敏电阻）当前是否“有地面压力”。

• 这通常是一个bool（0/1），传感器阈值判决由FSR类内部实现。

同步当前heel/toe状态到side全局数据

_side_data->heel_stance = heel_contact_state;
_side_data->toe_stance = toe_contact_state;

• 方便整个系统（UI、控制、记录）直接用side数据就能拿到“当前这一帧”的足底状态。

步态事件标志初始化

bool ground_strike = false;

• 初始化一个flag，用于记录“这一帧是否检测到地面冲击事件（足部落地）”。

实时记录toe strike事件

_side_data->toe_strike = toe_contact_state > _prev_toe_contact_state;

• 如果当前脚趾由“未触地”变成“触地”（即从0变1），则toe_strike为真，代表检测到脚趾首次着地。

• 用于某些步态模型（如平足或脚尖先着地的人群）。

只在摆动相（悬空）检测ground strike

if(!_prev_heel_contact_state & !_prev_toe_contact_state) //If we were previously in swing
{//...
}

• 只有上一次采样时，脚跟和脚趾都没触地（即这只脚在摆动），才判定接下来有无落地事件。

• 避免反复触发、只在真实“从空中到落地”那一瞬间识别。

检测heel或toe的上升沿（rising edge）

if ((heel_contact_state > _prev_heel_contact_state) | (toe_contact_state > _prev_toe_contact_state))
{ground_strike = true;_prev_ground_strike_timestamp = _ground_strike_timestamp;_ground_strike_timestamp = millis();
}

• 如果“脚跟”或“脚趾”任何一个传感器状态从0变1（刚好踩到地面），判定为ground strike。

• 为什么要“或”而不是“与”？——有人落地先脚跟，有人先脚趾（甚至平足），都能检测到，更健壮。

• 一旦检测到，更新落地时间戳，为步态周期估算等后续分析做准备。

更新历史状态

_prev_heel_contact_state = heel_contact_state;
_prev_toe_contact_state = toe_contact_state;

• 把本帧的触地状态存下来，供下次采样时做“上升沿”判断。

返回ground strike事件标志

return ground_strike;

• 外部主循环会用它来决定是否触发步态事件、调整助力、记录数据等。

_check_toe_on()

源代码

bool Side::_check_toe_on()
{bool toe_on = false;if (_side_data->toe_stance > _prev_toe_contact_state_toe_on){toe_on = true;_prev_toe_strike_timestamp = _toe_strike_timestamp;_toe_strike_timestamp = millis();}_prev_toe_contact_state_toe_on = _side_data->toe_stance;return toe_on;
};

• 步态检测：toe on常用于区分“摆动相结束、站立相开始”，也就是脚尖第一次踩到地面。

• 实时性强：通过对比上/下采样周期，只会在真正状态变化瞬间触发，避免长时间触地反复触发。

• 事件驱动：便于同步下游模块（比如：助力控制、数据采集、相位切换、计时等）。

• 时间戳为分析和自适应奠定基础：累计周期时间可用来估算步频、步幅、异常检测等。

变量初始化

bool toe_on = false;

• 初始设置返回值为false，即“默认本周期没有检测到toe on事件”。

检测 toe on 上升沿

if (_side_data->toe_stance > _prev_toe_contact_state_toe_on)
{toe_on = true;_prev_toe_strike_timestamp = _toe_strike_timestamp;_toe_strike_timestamp = millis();
}

• _side_data->toe_stance 是当前采样帧脚趾的触地状态（一般是bool，0/1）。

• _prev_toe_contact_state_toe_on 是上一帧的脚趾触地状态。

• 如果 当前 > 上一帧，说明刚刚由“悬空”变为“落地”，也就是toe on事件发生了。

• 一旦检测到：

  • toe_on = true，主循环可根据它做响应（如记录步态相位切换、辅助控制等）。

  • 时间戳管理：

    • _prev_toe_strike_timestamp = _toe_strike_timestamp; 把“上次toe on”的时间存为“前一次”，方便统计周期、滤除异常等。

  • _toe_strike_timestamp = millis(); 记录本次toe on发生的时刻。

保存本次 toe 状态

_prev_toe_contact_state_toe_on = _side_data->toe_stance;

• 更新 toe 状态，为下次采样时做“上升沿检测”准备。

返回结果

return toe_on;

• 返回是否检测到 toe on 事件。

_calc_percent_gait()

实时计算当前步态周期内已经走了多少百分比，也就是gait phase progress，通常用于步态相位同步、控制器参数插值、实时分析等场景。

源码

float Side::_calc_percent_gait()
{int timestamp = millis();int percent_gait = -1;//Only calulate if the expected step duration has been established.if (_side_data->expected_step_duration > 0){percent_gait = 100 * ((float)timestamp - _ground_strike_timestamp) / _side_data->expected_step_duration;percent_gait = min(percent_gait, 100); //Set saturation.// logger::print("Side::_calc_percent_gait : percent_gait_x10 = ");// logger::print(percent_gait_x10);// logger::print("\n");}return percent_gait;
};

获取当前时间戳

int timestamp = millis();

• millis() 返回系统运行到现在的毫秒数。

• 这个时间戳用来计算距离本周期步态开始（上一次ground strike）已经过去多久。

默认值初始化

int percent_gait = -1;

• 默认值为-1，表明当前还无法判断百分比（比如未初始化或步态未检测到）。

只有有期望步态周期时才进行计算

if (_side_data->expected_step_duration > 0)
{percent_gait = 100 * ((float)timestamp - _ground_strike_timestamp) / _side_data->expected_step_duration;percent_gait = min(percent_gait, 100); //Set saturation.
}

• 只有expected_step_duration（期望步态周期，单位ms）大于0才进行计算，防止未初始化导致错误。

• 计算公式为：

  • ((float)timestamp - _ground_strike_timestamp)：距离上次落地（ground strike）已经过去的时间。

  • 除以期望步态周期，得到本周期已完成的比例。

  • 乘100，得到百分数。

  • 最后min(percent_gait, 100)：保证最大不会超过100%（即0-100之间），防止走慢或滞后造成溢出。

返回结果

return percent_gait;

• 返回步态周期百分比，供后续模块（如相位插值、控制曲线索引、步态进度显示）使用。

_update_expected_duration()

• Side::_update_expected_duration() 的作用是自适应地估算当前步态周期（一步所需的平均时长），用来动态调整后续的步态判别、步态百分比计算、控制器相位插值等参数。
• 它的核心是“滑动平均+窗口判断”，有点像低通滤波器+异常剔除，适应用户步频的变化。

源码

float Side::_update_expected_duration()
{unsigned int step_time = _ground_strike_timestamp - _prev_ground_strike_timestamp;float expected_step_duration = _side_data->expected_step_duration;if (0 == _prev_ground_strike_timestamp) //If the prev time isn't set just return.{return expected_step_duration;}uint8_t num_uninitialized = 0;//Check that everything is set.for (int i = 0; i < _num_steps_avg; i++){num_uninitialized += (_step_times[i] == 0);}//Get the max and min values of the array for determining the window for expected values.unsigned int* max_val = std::max_element(_step_times, _step_times + _num_steps_avg);unsigned int* min_val = std::min_element(_step_times, _step_times + _num_steps_avg);if  (num_uninitialized > 0)  //If all the values haven't been replaced{//Shift all the values and insert the new onefor (int i = (_num_steps_avg - 1); i>0; i--){_step_times[i] = _step_times[i-1];}_step_times[0] = step_time;// logger::print("Side::_update_expected_duration : _step_times not fully initialized- [\t");// for (int i = 0; i < _num_steps_avg; i++)// {// logger::print(_step_times[i]);// logger::print("\t");// }// logger::print("\t]\n");    }//Consider it a good step if the ground strike falls within a window around the expected duration. Then shift the step times and put in the new value.else if ((step_time <= (_side_data->expected_duration_window_upper_coeff * *max_val)) & (step_time >= (_side_data->expected_duration_window_lower_coeff * *min_val))) // and (armed_time > ARMED_DURATION_PERCENT * self.expected_duration)): # a better check can be used.  If the person hasn't stopped or the step is good update the vector.  {int sum_step_times = step_time;for (int i = (_num_steps_avg - 1); i>0; i--){sum_step_times += _step_times[i-1];_step_times[i] = _step_times[i-1];}_step_times[0] = step_time;expected_step_duration = sum_step_times / _num_steps_avg;  //Average to the nearest ms// logger::print("Side::_update_expected_duration : _expected_step_duration - ");// logger::print(_expected_step_duration);// logger::print("\n");}return expected_step_duration;
};

步态周期测量

unsigned int step_time = _ground_strike_timestamp - _prev_ground_strike_timestamp;

用当前一次落地（ground strike）和上一次的时间戳差，得到本次步态周期的实际时长。

初始化与边界检查

if (0 == _prev_ground_strike_timestamp) //If the prev time isn't set just return.
{return expected_step_duration;
}

• 如果是第一次运行，_prev_ground_strike_timestamp还没有设置，直接返回之前的expected_step_duration，避免出错。

检查滑动窗口内未初始化的数量

uint8_t num_uninitialized = 0;
for (int i = 0; i < _num_steps_avg; i++)
{num_uninitialized += (_step_times[i] == 0);
}

• _step_times[]保存最近N步的周期（比如N=5或N=10）。

• 只要有一个没初始化（==0），num_uninitialized就大于0。

获取滑动窗口的最大最小值（为“异常剔除窗口”做准备）

unsigned int* max_val = std::max_element(_step_times, _step_times + _num_steps_avg);
unsigned int* min_val = std::min_element(_step_times, _step_times + _num_steps_avg);

• 记录最近N步最大和最小的步态周期。

初始化期：填满滑动窗口

if  (num_uninitialized > 0)
{//Shift all the values and insert the new onefor (int i = (_num_steps_avg - 1); i>0; i--){_step_times[i] = _step_times[i-1];}_step_times[0] = step_time;
}

• 只要有没填满的数据，采用“先进先出”方式：把新的step_time插到第0个，其他依次后移。

• 填满滑动窗口后才进行下一步的“正常采样和异常剔除”。

正常采样期：窗口剔除+滑动平均

else if ((step_time <= (_side_data->expected_duration_window_upper_coeff * *max_val)) & (step_time >= (_side_data->expected_duration_window_lower_coeff * *min_val)))
{int sum_step_times = step_time;for (int i = (_num_steps_avg - 1); i>0; i--){sum_step_times += _step_times[i-1];_step_times[i] = _step_times[i-1];}_step_times[0] = step_time;expected_step_duration = sum_step_times / _num_steps_avg;  //Average to the nearest ms
}

• 如果新采样的step_time在合理的窗口范围内（上下限由expected_duration_window_upper_coeff和expected_duration_window_lower_coeff修正的最大/最小步长）：

  • 将其加入滑动窗口（同上）。

  • 用全部N步求均值，得到新的expected_step_duration。

  • 这样可以“剔除异常值”（如偶发停顿、抖动导致的极大极小步长），只吸收正常步态变化。

  • 如果不在窗口内，则忽略本次采样（不更新均值，防止异常值污染估算）。

返回

return expected_step_duration;

• 得到最新的滑动平均步态周期，用于后续所有需要步态相位的模块。