[FEATURE] An idear for shaftAngle & filter.

[FFiot]

1. mechanic_angle & mechanic_angle_prev from 0° ~ 360°.
2. The angle changes from -180° to +180°.
3. rotation_cnt for angle control.

step 1:
if(mechanic_angle > mechanic_angle_prev + 180){
mechanic_angle -= 360;
}else if(mechanic_angle < mechanic_angle_prev - 180){
mechanic_angle += 360;
}

step 2:
LPF_angle(mechanic_angle);

step 3:
if(mechanic_angle >= 360.0f){
mechanic_angle -= 360.0f;
}else if(mechanic_angle < 0.0f){
mechanic_angle += 360.0f;
}

step 4:
if(LPF_angle.y_prev >= 360){
LPF_angle.y_prev -=360;
rotation_cnt += 1;
}
if(LPF_angle.y_prev < 0){
LPF_angle.y_prev +=360;
rotation_cnt -= 1；
}

[runger1101001]

I am not sure I understand… what problem are you trying to improve with this?

the shaft angle is already computed via LPF filtering, and the full rotations are accounted for in the sensor class…

[FFiot]

src\common\base_classes\Sensor.cpp Line 54：
float Sensor::getAngle(){
return (float)full_rotations * _2PI + angle_prev;
}

1. Do not need：(float)full_rotations * _2PI.
2. Angle & angle_prev from 0 ~ 360°. Angles will not become large.

// TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
// For this reason it is NOT precise when the angles become large.
// Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
// when switching to a 2-component representation.

[runger1101001]

I think internally it already works this way - see sensor.getMechanicalAngle()

the precision problem exists still in position mode - where you can command angles mich larger than 2PI

we would have to change the API for position mode to solve this problem.

on the sensor side it is already solved…

[dekutree64]

I do think this is the proper way to handle large angles, but it has a fairly large performance penalty to solve a problem that only affects a few users. The PID code in motor.move will have more added cost, and motor.target is still trouble.

I'm more inclined to abandon the idea of handling large angles entirely, and instead provide a way for users to manually wrap the angle back into the safe range when needed.

int32_t FOCMotor::wrapAngle(){
  int32_t full_rotations = sensor->wrapAngle();
  shaft_angle -= full_rotations*_2PI;
  LFP_angle.y_prev -= full_rotations*_2PI;
  return full_rotations;
}

LowPassFilter::y_prev will need to be made public or FOCMotor added as a friend class, and several sensor classes will need to implement wrapAngle, which in the case of the base class will simply be int32_t Sensor::wrapAngle(){int32_t temp = full_rotations; full_rotations = 0; return temp;}

I think most applications that need unlimited angle range are controlled by step/dir, in which case you don't even need to keep track of the full rotations. Just do step_counter -= motor.wrapAngle() * steps_per_revolution each frame and the problem is solved.

Or if you do need to keep track of it, call motor.wrapAngle() each frame and add the result to a full rotation counter. When commanding a new target, subtract that full rotation counter from the real high-resolution target value before passing to motor.move.

[runger1101001]

Or we kick this problem down the road until 64bit MCUs and double precision FPUs are the standard :-D