From 708f86b93cfd69bb5a1afa70a937266316aefabd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 18:39:16 +1000 Subject: [PATCH 1/3] switchkins: reject a bad kinematics type before switching to it kinematicsSwitch() stored the requested type in switchkins_type and only then ran the switch statement that validates it, so an out of range request left the module pointing at a kinematics that does not exist. The switch itself returned -1 and motion raised its error flag, but switchkins_type kept the bad value, so every kinematicsForward() and kinematicsInverse() call after that failed too and printed switchkins: Forward BAD switchkins_type once per servo cycle for as long as the machine stayed up. Validate the request first and return without touching switchkins_type, which leaves the running kinematics in place. With the range checked up front the default arm of the switch is unreachable, so it goes away. --- src/emc/kinematics/switchkins.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 19a7ac23bbd..d0d7065b892 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -128,6 +128,15 @@ int kinematicsSwitchable() {return 1;} int kinematicsSwitch(int new_switchkins_type) { int k; + + // reject first, so a bad request leaves the running kinematics alone + if (new_switchkins_type < 0 || new_switchkins_type >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinematicsSwitch:BAD VALUE <%d>\n", + new_switchkins_type); + return -1; // FAIL + } + for (k=0; k< SWITCHKINS_MAX_TYPES; k++) { use_lastpose[k] = 0;} switchkins_type = new_switchkins_type; @@ -150,13 +159,6 @@ int kinematicsSwitch(int new_switchkins_type) hal_set_bool(swdata->kinstype_is_1, 0); hal_set_bool(swdata->kinstype_is_2, 1); break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(swdata->kinstype_is_1, 0); - hal_set_bool(swdata->kinstype_is_0, 0); - hal_set_bool(swdata->kinstype_is_2, 0); - return -1; // FAIL } if (fwd_iterates[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types From a39d47891e9064f962213b597146cf1c282d0938 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 18:39:17 +1000 Subject: [PATCH 2/3] switchkins: allow more than three kinematics types switchkins.c dispatched on switchkins_type with a three way switch and created a fixed kinstype.is-0/1/2, so a module could never provide more than three kinematics. Hold the setup, forward and inverse functions in arrays and dispatch by index. switchkinsSetup() still provides types 0,1,2 exactly as before, so no kinematics module changes and out of tree modules keep compiling. A module wanting more calls the new switchkinsRegister() from within switchkinsSetup(), once per additional type. The kinstype.is-N pins are created in a loop, which leaves the names of the first three unchanged. --- docs/src/motion/switchkins.adoc | 31 +++++-- src/emc/kinematics/switchkins.c | 149 ++++++++++++++++---------------- src/emc/kinematics/switchkins.h | 8 +- 3 files changed, 104 insertions(+), 84 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index bf4156f8274..7f5a95bf283 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -128,6 +128,9 @@ program behavior in accordance with the active kinematics type. . *kinstype.is-1* Output (bit) . *kinstype.is-2* Output (bit) +A module providing more than three kinematics types has one +'kinstype.is-N' pin per type. + == Usage === HAL Connections @@ -388,13 +391,29 @@ routines and the functions for forward an inverse calculation for each kinstype (0,1,2) and sets a number of configuration settings. +A module needing more than three kinstypes calls +switchkinsRegister() from within switchkinsSetup() for each +additional one: + +---- +int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); +---- + +'ktype' runs from 3 to SWITCHKINS_MAX_TYPES-1 (defined in +switchkins.h) and every kinstype below the highest one registered +must be provided. Each additional kinstype gets its own +'kinstype.is-N' pin, so 'kinstype.is-0', 'kinstype.is-1' and +'kinstype.is-2' keep the names they always had. + After calling switchkinsSetup(), rtapi_app_main() checks the supplied parameters, creates a HAL component, and then invokes -the setup routine identified for each kinstype (0,1,2). - -Each kinstype (0,1,2) setup routine can (optionally) create HAL -pins and set them to default values. When all setup routines -finish, rtapi_app_main() issues hal_ready() for the component -to complete creation of the module. +the setup routine identified for each kinstype. + +Each kinstype setup routine can (optionally) create HAL +pins and set them to default values. A setup routine is called +once per kinstype it is registered for, so a routine used for two +kinstypes must not create the same pin twice. When all setup +routines finish, rtapi_app_main() issues hal_ready() for the +component to complete creation of the module. // vim: set syntax=asciidoc: diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index d0d7065b892..971fe198fa2 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -38,19 +38,17 @@ // kinematic functions (default=0 for err detection): static kparms kp; // kinematics parms (common all types) -static KF kfwd0 = NULL; // 0==switchkins_type kinematics forward -static KF kfwd1 = NULL; // 1 -static KF kfwd2 = NULL; // 2 +// indexed by switchkins_type (NULL==not provided, for err detection): +static KS ksetups[SWITCHKINS_MAX_TYPES] = {NULL}; +static KF kfwds[SWITCHKINS_MAX_TYPES] = {NULL}; +static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL}; -static KI kinv0 = NULL; // 0==switchkins_type kinematics inverse -static KI kinv1 = NULL; // 1 -static KI kinv2 = NULL; // 2 +// types provided: 3 from switchkinsSetup(), more from switchkinsRegister() +static int kins_count = 3; static int switchkins_type; static struct swdata { - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; + hal_bool_t kinstype_is[SWITCHKINS_MAX_TYPES]; hal_real_t gui_x; hal_real_t gui_y; @@ -104,15 +102,16 @@ static int gui_forward_kins(const double *joints) int res; KINEMATICS_FORWARD_FLAGS fflags = 0; KINEMATICS_INVERSE_FLAGS iflags; - switch (kp.gui_kinstype) { - case 0: res = kfwd0(joints, &lastpose[0], &fflags, &iflags);break; - case 1: res = kfwd1(joints, &lastpose[1], &fflags, &iflags);break; - case 2: res = kfwd2(joints, &lastpose[2], &fflags, &iflags);break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "gui_forward_kins BAD gui_kinstype <%d>\n", - kp.gui_kinstype); - return -1; - } + if ( kp.gui_kinstype < 0 + || kp.gui_kinstype >= kins_count + || !kfwds[kp.gui_kinstype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "gui_forward_kins BAD gui_kinstype <%d>\n", + kp.gui_kinstype); + return -1; + } + res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], + &fflags, &iflags); hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x); hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y); hal_set_real(swdata->gui_z, lastpose[kp.gui_kinstype].tran.z); @@ -130,7 +129,7 @@ int kinematicsSwitch(int new_switchkins_type) int k; // reject first, so a bad request leaves the running kinematics alone - if (new_switchkins_type < 0 || new_switchkins_type >= SWITCHKINS_MAX_TYPES) { + if (new_switchkins_type < 0 || new_switchkins_type >= kins_count) { rtapi_print_msg(RTAPI_MSG_ERR, "kinematicsSwitch:BAD VALUE <%d>\n", new_switchkins_type); @@ -140,26 +139,13 @@ int kinematicsSwitch(int new_switchkins_type) for (k=0; k< SWITCHKINS_MAX_TYPES; k++) { use_lastpose[k] = 0;} switchkins_type = new_switchkins_type; - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(swdata->kinstype_is_0, 1); - hal_set_bool(swdata->kinstype_is_1, 0); - hal_set_bool(swdata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(swdata->kinstype_is_0, 0); - hal_set_bool(swdata->kinstype_is_1, 1); - hal_set_bool(swdata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE2\n"); - hal_set_bool(swdata->kinstype_is_0, 0); - hal_set_bool(swdata->kinstype_is_1, 0); - hal_set_bool(swdata->kinstype_is_2, 1); - break; + + rtapi_print_msg(RTAPI_MSG_INFO, + "kinematicsSwitch:TYPE%d\n", switchkins_type); + for (k=0; k < kins_count; k++) { + hal_set_bool(swdata->kinstype_is[k], k == switchkins_type); } + if (fwd_iterates[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } @@ -179,15 +165,15 @@ int kinematicsForward(const double *joint, use_lastpose[switchkins_type] = 0; } - switch (switchkins_type) { - case 0: r = kfwd0(joint, pos, fflags, iflags); break; - case 1: r = kfwd1(joint, pos, fflags, iflags); break; - case 2: r = kfwd2(joint, pos, fflags, iflags); break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "switchkins: Forward BAD switchkins_type \n", - switchkins_type); - return -1; + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kfwds[switchkins_type]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: Forward BAD switchkins_type \n", + switchkins_type); + return -1; } + r = kfwds[switchkins_type](joint, pos, fflags, iflags); if (fwd_iterates[switchkins_type]) {save_lastpose(switchkins_type,pos);} if (r) return r; @@ -213,15 +199,15 @@ int kinematicsInverse(const EmcPose * pos, { int r; - switch (switchkins_type) { - case 0: r = kinv0(pos, joint, iflags, fflags); break; - case 1: r = kinv1(pos, joint, iflags, fflags); break; - case 2: r = kinv2(pos, joint, iflags, fflags); break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "switchkins: Inverse BAD switchkins_type \n", - switchkins_type); - return -1; + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kinvs[switchkins_type]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: Inverse BAD switchkins_type \n", + switchkins_type); + return -1; } + r = kinvs[switchkins_type](pos, joint, iflags, fflags); return r; } // kinematicsInverse() @@ -230,6 +216,22 @@ KINEMATICS_TYPE kinematicsType() return KINEMATICS_BOTH; } +int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) +{ + if (ktype < 3 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegister: BAD switchkins_type <%d>" + " (must be 3..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + return -1; + } + ksetups[ktype] = kset; + kfwds[ktype] = kfwd; + kinvs[ktype] = kinv; + if (ktype >= kins_count) { kins_count = ktype + 1; } + return 0; +} // switchkinsRegister() + //********************************************************************* static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); @@ -241,6 +243,7 @@ EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(switchkinsRegister); MODULE_LICENSE("GPL"); static int comp_id; @@ -261,14 +264,11 @@ int rtapi_app_main(void) kp.sparm = sparm; // module parm passed to kins - KS ksetup0 = NULL; - KS ksetup1 = NULL; - KS ksetup2 = NULL; - + // may call switchkinsRegister() for types above 2 res = switchkinsSetup(&kp, - &ksetup0, &ksetup1, &ksetup2, - &kfwd0, &kfwd1, &kfwd2, - &kinv0, &kinv1, &kinv2); + &ksetups[0], &ksetups[1], &ksetups[2], + &kfwds[0], &kfwds[1], &kfwds[2], + &kinvs[0], &kinvs[1], &kinvs[2]); if (res) {emsg="switchkinsSetp FAIL"; goto error;} for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { @@ -288,18 +288,14 @@ int rtapi_app_main(void) if (kp.max_joints <= 0 || kp.max_joints > EMCMOT_MAX_JOINTS) { emsg = "bogus max_joints"; goto error; } - if (kp.gui_kinstype >= SWITCHKINS_MAX_TYPES) { + if (kp.gui_kinstype >= kins_count) { emsg = "bogus gui_kinstype"; goto error; } - if (!ksetup0 || !ksetup1 || !ksetup2) { - emsg = "Missing setup function"; goto error; - } - if (!kfwd0 || !kfwd1 || !kfwd2) { - emsg = "Missing fwd functionn"; goto error; - } - if (!kinv0 || !kinv1 || !kinv2) { - emsg = "Missing inv function"; goto error; + for (i=0; i < kins_count; i++) { + if (!ksetups[i]) { emsg = "Missing setup function"; goto error; } + if (!kfwds[i]) { emsg = "Missing fwd function"; goto error; } + if (!kinvs[i]) { emsg = "Missing inv function"; goto error; } } comp_id = hal_init(kp.kinsname); @@ -308,9 +304,10 @@ int rtapi_app_main(void) swdata = hal_malloc(sizeof(struct swdata)); if (!swdata) goto error; - res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is_0), 0, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is_1), 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is_2), 0, "kinstype.is-2"); + for (i=0; i < kins_count; i++) { + res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), + 0, "kinstype.is-%d", i); + } if (kp.gui_kinstype >=0) { res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_x, 0.0, "skgui.x"); @@ -327,9 +324,9 @@ int rtapi_app_main(void) if (!coordinates) {coordinates = kp.required_coordinates;} - ksetup0(comp_id,coordinates,&kp); - ksetup1(comp_id,coordinates,&kp); - ksetup2(comp_id,coordinates,&kp); + for (i=0; i < kins_count; i++) { + ksetups[i](comp_id,coordinates,&kp); + } hal_ready(comp_id); return 0; diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 1cad41bd691..0030138a03d 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -6,8 +6,8 @@ #include -//hardcoded number of switchkins types (KS,KF,KI): -#define SWITCHKINS_MAX_TYPES 3 +//max number of switchkins types (KS,KF,KI) a module may provide: +#define SWITCHKINS_MAX_TYPES 9 // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -28,9 +28,13 @@ typedef int (*KS)(const int comp_id, // halpins ); //********************************************************************* +// supplied by the using module, provides types 0,1,2 extern int switchkinsSetup(kparms* ksetup_parms, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, KI* kinv0, KI* kinv1, KI* kinv2 ); + +// called from switchkinsSetup() for each type above 2 +extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); #endif // } From 8db6fbfecce557d0dfb680bbf02d997bdae2edd7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 20:26:02 +1000 Subject: [PATCH 3/3] switchkins: let switchkinsRegister() provide any kinematics type Registration was restricted to types 3 and up, which left types 0,1,2 arriving one way and the rest another. Allow any type from 0, so registration is the general mechanism and the switchkinsSetup() arguments are a shorthand for the first three. A type has to come from one route or the other. Registering one that switchkinsSetup() already filled in is refused, and a rejected registration now fails the module load instead of only printing, since no caller checks the return value. The count of provided types is taken from the highest one filled by either route rather than assumed to be three, so a module can register all of them. A type left out below that is a gap, and the load time message now names the type and which of the three functions is missing instead of saying only "Missing setup function". --- docs/src/motion/switchkins.adoc | 20 ++++++++++------- src/emc/kinematics/switchkins.c | 39 +++++++++++++++++++++++++-------- src/emc/kinematics/switchkins.h | 2 +- 3 files changed, 43 insertions(+), 18 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 7f5a95bf283..3d81a1ffc2b 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -391,19 +391,23 @@ routines and the functions for forward an inverse calculation for each kinstype (0,1,2) and sets a number of configuration settings. -A module needing more than three kinstypes calls -switchkinsRegister() from within switchkinsSetup() for each -additional one: +A module can provide further kinstypes by calling +switchkinsRegister() from within switchkinsSetup(), once per +kinstype: ---- int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- -'ktype' runs from 3 to SWITCHKINS_MAX_TYPES-1 (defined in -switchkins.h) and every kinstype below the highest one registered -must be provided. Each additional kinstype gets its own -'kinstype.is-N' pin, so 'kinstype.is-0', 'kinstype.is-1' and -'kinstype.is-2' keep the names they always had. +'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in +switchkins.h). A kinstype has to come from one route or the +other, so registering one that switchkinsSetup() has already +filled in is an error, and so is leaving a gap below the highest +kinstype provided. Either mistake fails the module load and says +which kinstype is at fault. + +Each kinstype gets its own 'kinstype.is-N' pin, so a module +providing the usual three keeps the pin names it always had. After calling switchkinsSetup(), rtapi_app_main() checks the supplied parameters, creates a HAL component, and then invokes diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 971fe198fa2..f1393867e35 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -43,8 +43,9 @@ static KS ksetups[SWITCHKINS_MAX_TYPES] = {NULL}; static KF kfwds[SWITCHKINS_MAX_TYPES] = {NULL}; static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL}; -// types provided: 3 from switchkinsSetup(), more from switchkinsRegister() -static int kins_count = 3; +// types provided, counted in rtapi_app_main() once they are all in +static int kins_count; +static int register_error; static int switchkins_type; static struct swdata { @@ -218,17 +219,24 @@ KINEMATICS_TYPE kinematicsType() int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) { - if (ktype < 3 || ktype >= SWITCHKINS_MAX_TYPES) { + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkinsRegister: BAD switchkins_type <%d>" - " (must be 3..%d)\n", + " (must be 0..%d)\n", ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegister: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; return -1; } ksetups[ktype] = kset; kfwds[ktype] = kfwd; kinvs[ktype] = kinv; - if (ktype >= kins_count) { kins_count = ktype + 1; } return 0; } // switchkinsRegister() @@ -264,12 +272,19 @@ int rtapi_app_main(void) kp.sparm = sparm; // module parm passed to kins - // may call switchkinsRegister() for types above 2 + // may also call switchkinsRegister() res = switchkinsSetup(&kp, &ksetups[0], &ksetups[1], &ksetups[2], &kfwds[0], &kfwds[1], &kfwds[2], &kinvs[0], &kinvs[1], &kinvs[2]); if (res) {emsg="switchkinsSetp FAIL"; goto error;} + if (register_error) {emsg="switchkinsRegister FAIL"; goto error;} + + // the highest type provided by either route sets the count + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } + } + if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (kp.fwd_iterates_mask & (1<