Add interpolated position pin to Quadrature encoder module

Author: pcw-mesa

docs/man/man9/hostmot2.9

@@ -290,6 +290,14 @@ Number of encoder counts since the previous reset.
 (float out) position
 Encoder position in position units (count / scale).
 
+.TP
+(float out) position-interpolated
+Encoder interpolated position in position units (count / scale).
+Only valid  when velocity is approximately constant and the time 
+between counts is less than the velocity timeout parameter value.  
+Do not use for position control. Useful for spindle synchronized
+moves with low resolution encoders.
+
 .TP
 (float out) position-latched
 Encoder latched position in position units (count / scale).

src/hal/drivers/mesa-hostmot2/encoder.c

@@ -537,6 +537,13 @@ int hm2_encoder_parse_md(hostmot2_t *hm2, int md_index) {
                 goto fail1;
             }
 
+            rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.position-interpolated", hm2->llio->name, i);
+            r = hal_pin_float_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.position_interpolated), hm2->llio->comp_id);
+            if (r < 0) {
+                HM2_ERR("error adding pin '%s', aborting\n", name);
+                goto fail1;
+            }
+
             rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.position-latched", hm2->llio->name, i);
             r = hal_pin_float_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.position_latch), hm2->llio->comp_id);
             if (r < 0) {
@@ -824,6 +831,10 @@ static void hm2_encoder_instance_update_rawcounts_and_handle_index(hostmot2_t *h
 
             e->zero_offset = prev_rawcounts + reg_count_diff;
             *e->hal.pin.index_enable = 0;
+            // may need to update interpolated position after index event because
+            // this _may_ happen without a count but its pretty ugly...
+            // *e->hal.pin.count = *e->hal.pin.rawcounts - e->zero_offset;
+            //*e->hal.pin.position_interpolated = *e->hal.pin.count / e->hal.param.scale;
         }
     } else if(e->prev_control & HM2_ENCODER_LATCH_ON_PROBE) {
         if (hm2->encoder.firmware_supports_probe) {
@@ -888,6 +899,8 @@ static void hm2_encoder_instance_update_position(hostmot2_t *hm2, int instance)
 
     if (*e->hal.pin.reset) {
         e->zero_offset = *e->hal.pin.rawcounts;
+        *e->hal.pin.position_interpolated = *e->hal.pin.position; 
+
     }
 
 
@@ -1000,6 +1013,7 @@ static void hm2_encoder_instance_process_tram_read(hostmot2_t *hm2, int instance
                 if (dT_s >= e->hal.param.vel_timeout) {
                     *e->hal.pin.velocity = 0.0;
                     *e->hal.pin.velocity_rpm = 0.0;
+                    *e->hal.pin.position_interpolated = *e->hal.pin.position;
                     e->state = HM2_ENCODER_STOPPED;
                     break;
                 }
@@ -1009,6 +1023,8 @@ static void hm2_encoder_instance_process_tram_read(hostmot2_t *hm2, int instance
                 } else {
                     dS_counts = -1;
                 }
+                // if no counts, calculate interpolated position
+                *e->hal.pin.position_interpolated = *e->hal.pin.position + (dT_s * *e->hal.pin.velocity);
 
                 dS_pos_units = dS_counts / e->hal.param.scale;
 
@@ -1039,7 +1055,7 @@ static void hm2_encoder_instance_process_tram_read(hostmot2_t *hm2, int instance
 
                 hm2_encoder_instance_update_rawcounts_and_handle_index(hm2, instance);
                 hm2_encoder_instance_update_position(hm2, instance);
-
+                *e->hal.pin.position_interpolated = *e->hal.pin.position;
                 time_of_interest = hm2_encoder_get_reg_timestamp(hm2, instance);
                 if (time_of_interest < e->prev_time_of_interest) {
                     // tsc rollover!
@@ -1059,6 +1075,8 @@ static void hm2_encoder_instance_process_tram_read(hostmot2_t *hm2, int instance
                 ) {
                     *e->hal.pin.velocity = 0.0;
                     *e->hal.pin.velocity_rpm = 0.0;
+                    *e->hal.pin.position_interpolated = *e->hal.pin.position;
+
                 } else {
                     dT_clocks = (time_of_interest - e->prev_event_reg_timestamp) + (e->tsc_num_rollovers << 16);
                     dT_s = (double)dT_clocks * hm2->encoder.seconds_per_tsdiv_clock;

src/hal/drivers/mesa-hostmot2/hostmot2.h

@@ -281,6 +281,7 @@ typedef struct {
             hal_s32_t *count_latch;  // (rawlatch - zero_offset)
             hal_float_t *position;
             hal_float_t *position_latch;
+            hal_float_t *position_interpolated;
             hal_float_t *velocity;
             hal_float_t *velocity_rpm;
             hal_bit_t *reset;
@@ -338,7 +339,7 @@ typedef struct {
         hal_u32_t *sample_frequency;
         hal_u32_t *skew;
         hal_s32_t *dpll_timer_num;
-	hal_bit_t *hires_timestamp;
+        hal_bit_t *hires_timestamp;
 
     } pin;
 } hm2_encoder_module_global_t;