This improves the hm2 stepgen - less position& velocity hunting.

Also makes hm2's stepgen.position-fb have sub-step resolution (like the sw stepgen does).
author: Sebastian Kuzminsky <seb@highlab.com> 2009-05-22 17:48:43 +0000
committer: Sebastian Kuzminsky <seb@highlab.com> 2009-05-22 17:48:43 +0000
commit: d68a34e48b15dcf0440c9d9b1e9881fd35d73c9f (patch)
tree: 8784eb8e50d577624635e9fc020f44980290b24c
parent: 0b2df83b026b81f7144a3008d1ff47d7e44805fa (diff)
download: linuxcnc-d68a34e48b15dcf0440c9d9b1e9881fd35d73c9f.tar.gz
linuxcnc-d68a34e48b15dcf0440c9d9b1e9881fd35d73c9f.zip
4 files changed, 120 insertions, 31 deletions
diff --git a/docs/man/man9/hostmot2.9 b/docs/man/man9/hostmot2.9
index 60c07ae7f..656b8224f 100644
--- a/docs/man/man9/hostmot2.9
+++ b/docs/man/man9/hostmot2.9
@@ -310,8 +310,9 @@ position units.
 
 (s32 output) counts: Feedback position in counts (number of steps).
 
-(float output) position-fb: Feedback position in arbitrary position units
-(counts / position_scale).
+(float output) position-fb: Feedback position in arbitrary position
+units.  This is similar to "counts/position_scale", but has finer than
+step resolution.
 
 (float output) velocity-fb: Feedback velocity in arbitrary position
 units per second.
diff --git a/src/hal/drivers/mesa-hostmot2/TODO b/src/hal/drivers/mesa-hostmot2/TODO
index dffb155eb..e116443cb 100644
--- a/src/hal/drivers/mesa-hostmot2/TODO
+++ b/src/hal/drivers/mesa-hostmot2/TODO
@@ -2,11 +2,22 @@
 
 next steps:
 
+    hm2_pci calls pci_register_driver,
+    and should fail if no boards are found:
+    <http://people.nl.linux.org/ftp/pub/anoncvs/kernelnewbies/documents/kdoc/kernel-api/r8314.html>
+
+
     stepgen:
 
+        fix micges' problem: the new stepgen algorithm can reverse
+        direction at the end of a move (because it gets ahead of cmd?)
+
         timing fixes
 
 
+    test encoders with +/- scale
+
+
     the config modparam is confusing and poorly documented
 
         > Note: Double quotes in hm2_7i43 command took me some time to figure out,
@@ -83,6 +94,10 @@ SPI:
     <pcw> ( AD5754 quad 16 bit +-10V out DAC)
     <pcw> ( AD7329 8 channel 1 M/S 12 bit + sign A-D)
 
+    2009-04-27 20:30:58 <seb_kuzminsky> what SPI hardware are you plugging into your 7i43?
+    2009-04-27 20:31:27 <Jon_geo01005> just a Max 6675
+    2009-04-27 20:31:41 <Jon_geo01005> thermocouple to digital converter
+
 
 
 
@@ -165,6 +180,8 @@ stepgen:
         to ClockLow periods (rounded up); the result decremented (zero
         ok); and the result of *that* sent to the pulsewait FPGA register.
 
+    make maxaccel=0 work right
+
     needs a cleanup() function
 
     setting .scale to 1600 (1600 steps/rev) and then commanding position
@@ -176,7 +193,7 @@ stepgen:
     it resumes at its previous velocity.  Is that bad?  Does this still
     happen with the new stepping logic?
 
-    test different steps-pre-rev on different stepgen instances
+    test different steps-per-rev on different stepgen instances
 
     support table output mode
 
diff --git a/src/hal/drivers/mesa-hostmot2/hostmot2.h b/src/hal/drivers/mesa-hostmot2/hostmot2.h
index c448557d6..b87579f91 100644
--- a/src/hal/drivers/mesa-hostmot2/hostmot2.h
+++ b/src/hal/drivers/mesa-hostmot2/hostmot2.h
@@ -22,6 +22,12 @@
 #endif
 
 
+// please God where do these live in real life?
+#define INT32_MIN (-2147483647-1)
+#define INT32_MAX (2147483647)
+#define UINT32_MAX (4294967295U)
+
+
 #include "rtapi.h"
 #include "hal.h"
 
@@ -423,6 +429,14 @@ typedef struct {
             hal_float_t *position_fb;
             hal_float_t *velocity_fb;
             hal_bit_t *enable;
+
+            // debug pins
+            hal_float_t *dbg_ff_vel;
+            hal_float_t *dbg_vel_error;
+            hal_float_t *dbg_pos_error;
+            hal_float_t *dbg_s_to_match;
+            hal_float_t *dbg_err_at_match;
+            hal_s32_t *dbg_step_rate;
         } pin;
 
         struct {
@@ -444,13 +458,12 @@ typedef struct {
     // computing the feedforward velocity
     hal_float_t old_position_cmd;
 
-    // HM2 tracks stepper position with 32 bits of sub-step
-    // precision.  This holds the top 16 of those bits, in the
-    // bottom 16 bits of the u32.
-    u32 counts_fractional;
-
     u32 prev_accumulator;
 
+    // this is a 48.16 signed fixed-point representation of the current
+    // stepgen position (16 bits of sub-step resolution)
+    s64 subcounts;
+
     u32 written_steplen;
     u32 written_stepspace;
     u32 written_dirsetup;
diff --git a/src/hal/drivers/mesa-hostmot2/stepgen.c b/src/hal/drivers/mesa-hostmot2/stepgen.c
index 64615957c..afa60847e 100644
--- a/src/hal/drivers/mesa-hostmot2/stepgen.c
+++ b/src/hal/drivers/mesa-hostmot2/stepgen.c
@@ -42,7 +42,7 @@ void hm2_stepgen_process_tram_read(hostmot2_t *hm2, long l_period_ns) {
     int i;
     for (i = 0; i < hm2->stepgen.num_instances; i ++) {
         u32 acc = hm2->stepgen.accumulator_reg[i];
-        s32 counts_delta;
+        s64 acc_delta;
 
         // those tricky users are always trying to get us to divide by zero
         if (fabs(hm2->stepgen.instance[i].hal.param.position_scale) < 1e-6) {
@@ -55,18 +55,22 @@ void hm2_stepgen_process_tram_read(hostmot2_t *hm2, long l_period_ns) {
             }
         }
 
-        // the HM2 Accumulator is a 16.16 bit fixed-point representation of the stepper position
-        // the fractional part gives accurate velocity at low speeds
-        // FIXME: should it be used to provide sub-step position feedback too?  Or is integral-step-only position feedback more accurate?
-        counts_delta = (acc >> 16) - (hm2->stepgen.instance[i].prev_accumulator >> 16);
-        if (counts_delta > 32768) {
-            counts_delta -= 65536;
-        } else if (counts_delta < -32768) {
-            counts_delta += 65536;
+        // The HM2 Accumulator Register is a 16.16 bit fixed-point
+        // representation of the current stepper position.
+        // The fractional part gives accurate velocity at low speeds, and
+        // sub-step position feedback (like sw stepgen).
+        acc_delta = acc - hm2->stepgen.instance[i].prev_accumulator;
+        if (acc_delta > INT32_MAX) {
+            acc_delta -= UINT32_MAX;
+        } else if (acc_delta < INT32_MIN) {
+            acc_delta += UINT32_MAX;
         }
-        *(hm2->stepgen.instance[i].hal.pin.counts) += counts_delta;
-        hm2->stepgen.instance[i].counts_fractional = acc & 0xFFFF;
-        *(hm2->stepgen.instance[i].hal.pin.position_fb) = (double)(*hm2->stepgen.instance[i].hal.pin.counts) / hm2->stepgen.instance[i].hal.param.position_scale;
+
+        hm2->stepgen.instance[i].subcounts += acc_delta;
+
+        *(hm2->stepgen.instance[i].hal.pin.counts) = hm2->stepgen.instance[i].subcounts >> 16;
+
+        *(hm2->stepgen.instance[i].hal.pin.position_fb) = ((hal_float_t)hm2->stepgen.instance[i].subcounts / 65536.0) / hm2->stepgen.instance[i].hal.param.position_scale;
 
         hm2->stepgen.instance[i].prev_accumulator = acc;
     }
@@ -128,22 +132,26 @@ static void hm2_stepgen_instance_prepare_tram_write(hostmot2_t *hm2, long l_peri
 
     velocity_error = *s->hal.pin.velocity_fb - ff_vel;
 
-    // do we need to speed up or slow down?
-    if (s->hal.param.maxaccel > 0) {
-        // user has specified a maxaccel (in units/second)
-        if (velocity_error > 0) {
+    // Do we need to change speed to match speed of position-cmd?
+    // If maxaccel is 0, there's no accel limit: fix this velocity error
+    // by the next servo period!  This leaves acceleration control up to
+    // the trajectory planner.
+    // If maxaccel is not zero, the user has specified a maxaccel and we
+    // adhere to that.
+    if (velocity_error > 0.0) {
+        if (s->hal.param.maxaccel == 0) {
+            match_accel = velocity_error / f_period_s;
+        } else {
             match_accel = -s->hal.param.maxaccel;
+        }
+    } else if (velocity_error < 0.0) {
+        if (s->hal.param.maxaccel == 0) {
+            match_accel = -velocity_error / f_period_s;
         } else {
             match_accel = s->hal.param.maxaccel;
         }
     } else {
-        // maxaccel is 0, no limit, fix this velocity error by the next servo period!
-        // this leaves acceleration control up to the trajectory planner
-        if (velocity_error > 0) {
-            match_accel = velocity_error / f_period_s;
-        } else {
-            match_accel = -velocity_error / f_period_s;
-        }
+        match_accel = 0;
     }
 
     if (match_accel == 0) {
@@ -163,6 +171,11 @@ static void hm2_stepgen_instance_prepare_tram_write(hostmot2_t *hm2, long l_peri
     position_cmd_at_match = *s->hal.pin.position_cmd + (ff_vel * seconds_to_vel_match);
     error_at_match = position_at_match - position_cmd_at_match;
 
+    *s->hal.pin.dbg_ff_vel = ff_vel;
+    *s->hal.pin.dbg_vel_error = velocity_error;
+    *s->hal.pin.dbg_err_at_match = error_at_match;
+    *s->hal.pin.dbg_s_to_match = seconds_to_vel_match;
+
     if (seconds_to_vel_match < f_period_s) {
         // we can match velocity in one period
 
@@ -220,6 +233,7 @@ static void hm2_stepgen_instance_prepare_tram_write(hostmot2_t *hm2, long l_peri
 
     steps_per_sec_cmd = velocity_cmd * s->hal.param.position_scale;
     hm2->stepgen.step_rate_reg[i] = steps_per_sec_cmd * (4294967296.0 / (hal_float_t)hm2->stepgen.clock_frequency);
+    *s->hal.pin.dbg_step_rate = hm2->stepgen.step_rate_reg[i];
 }
 
 
@@ -627,6 +641,48 @@ int hm2_stepgen_parse_md(hostmot2_t *hm2, int md_index) {
                 goto fail5;
             }
 
+            // debug pins
+
+            rtapi_snprintf(name, HAL_NAME_LEN, "%s.stepgen.%02d.dbg_ff_vel", hm2->llio->name, i);
+            r = hal_pin_float_new(name, HAL_OUT, &(hm2->stepgen.instance[i].hal.pin.dbg_ff_vel), hm2->llio->comp_id);
+            if (r != HAL_SUCCESS) {
+                HM2_ERR("error adding pin '%s', aborting\n", name);
+                r = -ENOMEM;
+                goto fail5;
+            }
+
+            rtapi_snprintf(name, HAL_NAME_LEN, "%s.stepgen.%02d.dbg_s_to_match", hm2->llio->name, i);
+            r = hal_pin_float_new(name, HAL_OUT, &(hm2->stepgen.instance[i].hal.pin.dbg_s_to_match), hm2->llio->comp_id);
+            if (r != HAL_SUCCESS) {
+                HM2_ERR("error adding pin '%s', aborting\n", name);
+                r = -ENOMEM;
+                goto fail5;
+            }
+
+            rtapi_snprintf(name, HAL_NAME_LEN, "%s.stepgen.%02d.dbg_vel_error", hm2->llio->name, i);
+            r = hal_pin_float_new(name, HAL_OUT, &(hm2->stepgen.instance[i].hal.pin.dbg_vel_error), hm2->llio->comp_id);
+            if (r != HAL_SUCCESS) {
+                HM2_ERR("error adding pin '%s', aborting\n", name);
+                r = -ENOMEM;
+                goto fail5;
+            }
+
+            rtapi_snprintf(name, HAL_NAME_LEN, "%s.stepgen.%02d.dbg_err_at_match", hm2->llio->name, i);
+            r = hal_pin_float_new(name, HAL_OUT, &(hm2->stepgen.instance[i].hal.pin.dbg_err_at_match), hm2->llio->comp_id);
+            if (r != HAL_SUCCESS) {
+                HM2_ERR("error adding pin '%s', aborting\n", name);
+                r = -ENOMEM;
+                goto fail5;
+            }
+
+            rtapi_snprintf(name, HAL_NAME_LEN, "%s.stepgen.%02d.dbg_step_rate", hm2->llio->name, i);
+            r = hal_pin_s32_new(name, HAL_OUT, &(hm2->stepgen.instance[i].hal.pin.dbg_step_rate), hm2->llio->comp_id);
+            if (r != HAL_SUCCESS) {
+                HM2_ERR("error adding pin '%s', aborting\n", name);
+                r = -ENOMEM;
+                goto fail5;
+            }
+
 
             // parameters
             rtapi_snprintf(name, HAL_NAME_LEN, "%s.stepgen.%02d.position-scale", hm2->llio->name, i);
@@ -704,6 +760,8 @@ int hm2_stepgen_parse_md(hostmot2_t *hm2, int md_index) {
             hm2->stepgen.instance[i].hal.param.maxvel = 1.0;
             hm2->stepgen.instance[i].hal.param.maxaccel = 1.0;
 
+            hm2->stepgen.instance[i].subcounts = 0;
+
             // start out the slowest possible, let the user speed up if they want
             hm2->stepgen.instance[i].hal.param.steplen   = (double)0x3FFF * ((double)1e9 / (double)hm2->stepgen.clock_frequency);
             hm2->stepgen.instance[i].hal.param.stepspace = (double)0x3FFF * ((double)1e9 / (double)hm2->stepgen.clock_frequency);
author	Sebastian Kuzminsky <seb@highlab.com>	2009-05-22 17:48:43 +0000
committer	Sebastian Kuzminsky <seb@highlab.com>	2009-05-22 17:48:43 +0000
commit	d68a34e48b15dcf0440c9d9b1e9881fd35d73c9f (patch)
tree	8784eb8e50d577624635e9fc020f44980290b24c
parent	0b2df83b026b81f7144a3008d1ff47d7e44805fa (diff)
download	linuxcnc-d68a34e48b15dcf0440c9d9b1e9881fd35d73c9f.tar.gz linuxcnc-d68a34e48b15dcf0440c9d9b1e9881fd35d73c9f.zip