構造函數

源代碼

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;

• 得到最新的滑動平均步態周期，用于后續所有需要步態相位的模塊。