summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorRobert W. Ellenberg <rwe24g@gmail.com>2014-02-16 14:34:07 -0500
committerChris Radek <chris@timeguy.com>2014-06-11 14:44:08 -0500
commit1ff4abc9305718dacd1be1e9dd6becf93ed73a63 (patch)
treeba00b6a9bcecd21a1da34722cba40f9be093630c
parent9f2aa45b3a0ba7a75490ee464a0685159a5fbbfe (diff)
downloadlinuxcnc-1ff4abc9305718dacd1be1e9dd6becf93ed73a63.tar.gz
linuxcnc-1ff4abc9305718dacd1be1e9dd6becf93ed73a63.zip
Added validation for EmcPose to prevent NaN's from being assigned to trajectory planner output
Diffstat
-rw-r--r--src/emc/motion/control.c6
-rw-r--r--src/emc/motion/motion.c2
-rw-r--r--src/emc/nml_intf/emcpose.c23
-rw-r--r--src/emc/nml_intf/emcpose.h5
-rw-r--r--src/emc/tp/tp.c63
-rw-r--r--src/emc/tp/tp.h4
6 files changed, 88 insertions, 15 deletions
diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c
index 53d67fc1a..459934f81 100644
--- a/src/emc/motion/control.c
+++ b/src/emc/motion/control.c
@@ -826,7 +826,7 @@ static void set_operating_mode(void)
/* check for emcmotDebug->enabling */
if (emcmotDebug->enabling && !GET_MOTION_ENABLE_FLAG()) {
- tpSetPos(&emcmotDebug->tp, emcmotStatus->carte_pos_cmd);
+ tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd);
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
@@ -851,7 +851,7 @@ static void set_operating_mode(void)
if (GET_MOTION_INPOS_FLAG()) {
/* update coordinated emcmotDebug->tp position */
- tpSetPos(&emcmotDebug->tp, emcmotStatus->carte_pos_cmd);
+ tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd);
/* drain the cubics so they'll synch up */
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to joint data */
@@ -889,7 +889,7 @@ static void set_operating_mode(void)
if (emcmotDebug->coordinating && !GET_MOTION_COORD_FLAG()) {
if (GET_MOTION_INPOS_FLAG()) {
/* preset traj planner to current position */
- tpSetPos(&emcmotDebug->tp, emcmotStatus->carte_pos_cmd);
+ tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd);
/* drain the cubics so they'll synch up */
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to joint data */
diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c
index ec898aaaa..788464963 100644
--- a/src/emc/motion/motion.c
+++ b/src/emc/motion/motion.c
@@ -1045,7 +1045,7 @@ static int init_comm_buffers(void)
}
// tpInit(&emcmotDebug->tp); // tpInit called from tpCreate
tpSetCycleTime(&emcmotDebug->tp, emcmotConfig->trajCycleTime);
- tpSetPos(&emcmotDebug->tp, emcmotStatus->carte_pos_cmd);
+ tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd);
tpSetVmax(&emcmotDebug->tp, emcmotStatus->vel, emcmotStatus->vel);
tpSetAmax(&emcmotDebug->tp, emcmotStatus->acc);
diff --git a/src/emc/nml_intf/emcpose.c b/src/emc/nml_intf/emcpose.c
index 8baa04006..5ad096ed1 100644
--- a/src/emc/nml_intf/emcpose.c
+++ b/src/emc/nml_intf/emcpose.c
@@ -278,3 +278,26 @@ int emcPoseMagnitude(EmcPose const * const pose, double * const out) {
*out = mag;
return EMCPOSE_ERR_OK;
}
+
+
+/**
+ * Return true for a numerically valid pose, or false for an invalid pose (or null pointer).
+ */
+bool emcPoseValid(EmcPose const * const pose)
+{
+
+ if (!pose ||
+ isnan(pose->tran.x) ||
+ isnan(pose->tran.y) ||
+ isnan(pose->tran.z) ||
+ isnan(pose->a) ||
+ isnan(pose->b) ||
+ isnan(pose->c) ||
+ isnan(pose->u) ||
+ isnan(pose->v) ||
+ isnan(pose->w)) {
+ return false;
+ } else {
+ return true;
+ }
+}
diff --git a/src/emc/nml_intf/emcpose.h b/src/emc/nml_intf/emcpose.h
index 9c3e25029..86cddbcd9 100644
--- a/src/emc/nml_intf/emcpose.h
+++ b/src/emc/nml_intf/emcpose.h
@@ -14,6 +14,9 @@
#define EMCPOSE_H
#include "emcpos.h"
+#include <stdbool.h>
+#include "rtapi_math.h"
+
typedef enum {
EMCPOSE_ERR_OK = 0,
@@ -46,4 +49,6 @@ int emcPoseGetUVW(EmcPose const * const pose, PmCartesian * const uvw);
int emcPoseMagnitude(EmcPose const * const pose, double * const out);
+bool emcPoseValid(EmcPose const * const pose);
+
#endif
diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c
index f4834d222..5e5700300 100644
--- a/src/emc/tp/tp.c
+++ b/src/emc/tp/tp.c
@@ -536,18 +536,59 @@ int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance)
* It sets the current position AND the goal position to be the same. Used
* only at TP initialization and when switching modes.
*/
-int tpSetPos(TP_STRUCT * const tp, EmcPose pos)
+int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos)
{
if (0 == tp) {
return TP_ERR_FAIL;
}
- tp->currentPos = pos;
- tp->goalPos = pos;
+ int res_invalid = tpSetCurrentPos(tp, pos);
+ if (res_invalid) {
+ return TP_ERR_FAIL;
+ }
+ tp->goalPos = *pos;
return TP_ERR_OK;
}
+
+/**
+ * Set current position.
+ * It sets the current position AND the goal position to be the same. Used
+ * only at TP initialization and when switching modes.
+ */
+int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos)
+{
+ if (0 == tp) {
+ return TP_ERR_FAIL;
+ }
+
+ if (emcPoseValid(pos)) {
+ tp->currentPos = *pos;
+ return TP_ERR_OK;
+ } else {
+ rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpSetCurrentPos!\n");
+ return TP_ERR_FAIL;
+ }
+}
+
+
+int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp)
+{
+ if (0 == tp) {
+ return TP_ERR_FAIL;
+ }
+
+ if (emcPoseValid(disp)) {
+ emcPoseSelfAdd(&tp->currentPos, disp);
+ return TP_ERR_OK;
+ } else {
+ rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpSetCurrentPos!\n");
+ return TP_ERR_FAIL;
+ }
+}
+
+
/**
* Check for valid tp before queueing additional moves.
*/
@@ -766,6 +807,7 @@ STATIC int tpDebugTangentInfo(TC_STRUCT const * const prev_tc, TC_STRUCT const *
pmCartCartCross(&geom->binormal, &blend_tc->coords.arc.xyz.rEnd, &u_arc_end);
pmCartUnitEq(&u_arc_end);
+ //TODO fail if theta is too large
double dot;
pmCartCartDot(&u_circ1_end, &u_arc_start, &dot);
double theta1 = acos(dot);
@@ -2717,19 +2759,20 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
EmcPose displacement;
- //Calculate displacement
+ // Calculate displacement
tcGetPos(tc, &displacement);
emcPoseSelfSub(&displacement, &before);
- emcPoseSelfAdd(&tp->currentPos, &displacement);
+ //Store displacement (checking for valid pose)
+ int res_set = tpAddCurrentPos(tp, &displacement);
#ifdef TC_DEBUG
double mag;
emcPoseMagnitude(&displacement, &mag);
- tc_debug_print("cycle movement = %f\n",mag);
+ tc_debug_print("cycle movement = %f\n", mag);
#endif
- return TP_ERR_OK;
+ return res_set;
}
@@ -2895,8 +2938,8 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc,
tcGetPos(tc, &displacement);
emcPoseSelfSub(&displacement, &before);
- //Update tp's position
- emcPoseSelfAdd(&tp->currentPos, &displacement);
+ // Update tp's position (checking for valid pose)
+ tpAddCurrentPos(tp, &displacement);
#ifdef TC_DEBUG
double mag;
@@ -3073,7 +3116,7 @@ int tpRunCycle(TP_STRUCT * const tp, long period)
#ifdef TC_DEBUG
double mag;
EmcPose disp;
- emcPoseSub(&tp->currentPos,&pos_before, &disp);
+ emcPoseSub(&tp->currentPos, &pos_before, &disp);
emcPoseMagnitude(&disp, &mag);
tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n",
time_elapsed,
diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h
index 020733b94..073beee9e 100644
--- a/src/emc/tp/tp.h
+++ b/src/emc/tp/tp.h
@@ -142,7 +142,9 @@ int tpSetAmax(TP_STRUCT * tp, double amax);
int tpSetId(TP_STRUCT * tp, int id);
int tpGetExecId(TP_STRUCT * tp);
int tpSetTermCond(TP_STRUCT * tp, int cond, double tolerance);
-int tpSetPos(TP_STRUCT * tp, EmcPose pos);
+int tpSetPos(TP_STRUCT * tp, EmcPose const * const pos);
+int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp);
+int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos);
int tpAddRigidTap(TP_STRUCT * tp, EmcPose end, double vel, double
ini_maxvel, double acc, unsigned char enables);
int tpAddLine(TP_STRUCT * tp, EmcPose end, int type, double vel, double
generated by cgit v1.2.3 (git 2.39.1) at 2025-05-01 02:28:57 +0000