Skip to content

hal: Update the pid component to 64-bit clean operation #4773

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
grandixximo merged 1 commit into from
Nov 4, 2026
Merged

hal: Update the pid component to 64-bit clean operation #4773

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 38 additions & 13 deletions src/hal/components/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,6 +225,7 @@ typedef struct {
hal_real_t maxoutput; /* pin: limit for PID output */
hal_real_t output; /* pin: the output value */
hal_bool_t saturated; /* pin: TRUE when the output is saturated */
rtapi_sint saturated_t; /* Sum of time in saturated state in nanoseconds */
hal_real_t saturated_s; /* pin: the time the output has been saturated */
hal_sint_t saturated_count; /* pin: the time the output has been saturated */
hal_bool_t index_enable; /* pin: to monitor for step changes that would otherwise screw up FF */
Expand All @@ -244,8 +245,8 @@ typedef struct {

/* Private data */
State state;
rtapi_u32 cycleCount;
rtapi_u32 cyclePeriod;
rtapi_uint cycleCount;
rtapi_uint cyclePeriod;
rtapi_real cycleAmplitude;
rtapi_real totalTime;
rtapi_real avgAmplitude;
Expand Down Expand Up @@ -350,7 +351,7 @@ static void
Pid_CycleEnd(hal_pid_t *pid)
{
pid->cycleCount++;
pid->avgAmplitude += pid->cycleAmplitude / hal_get_ui32(pid->tuneCycles);
pid->avgAmplitude += pid->cycleAmplitude / hal_get_uint(pid->tuneCycles);
pid->cycleAmplitude = 0;
pid->totalTime += pid->cyclePeriod * 0.000000001;
pid->cyclePeriod = 0;
Expand Down Expand Up @@ -440,16 +441,16 @@ Pid_AutoTune(hal_pid_t *pid, long period)

// Check if the last cycle just ended. This is really the number
// of half cycles.
if(pid->cycleCount < hal_get_ui32(pid->tuneCycles))
if(pid->cycleCount < hal_get_uint(pid->tuneCycles))
break;

// Calculate PID using Relay (Åström-Hägglund) method
hal_set_real(pid->ultimateGain, (4.0 * fabs(hal_get_real(pid->tuneEffort)))/(PI * pid->avgAmplitude));
hal_set_real(pid->ultimatePeriod, 2.0 * pid->totalTime / hal_get_ui32(pid->tuneCycles));
hal_set_real(pid->ultimatePeriod, 2.0 * pid->totalTime / hal_get_uint(pid->tuneCycles));
hal_set_real(pid->ff0gain, 0);
hal_set_real(pid->ff2gain, 0);

if(hal_get_ui32(pid->tuneType) == TYPE_PID){
if(hal_get_uint(pid->tuneType) == TYPE_PID){
// insert ultimate gain and period in Ziegler-Nichols PID method
hal_set_real(pid->pgain, 0.6 * hal_get_real(pid->ultimateGain));
hal_set_real(pid->igain, 1.2 * hal_get_real(pid->ultimateGain) / (hal_get_real(pid->ultimatePeriod)));
Expand Down Expand Up @@ -676,13 +677,36 @@ static void calc_pid(void *arg, long period)
/* set 'saturated' outputs */
if(pid->limit_state) {
hal_set_bool(pid->saturated, 1);
hal_set_real(pid->saturated_s, hal_get_real(pid->saturated_s) + period * 1e-9);
if(hal_get_si32(pid->saturated_count) != 2147483647)
hal_set_si32(pid->saturated_count, hal_get_si32(pid->saturated_count) + 1);

// The following was: saturated_s += period * 1e-9;
// Computing the sum of a small increment and a large value will result in
// loss of significance. There are no guarantees that 'period' is a perfect
// constant. Therefore, it must accumulate the actual period and scale
// accordingly using an integer intermediary type.
// The counter will wrap after about 292 years, which should be enough that
// several generations of engineers have been able to look at the progress
// and die before the wrap occurs.
pid->saturated_t += period;
hal_set_real(pid->saturated_s, pid->saturated_t * 1e-9);

// The 'saturated_count' increment now runs until 2**63-1 before it wraps.
// Therefore, the if() can be safely removed.
// The old saturated_count topped at 24.8 days with a 32-bit signed value and
// a 1 millisecond thread and the condition just caps that at its maximum
// value. Using the 64-bit signed value it will be in the order of 292 million
// years. I guess we'd not survive that and the machine will have been recycled
// at near molecular level by geological forces. Even a 1 microsecond thread
// would not reach the top before society has crumbled and humanity morphed
// into some natural selected genetically engineered savagery hidden in a newly
// constructed Dyson Sphere.
//
// if(hal_get_sint(pid->saturated_count) != 2147483647)
hal_set_sint(pid->saturated_count, hal_get_sint(pid->saturated_count) + 1);
} else {
hal_set_bool(pid->saturated, 0);
hal_set_real(pid->saturated_s, 0);
hal_set_si32(pid->saturated_count, 0);
hal_set_sint(pid->saturated_count, 0);
pid->saturated_t = 0;
}
/* done */
}
Expand Down Expand Up @@ -722,7 +746,7 @@ static int export_pid(hal_pid_t * addr, char * prefix)
CHK(hal_pin_new_real(comp_id, HAL_OUT, &(addr->output), 0.0, "%s.output", prefix));
CHK(hal_pin_new_bool(comp_id, HAL_OUT, &(addr->saturated), 0, "%s.saturated", prefix));
CHK(hal_pin_new_real(comp_id, HAL_OUT, &(addr->saturated_s), 0.0, "%s.saturated-s", prefix));
CHK(hal_pin_new_si32(comp_id, HAL_OUT, &(addr->saturated_count), 0, "%s.saturated-count", prefix));
CHK(hal_pin_new_sint(comp_id, HAL_OUT, &(addr->saturated_count), 0, "%s.saturated-count", prefix));
CHK(hal_pin_new_real(comp_id, HAL_IN, &(addr->pgain), 1.0, "%s.Pgain", prefix));
CHK(hal_pin_new_real(comp_id, HAL_IN, &(addr->igain), 0.0, "%s.Igain", prefix));
CHK(hal_pin_new_real(comp_id, HAL_IN, &(addr->dgain), 0.0, "%s.Dgain", prefix));
Expand All @@ -745,8 +769,8 @@ static int export_pid(hal_pid_t * addr, char * prefix)
#ifdef AUTO_TUNER
/* Auto tune related */
CHK(hal_pin_new_real(comp_id, HAL_IO, &(addr->tuneEffort), 0.5, "%s.tune-effort", prefix));
CHK(hal_pin_new_ui32(comp_id, HAL_IO, &(addr->tuneCycles), 50, "%s.tune-cycles", prefix));
CHK(hal_pin_new_ui32(comp_id, HAL_IO, &(addr->tuneType), TYPE_PID, "%s.tune-type", prefix));
CHK(hal_pin_new_uint(comp_id, HAL_IO, &(addr->tuneCycles), 50, "%s.tune-cycles", prefix));
CHK(hal_pin_new_uint(comp_id, HAL_IO, &(addr->tuneType), TYPE_PID, "%s.tune-type", prefix));
CHK(hal_pin_new_bool(comp_id, HAL_IN, &(addr->pTuneMode), 0, "%s.tune-mode", prefix));
CHK(hal_pin_new_bool(comp_id, HAL_IO, &(addr->pTuneStart), 0, "%s.tune-start", prefix));
#endif /* AUTO_TUNER */
Expand Down Expand Up @@ -782,6 +806,7 @@ static int export_pid(hal_pid_t * addr, char * prefix)
addr->prev_error = 0.0;
addr->prev_cmd = 0.0;
addr->limit_state = 0.0;
addr->saturated_t = 0;
#ifdef AUTO_TUNER
/* Initialize auto tune related values */
addr->state = STATE_PID;
Expand Down
Loading