Skip to content

hal: Update more easy-to-convert components to getter/setter #4272

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
Jul 28, 2026
Merged

hal: Update more easy-to-convert components to getter/setter #4272

Show file tree
Hide file tree
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
78 changes: 39 additions & 39 deletions src/hal/components/anglejog.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,28 +36,28 @@ pin is current-scale-out as it depends on the iscale-factor setting.
Simulation Config: `configs/sim/axis/anglejog/anglejog.in`
""";

pin in bit enable_in "enables motion (disables alteration of angle and scale)";
pin in s32 counts_in "MPG (wheel) counts";
pin in float angle_degrees_in "vector angle";
pin in s32 iscale_factor = 10000 "integer scaling factor (>1)";
pin in float scale_in "magnitude units/count (mag = counts * scale)";
pin in float max_vel "vector max velocity magnitude";
pin in float max_accel "vector max acceleration magnitude";
pin in float accel_fraction_in = 1 "acceleration fraction input";
pin in bool enable_in "enables motion (disables alteration of angle and scale)";
pin in si32 counts_in "MPG (wheel) counts";
pin in real angle_degrees_in "vector angle";
pin in si32 iscale_factor = 10000 "integer scaling factor (>1)";
pin in real scale_in "magnitude units/count (mag = counts * scale)";
pin in real max_vel "vector max velocity magnitude";
pin in real max_accel "vector max acceleration magnitude";
pin in real accel_fraction_in = 1 "acceleration fraction input";

pin out bit enable_out "to: axis.M.jog-enable AND axis.N.jog-enable";
pin out float current_scale "effective scale (informational)";
pin out float current_scale_out "to: axis.M.jog-scale AND axis.N.jog-scale";
pin out s32 coscounts "to: axis.M.jog-counts (cosine counts)";
pin out s32 sincounts "to: axis.N.jog-counts (sine counts)";
pin out float cos_accel_fraction "to: axis.M.jog-accel-fraction";
pin out float sin_accel_fraction "to: axis.N.jog-accel-fraction";
pin out bool enable_out "to: axis.M.jog-enable AND axis.N.jog-enable";
pin out real current_scale "effective scale (informational)";
pin out real current_scale_out "to: axis.M.jog-scale AND axis.N.jog-scale";
pin out si32 coscounts "to: axis.M.jog-counts (cosine counts)";
pin out si32 sincounts "to: axis.N.jog-counts (sine counts)";
pin out real cos_accel_fraction "to: axis.M.jog-accel-fraction";
pin out real sin_accel_fraction "to: axis.N.jog-accel-fraction";

// output monitor pins:
pin out bit active "angle jog move in progress";
pin out float current_angle_degrees "current angle";
pin out float current_mag "current vector magnitude";
pin out float current_vel "current vector speed";
pin out bool active "angle jog move in progress";
pin out real current_angle_degrees "current angle";
pin out real current_mag "current vector magnitude";
pin out real current_vel "current vector speed";

function _;
license "GPL";
Expand All @@ -66,7 +66,7 @@ author "Dewey Garrett";
#include <rtapi_math.h>
#define TO_RAD M_PI/180
// replicate simple_tp.h define for tiny magnitude delta:
#define TINY_DP(max_accel,period) (max_accel*period*period*0.001)
#define TINY_DP(_max_accel,_period) ((_max_accel)*(_period)*(_period)*0.001)
#define MIN_ISCALE_FACTOR 10
#define MAX_ISCALE_FACTOR 100000

Expand All @@ -83,9 +83,9 @@ static int ifactor = 0;
int newcounts;

if (once) {
current_angle_degrees = angle_degrees_in;
current_scale = scale_in;
current_scale_out = scale_in/iscale_factor;
current_angle_degrees_set(angle_degrees_in);
current_scale_set(scale_in);
current_scale_out_set(scale_in/iscale_factor);
ifactor = iscale_factor;
once = 0;
}
Expand All @@ -100,13 +100,13 @@ static int ifactor = 0;
delta_counts = newcounts - old_counts_in;
old_enable_in = enable_in;
old_counts_in = newcounts;
enable_out = enable_in;
enable_out_set(enable_in);

if (delta_counts!=0) {wait_for_count_change = 0;}
if (enable_in) { tot_counts = tot_counts + delta_counts;}
mag_cmd = tot_counts * current_scale_out;

active = 0;
active_set(0);
/* compute max change in velocity per servo period */
max_dv = max_accel * fperiod;
/* compute a tiny magnitude range, to be treated as zero */
Expand All @@ -123,12 +123,12 @@ static int ifactor = 0;
vel_req = -max_dv +
sqrt(2.0 * max_accel * mag_err + max_dv * max_dv);
/* mark planner as active */
active = 1;
active_set(1);
} else if (mag_err < -tiny_dp) {
vel_req = max_dv -
sqrt(-2.0 * max_accel * mag_err + max_dv * max_dv);
/* mark planner as active */
active = 1;
active_set(1);
} else {
/* within 'tiny_dp' of desired mag, no need to move */
vel_req = 0.0;
Expand Down Expand Up @@ -158,11 +158,11 @@ static int ifactor = 0;
}
}
}
current_angle_degrees = angle_degrees_in;
current_angle_degrees_set(angle_degrees_in);
if ( current_scale_out != scale_in
|| new_ifactor) {
current_scale = scale_in;
current_scale_out = scale_in/ifactor;
current_scale_set(scale_in);
current_scale_out_set(scale_in/ifactor);
tot_counts = current_mag/current_scale_out;
}
}
Expand All @@ -176,24 +176,24 @@ static int ifactor = 0;
}
/* ramp velocity toward request at accel limit */
if (vel_req > current_vel + max_dv) {
current_vel += max_dv;
current_vel_set(current_vel + max_dv);
} else if (vel_req < current_vel - max_dv) {
current_vel -= max_dv;
current_vel_set(current_vel - max_dv);
} else {
current_vel = vel_req;
current_vel_set(vel_req);
}
/* check for still moving */
if (current_vel != 0.0) {
/* yes, mark planner active */
active = 1;
active_set(1);
}
/* integrate velocity to get new magnitude */
current_mag += current_vel * fperiod;
current_mag_set(current_mag + current_vel * fperiod);
double cos_angle,sin_angle;
cos_angle = cos(current_angle_degrees * TO_RAD);
sin_angle = sin(current_angle_degrees * TO_RAD);
coscounts = current_mag * cos_angle/current_scale_out;
sincounts = current_mag * sin_angle/current_scale_out;
cos_accel_fraction = accel_fraction_in * cos_angle;
sin_accel_fraction = accel_fraction_in * sin_angle;
coscounts_set(current_mag * cos_angle/current_scale_out);
sincounts_set(current_mag * sin_angle/current_scale_out);
cos_accel_fraction_set(accel_fraction_in * cos_angle);
sin_accel_fraction_set(accel_fraction_in * sin_angle);
}
62 changes: 31 additions & 31 deletions src/hal/components/axistest.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,22 +1,22 @@
component axistest """\
Used to allow testing of an axis. Used IN PnCconf.""";
pin in bit jog-minus "Drive TRUE to jog the axis in its negative ('minus') direction.";
pin in bit jog-plus "Drive TRUE to jog the axis in its positive direction.";
pin in bit run "Drive TRUE to run the axis near its current position_fb with a trapezoidal velocity profile.";
pin in float maxvel "Maximum velocity";
pin in float amplitude "Approximate amplitude of positions to command during 'run'";
pin in s32 dir "Direction from central point to test: 0 = both, 1 = positive, 2 = negative";
pin out float position-cmd;
pin in float position-fb;
pin out bit running;
pin out float run-target;
pin out float run-start;
pin out float run-low;
pin out float run-high;
pin in s32 pause = 0 "Pause time for each end of run in seconds";
param rw float epsilon = .001;
pin in bool jog-minus "Drive TRUE to jog the axis in its negative ('minus') direction.";
pin in bool jog-plus "Drive TRUE to jog the axis in its positive direction.";
pin in bool run "Drive TRUE to run the axis near its current position_fb with a trapezoidal velocity profile.";
pin in real maxvel "Maximum velocity";
pin in real amplitude "Approximate amplitude of positions to command during 'run'";
pin in si32 dir "Direction from central point to test: 0 = both, 1 = positive, 2 = negative";
pin in real position-fb;
pin out real position-cmd;
pin out bool running;
pin out real run-target;
pin out real run-start;
pin out real run-low;
pin out real run-high;
pin in si32 pause = 0 "Pause time for each end of run in seconds";
param rw real epsilon = .001;
variable double timer;
param r float elapsed "Current value of the internal timer";
param r real elapsed "Current value of the internal timer";
variable int timer_on;
function update;
license "GPL";
Expand All @@ -27,19 +27,19 @@ extern double fabs(double);
if (timer_on) {
timer += fperiod;
}
elapsed = timer;
elapsed_set(timer);
if(run) {
if(!running) {
running = 1;
run_start = position_fb;
running_set(1);
run_start_set(position_fb);

if(dir == 2) run_high = run_start;
else run_high = run_start + amplitude;
if(dir == 2) run_high_set(run_start);
else run_high_set(run_start + amplitude);

if(dir == 1) run_low = run_start;
else run_low = run_start - amplitude;
if(dir == 1) run_low_set(run_start);
else run_low_set(run_start - amplitude);

position_cmd = run_low;
position_cmd_set(run_low);
}

if(fabs(position_fb - position_cmd) < epsilon) {
Expand All @@ -50,26 +50,26 @@ if(run) {
} else if (timer >= pause) {
timer_on = false;
if(position_cmd == run_low) {
position_cmd = run_high;
position_cmd_set(run_high);
} else {
position_cmd = run_low;
position_cmd_set(run_low);
}
}
}
}
} else if(running) {
position_cmd = run_start;
position_cmd_set(run_start);
if(fabs(position_fb - run_start) < epsilon) {
running = 0;
running_set(0);
timer_on = false;
}
} else {
if(jog_minus) {
position_cmd = position_fb - 10;
position_cmd_set(position_fb - 10);
} else if(jog_plus) {
position_cmd = position_fb + 10;
position_cmd_set(position_fb + 10);
} else {
position_cmd = position_fb;
position_cmd_set(position_fb);
}
}

55 changes: 28 additions & 27 deletions src/hal/components/biquad.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,31 +38,31 @@ component biquad "Biquad IIR filter";
description """Biquad IIR filter. Implements the following transfer function:
H(z) = (n~0~ + n~1~z^-1^ + n~2~z^-2^) / (1 + d~1~z^-1^ + d~2~z^-2^)""";

pin in float in "Filter input.";
pin out float out "Filter output.";
pin in bit enable = 0 "Filter enable. When false, the *in* pin \
pin in real in "Filter input.";
pin out real out "Filter output.";
pin in bool enable = 0 "Filter enable. When false, the *in* pin \
is passed to the *out* pin without any filtering. \
A *transition from false to true* causes filter \
coefficients to be calculated according to the current \
*type* and the describing pin and parameter settings";
pin out bit valid = 0 "When false, indicates an error occurred when calculating \
pin out bool valid = 0 "When false, indicates an error occurred when calculating \
filter coefficients (require 2>**Q**>0.5 and *f0*>sampleRate/2)";

pin in u32 type_ = 0 "Filter type determines the type of filter \
pin in ui32 type_ = 0 "Filter type determines the type of filter \
coefficients calculated. When 0, coefficients must be loaded directly \
from the *n0,n1,n2,d1* params. When 1, \
a low pass filter is created specified by the *f0,Q* pins. \
When 2, a notch filter is created specified by the *f0,Q* pins.";
pin in float f0 = 250.0 "The corner frequency of the filter.";
pin in float Q = 0.7071 "The Q of the filter.";
pin in real f0 = 250.0 "The corner frequency of the filter.";
pin in real Q = 0.7071 "The Q of the filter.";

param rw float d1 = 0.0 "1st-delayed denominator coef";
param rw float d2 = 0.0 "2nd-delayed denominator coef";
param rw float n0 = 1.0 "non-delayed numerator coef";
param rw float n1 = 0.0 "1st-delayed numerator coef";
param rw float n2 = 0.0 "2nd-delayed numerator coef";
pin out float s1 = 0.0 "1st-delayed internal state (for debug only)";
pin out float s2 = 0.0 "2nd-delayed internal state (for debug only)";
param rw real d1 = 0.0 "1st-delayed denominator coef";
param rw real d2 = 0.0 "2nd-delayed denominator coef";
param rw real n0 = 1.0 "non-delayed numerator coef";
param rw real n1 = 0.0 "1st-delayed numerator coef";
param rw real n2 = 0.0 "2nd-delayed numerator coef";
pin out real s1 = 0.0 "1st-delayed internal state (for debug only)";
pin out real s2 = 0.0 "2nd-delayed internal state (for debug only)";

option data Internal;
option extra_setup;
Expand Down Expand Up @@ -92,7 +92,7 @@ typedef enum {


typedef struct {
hal_bit_t lastEnable;
rtapi_bool lastEnable;
} Internal;


Expand All @@ -117,7 +117,7 @@ FUNCTION(_)

// If not direct coefficient loading.
if(type_ != TYPE_DIRECT){
valid = 0;
valid_set(0);

sampleRate = 1.0 / (period * 1e-9);

Expand Down Expand Up @@ -145,25 +145,26 @@ FUNCTION(_)
break;
}

n0 = b0 / a0;
n1 = b1 / a0;
n2 = b2 / a0;
d1 = a1 / a0;
d2 = a2 / a0;
s1 = s2 = 0.0;
n0_set(b0 / a0);
n1_set(b1 / a0);
n2_set(b2 / a0);
d1_set(a1 / a0);
d2_set(a2 / a0);
s1_set(s2_set(0.0));
}

valid = 1;
valid_set(1);
} while(0);
}

if(!enable || !valid){
out = in;
out_set(in);
}else{
// Transposed direct form II.
out = in * n0 + s1;
s1 = in * n1 - out * d1 + s2;
s2 = in * n2 - out * d2;
rtapi_real in_ = in;
out_set(in_ * n0 + s1);
s1_set( in_ * n1 - out * d1 + s2);
s2_set( in_ * n2 - out * d2);
}
}

Loading
Loading