pbio/control: do not allow offset above limit
The actuation offset should never be higher than the actuation limit. Otherwise, the controller cannot do anything as all that happens is feed forward control.
@@ -301,7 +301,7 @@ void pbio_control_settings_get_limits(pbio_control_settings_t *s, int32_t *speed
}
pbio_error_t pbio_control_settings_set_limits(pbio_control_settings_t *s, int32_t speed, int32_t acceleration, int32_t actuation) {
- if (speed < 1 || acceleration < 1 || actuation < 1) {
+ if (speed < 1 || acceleration < 1 || actuation < 1 || actuation * s->actuation_scale <= s->control_offset) {
return PBIO_ERROR_INVALID_ARG;
}
s->max_rate = pbio_control_user_to_counts(s, speed);
@@ -320,7 +320,7 @@ void pbio_control_settings_get_pid(pbio_control_settings_t *s, int16_t *pid_kp,
}
pbio_error_t pbio_control_settings_set_pid(pbio_control_settings_t *s, int16_t pid_kp, int16_t pid_ki, int16_t pid_kd, int32_t integral_range, int32_t integral_rate, int32_t control_offset) {
- if (pid_kp < 0 || pid_ki < 0 || pid_kd < 0 || integral_range < 0) {
+ if (pid_kp < 0 || pid_ki < 0 || pid_kd < 0 || integral_range < 0 || integral_rate < 0 || control_offset < 0 || control_offset * s->actuation_scale >= s->max_control) {
return PBIO_ERROR_INVALID_ARG;
}
