SimpleFOC源码学习07 - Sensor类源码对比（V2.4.0 vs V2.3.2）

导言总的来说V2.4.0与V2.3.3对比后只有Sensor::getVelocity()有改动。一、源码1.1、V2.3.2的Sensor::getVelocity()floatSensor::getVelocity(){// calculate sample timefloatTs(angle_prev_ts-vel_angle_prev_ts)*1e-6;if(Ts0.0f){// handle micros() overflow - we need to reset vel_angle_prev_tsvel_angle_prevangle_prev;vel_full_rotationsfull_rotations;vel_angle_prev_tsangle_prev_ts;returnvelocity;}if(Tsmin_elapsed_time)returnvelocity;// dont update velocity if deltaT is too smallvelocity((float)(full_rotations-vel_full_rotations)*_2PI(angle_prev-vel_angle_prev))/Ts;vel_angle_prevangle_prev;vel_full_rotationsfull_rotations;vel_angle_prev_tsangle_prev_ts;returnvelocity;}1.2、V2.4.0的Sensor::getVelocity()/** get current angular velocity (rad/s) */floatSensor::getVelocity(){// calculate sample time// if timestamps were unsigned, we could get rid of this section, unsigned overflow handles it correctlyfloatTs(angle_prev_ts-vel_angle_prev_ts)*1e-6f;if(Ts0.0f){// handle micros() overflow - we need to reset vel_angle_prev_tsvel_angle_prevangle_prev;vel_full_rotationsfull_rotations;vel_angle_prev_tsangle_prev_ts;returnvelocity;}if(Tsmin_elapsed_time)returnvelocity;// dont update velocity if deltaT is too smallfloatcurrent_angle0.0f;floatprev_angle0.0f;// Avoid floating point precision loss for large full_rotations// this is likely optionalif(full_rotationsvel_full_rotations){current_angleangle_prev;prev_anglevel_angle_prev;}else{current_angle(float)full_rotations*_2PIangle_prev;prev_angle(float)vel_full_rotations*_2PIvel_angle_prev;}constfloatdelta_anglecurrent_angle-prev_angle;// floating point equality checks are bad, so instead we check that the angle change is very smallif(fabsf(delta_angle)1e-8f){velocitydelta_angle/Ts;vel_angle_prevangle_prev;vel_full_rotationsfull_rotations;vel_angle_prev_tsangle_prev_ts;}returnvelocity;}二、优化点2.1、对比总览优化点旧版 (2.3.x)新版 (2.4.0)浮点精度一律用全角度相减同圈时避免大数相减角度不动时的更新无论如何都更新状态增量为零时跳过更新乘法常数后缀1e-6double1e-6ffloat_2PI乘法位置展开在一个表达式里用 if/else 分支处理2.2、浮点精度保护最重要的优化旧版velocity((float)(full_rotations-vel_full_rotations)*_2PI(angle_prev-vel_angle_prev))/Ts;新版if(full_rotationsvel_full_rotations){current_angleangle_prev;prev_anglevel_angle_prev;}else{current_angle(float)full_rotations*_2PIangle_prev;prev_angle(float)vel_full_rotations*_2PIvel_angle_prev;}constfloatdelta_anglecurrent_angle-prev_angle;float只有 ~7 位有效十进制数字。当电机转了很多圈后full_rotations * _2PI是个大数例如 10000 圈 ≈ 62832而angle_prev是 0~2π 的小数。两者相加后小数部分的精度被大数吃掉了。新版在同圈时直接用裸角度相减完全避免了这个大数问题精度更高。2.3、角度静止时跳过状态更新旧版每次调用都无条件更新vel_angle_prev等状态变量。新版if(fabsf(delta_angle)1e-8f){velocitydelta_angle/Ts;vel_angle_prevangle_prev;vel_full_rotationsfull_rotations;vel_angle_prev_tsangle_prev_ts;}电机静止不动时delta_angle ≈ 0旧版会用接近 0 的分子除以 Ts得到一个接近 0 的速度并更新状态——这本身没错但如果传感器有微小噪声抖动每次都更新会导致 Ts 越来越小因为vel_angle_prev_ts一直在追angle_prev_ts进而velocity估算抖动加剧。新版角度不变就不更新时间戳让 Ts 保持拉长的状态下次测速时有更长的时间窗口噪声影响更小。2.4、字面量精度1e-6→1e-6f旧版float Ts (...) * 1e-6;新版float Ts (...) * 1e-6f;1e-6是double字面量会触发隐式的float→double→float转换在不支持硬件 double 的 MCU如 AVR、部分 ARM上会调用软件浮点库带来额外开销。加f后缀强制float运算在嵌入式平台上性能更好。三、总结2.4.0 的getVelocity()主要解决了两个问题多圈后的浮点精度损失同圈分支优化和静止时的噪声放大角度不变时不更新时间戳同时顺手修正了嵌入式平台上的double字面量性能问题。