Fixes for issues 68, 164, 167, 546, and 550

.gitignore

@@ -40,3 +40,8 @@ oprofile*
 position.txt
 *.9
 *.glade.h
+
+*.glade.h
+# expanded INI files
+*.ini.expanded
+build

README

@@ -46,3 +46,14 @@ to run the software go back to the top level directory, and issue:
 
   . scripts/rip-environment
   linuxcnc
+
+
+Unit Tests
+---------
+
+Building unit tests requires Meson (see https://mesonbuild.com/Getting-meson.html for details).
+
+To build and run the unit tests (from the repositor root directory):
+
+  meson build && cd build
+  ninja test

lib/hallib/core_sim9.hal

@@ -6,9 +6,6 @@ loadrt trivkins
 # motion controller, get name and thread periods from ini file
 loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
 # load 6 differentiators (for velocity and accel signals
-loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv
-# load additional blocks
-loadrt hypot names=vel_xy,vel_xyz
 
 # add motion controller functions to servo thread
 addf motion-command-handler servo-thread

lib/hallib/sim_spindle_encoder.hal

@@ -5,26 +5,39 @@ setp sim_spindle.scale 0.01666667
 loadrt limit2 names=limit_speed
 loadrt lowpass names=spindle_mass
 loadrt near names=near_speed
+loadrt quant names=sim_spindle_encoder,spindle_vel_est
 
-# this limit doesnt make any sense to me:
+# Bound spindle acceleration to something reasonable
 setp limit_speed.maxv 5000.0 # rpm/second
 
 # encoder reset control
 # hook up motion controller's sync output
 net spindle-index-enable motion.spindle-index-enable <=> sim_spindle.index-enable
 
+# Set resolution of spindle encoder in counts / revolution (Note: this value is 4 * LPR or PPR)
+setp sim_spindle_encoder.resolution 64.0
+
+# Simulated spindle encoder
+net spindle-pos-raw sim_spindle.position-fb => sim_spindle_encoder.in
+
 # report our revolution count to the motion controller
-net spindle-pos sim_spindle.position-fb => motion.spindle-revs
+net spindle-pos sim_spindle_encoder.out => motion.spindle-revs
 
 # simulate spindle mass
-setp spindle_mass.gain .07
+setp spindle_mass.gain .005
 
-# spindle speed control
+# Limit spindle speed by our maximum value
 net spindle-speed-cmd  motion.spindle-speed-out => limit_speed.in
-net spindle-speed-limited limit_speed.out => sim_spindle.velocity-cmd spindle_mass.in
 
-# for spindle velocity estimate
-net spindle-rpm-filtered spindle_mass.out motion.spindle-speed-in near_speed.in2
+# Simulate spindle mass with a low-pass filter on the velocity command
+net spindle-speed-limited limit_speed.out => spindle_mass.in
+
+# Feed the simulated spindle speed into the sim spindle to generate a position signal
+net spindle-rpm-filtered spindle_mass.out => sim_spindle.velocity-cmd near_speed.in2 spindle_vel_est.in
+
+# Approximate velocity quantization assuming 360 RPM, 30kHz base thread, 100 PPR encoder
+setp spindle_vel_est.resolution 8.0
+net spindle-rpm-est spindle_vel_est.out => motion.spindle-speed-in
 
 # at-speed detection
 setp near_speed.scale 1.1
@@ -37,3 +50,5 @@ addf limit_speed servo-thread
 addf spindle_mass servo-thread
 addf near_speed servo-thread
 addf sim_spindle servo-thread
+addf sim_spindle_encoder servo-thread
+addf spindle_vel_est servo-thread

lib/python/rs274/glcanon.py

@@ -49,6 +49,8 @@ def __init__(self, colors, geometry, is_foam=0):
         self.arcfeed = []; self.arcfeed_append = self.arcfeed.append
         # dwell list - [line number, color, pos x, pos y, pos z, plane]
         self.dwells = []; self.dwells_append = self.dwells.append
+        # spindle-synched feed list - (line number, (position deltas), S, fpr)
+        self.feed_synched = []
         self.choice = None
         self.feedrate = 1
         self.lo = (0,) * 9
@@ -162,6 +164,32 @@ def calc_extents(self):
                 self.min_extents_notool[0], self.min_extents_notool[1], min_z
             self.max_extents_notool = \
                 self.max_extents_notool[0], self.max_extents_notool[1], max_z
+    def calc_velocity(self, delta, axis_max_vel):
+        """Using techniques from getStraightVelocity() in emccanon.cc, given 9
+        axis deltas and velocity limits, calculate max velocity of a
+        straight move; deltas should be absolute; invalid axes should be 0
+        """
+        # Clean up tiny values
+        delta = tuple([(0.0 if i<1e-7 else i) for i in delta])
+        # Fastest time of coordinated move is the maximum time of any
+        # one axis to perform move at axis max velocity
+        tmax = max([(i[0]/i[1] if i[1] else 0.0)
+                     for i in zip(delta, axis_max_vel)])
+        # Total distance is the hypotenuse of a set of three axes;
+        # which set depends on the type of move
+        if sum(delta[0:3]) > 0:
+            # Linear XYZ with or without ABC or UVW
+            dtot = math.sqrt(sum(i*i for i in delta[0:3]))
+        elif sum(delta[6:9]) > 0:
+            # Linear UVW without XYZ and with or without ABC
+            dtot = math.sqrt(sum(i*i for i in delta[6:9]))
+        else:
+            # Angular-only
+            dtot = math.sqrt(sum(i*i for i in delta[3:6]))
+        # Max velocity = total distance / fastest time
+        max_vel = dtot/tmax
+        return max_vel
+
     def tool_offset(self, xo, yo, zo, ao, bo, co, uo, vo, wo):
         self.first_move = True
         x, y, z, a, b, c, u, v, w = self.lo
@@ -230,6 +258,18 @@ def straight_feed(self, x,y,z, a,b,c, u, v, w):
         self.lo = l
     straight_probe = straight_feed
 
+    def straight_feed_synched(self, lineno, x,y,z, a,b,c, u,v,w, s, fpr):
+        """For spindle-synched straight feeds, also collect data needed to
+        check if the commanded spindle rate and feed per revolution
+        will violate any axis MAX_VELOCITY constraints"""
+        if self.suppress > 0: return
+        # save segment start and record straight feed segment
+        lo = self.lo
+        self.straight_feed(x,y,z, a,b,c, u, v, w)
+        # record axis distances, spindle speed and feed per revolution
+        delta = tuple([abs(i[0]-i[1]) for i in zip(lo, self.lo)])
+        self.feed_synched.append((lineno, delta, s, fpr))
+
     def user_defined_function(self, i, p, q):
         if self.suppress > 0: return
         color = self.colors['m1xx']

meson.build

@@ -5,36 +5,60 @@ tp_src_dir = join_paths(src_root, 'emc/tp')
 
 inc_dir = include_directories([
   'unit_tests',
+  'unit_tests/mock',
   src_root,
+  join_paths(src_root, 'emc/kinematics'),
   tp_src_dir,
   'src/libnml/posemath',
   'src/emc/nml_intf',
   'src/emc/motion',
   'src/rtapi',
+  'src/hal',
   ])
 
 cc = meson.get_compiler('c')
 m_dep = cc.find_library('m', required : true)
 
 add_global_arguments(['-DULAPI','-DUNIT_TEST'], language : 'c')
 
-pm = static_library('posemath',
+# Define static libraries for easier linking with unit tests
+
+posemath_obj = static_library('posemath',
   'src/libnml/posemath/_posemath.c',
-  include_directories : inc_dir)
+  include_directories : inc_dir,
+  dependencies : [m_dep]
+  )
 
-# TODO implement subdir builds for tp, motion, etc
+emcpose_obj = static_library('emcpose',
+  join_paths(src_root,   'emc/nml_intf/emcpose.c'),
+  include_directories : inc_dir
+  )
 
-# KLUDGE just shove all the source files into the test build
-test_blendmath=executable('test_blendmath',
-  ['unit_tests/tp/test_blendmath.c',
+utlib_tp_deps = static_library('tp_deps',
+  [
+    join_paths(tp_src_dir, 'joint_util.c'), 
     join_paths(tp_src_dir, 'blendmath.c'), 
     join_paths(tp_src_dir, 'spherical_arc.c'), 
-    join_paths(src_root, 'emc/nml_intf/emcpose.c'), 
     join_paths(tp_src_dir, 'tc.c'), 
-    ],
+    join_paths(tp_src_dir, 'tcq.c'), 
+  ],
+  include_directories : inc_dir,
+  link_with: [emcpose_obj, posemath_obj])
+
+tp_test_files = [
+  'test_blendmath',
+  'test_joint_util',
+  'test_spherical_arc',
+  'test_posemath'
+  ]
+
+foreach n : tp_test_files
+
+test(n, executable(n,
+  join_paths('unit_tests/tp', n+'.c'),
   dependencies : [m_dep],
   include_directories : inc_dir,
-  link_with : pm
-  )
+  link_with : [ utlib_tp_deps]
+  ))
+endforeach
 
-test('blendmath test',test_blendmath)

src/Makefile

@@ -942,6 +942,8 @@ motmod-objs += emc/tp/tcq.o
 motmod-objs += emc/tp/tp.o
 motmod-objs += emc/tp/spherical_arc.o
 motmod-objs += emc/tp/blendmath.o
+motmod-objs += emc/tp/math_util.o
+motmod-objs += emc/tp/joint_util.o
 motmod-objs += emc/motion/motion.o
 motmod-objs += emc/motion/command.o
 motmod-objs += emc/motion/control.o

src/emc/ini/initraj.cc

@@ -193,6 +193,14 @@ static int loadTraj(EmcIniFile *trajInifile)
         old_inihal_data.traj_arc_blend_tangent_kink_ratio = arcBlendTangentKinkRatio;
         //TODO update inihal
 
+        int extraConsistencyChecks = 0;
+        double maxPositionDriftError = 0.0001;
+
+        trajInifile->Find(&extraConsistencyChecks, "EXTRA_CONSISTENCY_CHECKS", "TRAJ");
+        trajInifile->Find(&maxPositionDriftError, "MAX_POSITION_DRIFT_ERROR", "TRAJ");
+
+        emcSetupConsistencyChecks(extraConsistencyChecks, maxPositionDriftError);
+
         double maxFeedScale = 1.0;
         trajInifile->Find(&maxFeedScale, "MAX_FEED_OVERRIDE", "DISPLAY");
 

src/emc/motion-logger/motion-logger.c

@@ -571,13 +571,14 @@ int main(int argc, char* argv[]) {
                 break;
 
             case EMCMOT_SPINDLE_ON:
-                log_print("SPINDLE_ON speed=%f, css_factor=%f, xoffset=%f\n", c->vel, c->ini_maxvel, c->acc);
-                emcmotStatus->spindle.speed = c->vel;
+                log_print("SPINDLE_ON speed=%f, css_factor=%f, xoffset=%f\n",
+                          c->spindle_speed, c->css_factor, c->css_xoffset);
+                emcmotStatus->spindle_cmd.velocity_rpm_out = c->spindle_speed;
                 break;
 
             case EMCMOT_SPINDLE_OFF:
                 log_print("SPINDLE_OFF\n");
-                emcmotStatus->spindle.speed = 0;
+                emcmotStatus->spindle_cmd.velocity_rpm_out = 0;
                 break;
 
             case EMCMOT_SPINDLE_INCREASE:
@@ -625,6 +626,12 @@ int main(int argc, char* argv[]) {
                 log_print("SETUP_ARC_BLENDS\n");
                 break;
 
+            case EMCMOT_SETUP_CONSISTENCY_CHECKS:
+                log_print("SETUP_CONSISTENCY_CHECKS enabled=%d, position_drift=%f\n",
+                          c->consistencyCheckConfig.extraConsistencyChecks,
+                          c->consistencyCheckConfig.maxPositionDriftError);
+                break;
+
             default:
                 log_print("ERROR: unknown command %d\n", c->command);
                 break;