// // Copyright (C) 2007-2008 Sebastian Kuzminsky // // This program is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation; either version 2 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA // // // This file contains the driver for the HostMot2 encoder v2 Module. // // It supports Index and Index Mask, and high-fidelity velocity // estimation. // #include #include "rtapi.h" #include "rtapi_string.h" #include "rtapi_math.h" #include "hal.h" #include "hal/drivers/mesa-hostmot2/hostmot2.h" static void do_flag(u32 *reg, int condition, u32 bits) { if (condition) { *reg |= bits; } else { *reg &= ~bits; } } static void hm2_encoder_update_control_register(hostmot2_t *hm2) { int i; for (i = 0; i < hm2->encoder.num_instances; i ++) { hm2_encoder_instance_t *e = &hm2->encoder.instance[i]; int index_enable = *e->hal.pin.index_enable; int latch_enable = *e->hal.pin.latch_enable; hm2->encoder.control_reg[i] = 0; do_flag( &hm2->encoder.control_reg[i], index_enable, HM2_ENCODER_LATCH_ON_INDEX ); do_flag( &hm2->encoder.control_reg[i], !index_enable && latch_enable, HM2_ENCODER_LATCH_ON_PROBE ); do_flag( &hm2->encoder.control_reg[i], index_enable || latch_enable, HM2_ENCODER_INDEX_JUSTONCE ); do_flag( &hm2->encoder.control_reg[i], *e->hal.pin.latch_polarity, HM2_ENCODER_PROBE_POLARITY ); do_flag( &hm2->encoder.control_reg[i], e->hal.param.index_invert, HM2_ENCODER_INDEX_POLARITY ); do_flag( &hm2->encoder.control_reg[i], e->hal.param.index_mask, HM2_ENCODER_INDEX_MASK ); do_flag( &hm2->encoder.control_reg[i], e->hal.param.index_mask_invert, HM2_ENCODER_INDEX_MASK_POLARITY ); do_flag( &hm2->encoder.control_reg[i], e->hal.param.counter_mode, HM2_ENCODER_COUNTER_MODE ); do_flag( &hm2->encoder.control_reg[i], e->hal.param.filter, HM2_ENCODER_FILTER ); } } static void hm2_encoder_read_control_register(hostmot2_t *hm2) { int i; static int last_error_enable = 0; for (i = 0; i < hm2->encoder.num_instances; i ++) { hm2_encoder_instance_t *e = &hm2->encoder.instance[i]; if (*e->hal.pin.quadrature_error_enable) { if (!last_error_enable) { hm2_encoder_force_write(hm2); last_error_enable = 1; } else { int state = (hm2->encoder.read_control_reg[i] & HM2_ENCODER_CONTROL_MASK) & HM2_ENCODER_QUADRATURE_ERROR; if ((*e->hal.pin.quadrature_error == 0) && state) { HM2_ERR("Encoder %d: quadrature count error", i); } *e->hal.pin.quadrature_error = (hal_bit_t) state; } } else { *e->hal.pin.quadrature_error = 0; last_error_enable = 0; } *e->hal.pin.input_a = hm2->encoder.read_control_reg[i] & HM2_ENCODER_INPUT_A; *e->hal.pin.input_b = hm2->encoder.read_control_reg[i] & HM2_ENCODER_INPUT_B; *e->hal.pin.input_idx = hm2->encoder.read_control_reg[i] & HM2_ENCODER_INPUT_INDEX; } } static void hm2_encoder_set_filter_rate_and_skew(hostmot2_t *hm2) { u32 filter_rate = hm2->encoder.clock_frequency/(*hm2->encoder.hal->pin.sample_frequency); if (filter_rate == 1) { filter_rate = 0xFFF; } else { filter_rate -= 2; } *hm2->encoder.hal->pin.sample_frequency = hm2->encoder.clock_frequency/(filter_rate + 2); HM2_DBG("Setting encoder QFilterRate to %d\n", filter_rate); if (hm2->encoder.has_skew) { u32 skew = (*hm2->encoder.hal->pin.skew)/(1e9/hm2->encoder.clock_frequency); if (skew > 15) { skew = 15; } HM2_DBG("Setting mux encoder skew to %d\n", skew); filter_rate |= (skew & 0xF) << 28; *hm2->encoder.hal->pin.skew = skew*(1e9/hm2->encoder.clock_frequency); hm2->encoder.written_skew = *hm2->encoder.hal->pin.skew; } hm2->llio->write(hm2->llio, hm2->encoder.filter_rate_addr, &filter_rate, sizeof(u32)); hm2->encoder.written_sample_frequency = *hm2->encoder.hal->pin.sample_frequency; } void hm2_encoder_write(hostmot2_t *hm2) { int i; if (hm2->encoder.num_instances == 0) return; hm2_encoder_update_control_register(hm2); for (i = 0; i < hm2->encoder.num_instances; i ++) { if ((hm2->encoder.instance[i].prev_control & HM2_ENCODER_CONTROL_MASK) != (hm2->encoder.control_reg[i] & HM2_ENCODER_CONTROL_MASK)) { goto force_write; } } if (*hm2->encoder.hal->pin.sample_frequency != hm2->encoder.written_sample_frequency) { goto force_write; } if (hm2->encoder.has_skew) { if (*hm2->encoder.hal->pin.skew != hm2->encoder.written_skew) { goto force_write; } } return; force_write: hm2_encoder_force_write(hm2); } void hm2_encoder_force_write(hostmot2_t *hm2) { int i; if (hm2->encoder.num_instances == 0) return; hm2_encoder_update_control_register(hm2); hm2->llio->write( hm2->llio, hm2->encoder.latch_control_addr, hm2->encoder.control_reg, (hm2->encoder.num_instances * sizeof(u32)) ); for (i = 0; i < hm2->encoder.num_instances; i ++) { hm2->encoder.instance[i].prev_control = hm2->encoder.control_reg[i]; } hm2->llio->write( hm2->llio, hm2->encoder.timestamp_div_addr, &hm2->encoder.timestamp_div_reg, sizeof(u32) ); hm2_encoder_set_filter_rate_and_skew(hm2); } int hm2_encoder_parse_md(hostmot2_t *hm2, int md_index) { hm2_module_descriptor_t *md = &hm2->md[md_index]; int r; // // some standard sanity checks // if (hm2->md[md_index].gtag == HM2_GTAG_ENCODER) { if (hm2_md_is_consistent(hm2, md_index, 2, 5, 4, 0x0003)) { // ok } else if (hm2_md_is_consistent_or_complain(hm2, md_index, 3, 5, 4, 0x0003)) { // ok } else { HM2_ERR("inconsistent Encoder Module Descriptor!\n"); return -EINVAL; } } else if (hm2->md[md_index].gtag == HM2_GTAG_MUXED_ENCODER) { if (hm2_md_is_consistent(hm2, md_index, 2, 5, 4, 0x0003)) { HM2_PRINT("WARNING: this firmware has Muxed Encoder v2!\n"); HM2_PRINT("WARNING: velocity computation will be incorrect!\n"); HM2_PRINT("WARNING: upgrade your firmware!\n"); } else if (hm2_md_is_consistent(hm2, md_index, 3, 5, 4, 0x0003)) { // ok } else if (hm2_md_is_consistent_or_complain(hm2, md_index, 4, 5, 4, 0x0003)) { // ok } else { HM2_ERR("inconsistent Muxed Encoder Module Descriptor!\n"); return -EINVAL; } } if (hm2->encoder.num_instances != 0) { HM2_ERR( "found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n", hm2_get_general_function_name(md->gtag) ); return -EINVAL; } if (hm2->config.num_encoders > md->instances) { HM2_ERR( "config.num_encoders=%d, but only %d are available, not loading driver\n", hm2->config.num_encoders, md->instances ); return -EINVAL; } if (hm2->config.num_encoders == 0) { return 0; } // // looks good, start initializing // if (hm2->config.num_encoders == -1) { hm2->encoder.num_instances = md->instances; } else { hm2->encoder.num_instances = hm2->config.num_encoders; } // allocate the module-global HAL shared memory hm2->encoder.hal = (hm2_encoder_module_global_t *)hal_malloc(sizeof(hm2_encoder_module_global_t)); if (hm2->encoder.hal == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } hm2->encoder.instance = (hm2_encoder_instance_t *)hal_malloc(hm2->encoder.num_instances * sizeof(hm2_encoder_instance_t)); if (hm2->encoder.instance == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } hm2->encoder.stride = md->register_stride; hm2->encoder.clock_frequency = md->clock_freq; hm2->encoder.version = md->version; hm2->encoder.counter_addr = md->base_address + (0 * md->register_stride); hm2->encoder.latch_control_addr = md->base_address + (1 * md->register_stride); hm2->encoder.timestamp_div_addr = md->base_address + (2 * md->register_stride); hm2->encoder.timestamp_count_addr = md->base_address + (3 * md->register_stride); hm2->encoder.filter_rate_addr = md->base_address + (4 * md->register_stride); // it's important that the TSC gets read *before* the C&T registers below r = hm2_register_tram_read_region(hm2, hm2->encoder.timestamp_count_addr, sizeof(u32), &hm2->encoder.timestamp_count_reg); if (r < 0) { HM2_ERR("error registering tram read region for Encoder Timestamp Count Register (%d)\n", r); goto fail0; } // it's important that the C&T registers get read *after* the TSC register above r = hm2_register_tram_read_region(hm2, hm2->encoder.counter_addr, (hm2->encoder.num_instances * sizeof(u32)), &hm2->encoder.counter_reg); if (r < 0) { HM2_ERR("error registering tram read region for Encoder Counter register (%d)\n", r); goto fail0; } r = hm2_register_tram_read_region(hm2, hm2->encoder.latch_control_addr, (hm2->encoder.num_instances * sizeof(u32)), &hm2->encoder.read_control_reg); if (r < 0) { HM2_ERR("error registering tram read region for Encoder Latch/Control register (%d)\n", r); goto fail0; } hm2->encoder.control_reg = (u32 *)kmalloc(hm2->encoder.num_instances * sizeof(u32), GFP_KERNEL); if (hm2->encoder.control_reg == NULL) { HM2_ERR("out of memory!\n"); r = -ENOMEM; goto fail0; } // export the encoders to HAL // FIXME: r hides the r in enclosing function, and it returns the wrong thing { int i; int r; char name[HAL_NAME_LEN + 1]; // these hal parameters affect all encoder instances if (md->gtag == HM2_GTAG_MUXED_ENCODER) { rtapi_snprintf(name, sizeof(name), "%s.encoder.muxed-sample-frequency", hm2->llio->name); } else { rtapi_snprintf(name, sizeof(name), "%s.encoder.sample-frequency", hm2->llio->name); } r = hal_pin_u32_new(name, HAL_IN, &(hm2->encoder.hal->pin.sample_frequency), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding pin %s, aborting\n", name); goto fail1; } if ((md->gtag == HM2_GTAG_MUXED_ENCODER) && (md->version == 4)) { rtapi_snprintf(name, sizeof(name), "%s.encoder.muxed-skew", hm2->llio->name); r = hal_pin_u32_new(name, HAL_IN, &(hm2->encoder.hal->pin.skew), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding pin %s, aborting\n", name); goto fail1; } hm2->encoder.has_skew = 1; } for (i = 0; i < hm2->encoder.num_instances; i ++) { // pins rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.rawcounts", hm2->llio->name, i); r = hal_pin_s32_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.rawcounts), 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.rawlatch", hm2->llio->name, i); r = hal_pin_s32_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.rawlatch), 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.count", hm2->llio->name, i); r = hal_pin_s32_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.count), 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.count-latched", hm2->llio->name, i); r = hal_pin_s32_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.count_latch), 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", hm2->llio->name, i); r = hal_pin_float_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.position), 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) { HM2_ERR("error adding pin '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.velocity", hm2->llio->name, i); r = hal_pin_float_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.velocity), 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.reset", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_IN, &(hm2->encoder.instance[i].hal.pin.reset), 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.index-enable", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_IO, &(hm2->encoder.instance[i].hal.pin.index_enable), 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.latch-enable", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_IN, &(hm2->encoder.instance[i].hal.pin.latch_enable), 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.latch-polarity", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_IN, &(hm2->encoder.instance[i].hal.pin.latch_polarity), 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.quad-error", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.quadrature_error), 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.quad-error-enable", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_IN, &(hm2->encoder.instance[i].hal.pin.quadrature_error_enable), 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.input-a", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.input_a), 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.input-b", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.input_b), 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.input-index", hm2->llio->name, i); r = hal_pin_bit_new(name, HAL_OUT, &(hm2->encoder.instance[i].hal.pin.input_idx), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding pin '%s', aborting\n", name); goto fail1; } // parameters rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.scale", hm2->llio->name, i); r = hal_param_float_new(name, HAL_RW, &(hm2->encoder.instance[i].hal.param.scale), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.index-invert", hm2->llio->name, i); r = hal_param_bit_new(name, HAL_RW, &(hm2->encoder.instance[i].hal.param.index_invert), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.index-mask", hm2->llio->name, i); r = hal_param_bit_new(name, HAL_RW, &(hm2->encoder.instance[i].hal.param.index_mask), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.index-mask-invert", hm2->llio->name, i); r = hal_param_bit_new(name, HAL_RW, &(hm2->encoder.instance[i].hal.param.index_mask_invert), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.counter-mode", hm2->llio->name, i); r = hal_param_bit_new(name, HAL_RW, &(hm2->encoder.instance[i].hal.param.counter_mode), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.filter", hm2->llio->name, i); r = hal_param_bit_new(name, HAL_RW, &(hm2->encoder.instance[i].hal.param.filter), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.vel-timeout", hm2->llio->name, i); r = hal_param_float_new(name, HAL_RW, &(hm2->encoder.instance[i].hal.param.vel_timeout), hm2->llio->comp_id); if (r < 0) { HM2_ERR("error adding param '%s', aborting\n", name); goto fail1; } // // init the hal objects that need it // the things not initialized here will be set by hm2_encoder_tram_init() // *hm2->encoder.instance[i].hal.pin.reset = 0; *hm2->encoder.instance[i].hal.pin.index_enable = 0; hm2->encoder.instance[i].hal.param.scale = 1.0; hm2->encoder.instance[i].hal.param.index_invert = 0; hm2->encoder.instance[i].hal.param.index_mask = 0; hm2->encoder.instance[i].hal.param.index_mask_invert = 0; hm2->encoder.instance[i].hal.param.counter_mode = 0; hm2->encoder.instance[i].hal.param.filter = 1; hm2->encoder.instance[i].hal.param.vel_timeout = 0.5; hm2->encoder.instance[i].state = HM2_ENCODER_STOPPED; } } // // Set the Timestamp Divisor Register // // We want the timestamp to count as quickly as possible, so we get the // best temporal resolution. // // But we want it to count slow enough that the 16-bit counter doesnt // overrun between successive calls to the servo thread (easy), and // even slower so that we can do good low-speed velocity estimation // (long between roll-overs). // // A resonably slow servo thread runs at 1 KHz. A fast one runs at 10 // KHz. The actual servo period is unknown at loadtime, and is likely // to fluctuate slightly when the system is under load. // // Peter suggests a Quadrature Timestamp clock rate of 1 MHz. This // means that a 1 KHz servo period sees about 1000 clocks per period. // // // From the HM2 RegMap: // // Timestamp count rate is ClockLow/(TSDiv+2). // Any divisor with MSb set = divide by 1 // // This gives us: // // rate = 1 MHz = 1e6 = ClockLow / (TSDiv+2) // // TSDiv+2 = ClockLow / 1e6 // // TSDiv = (ClockLow / 1e6) - 2 // // seconds_per_clock = 1 / rate = (TSDiv+2) / ClockLow // // // The 7i43 has a 50 MHz ClockLow, giving TSDiv = 48 and 1 us/clock // The PCI cards have a 33 MHz ClockLow, giving TSDiv = 31 and again 1 us/clock // hm2->encoder.timestamp_div_reg = (hm2->encoder.clock_frequency / 1e6) - 2; hm2->encoder.seconds_per_tsdiv_clock = (double)(hm2->encoder.timestamp_div_reg + 2) / (double)hm2->encoder.clock_frequency; if (md->gtag == HM2_GTAG_ENCODER) { *hm2->encoder.hal->pin.sample_frequency = 25000000; } else { *hm2->encoder.hal->pin.sample_frequency = 8000000; } return hm2->encoder.num_instances; fail1: kfree(hm2->encoder.control_reg); fail0: hm2->encoder.num_instances = 0; return r; } void hm2_encoder_tram_init(hostmot2_t *hm2) { int i; if (hm2->encoder.num_instances <= 0) return; // "all time is now" hm2->encoder.prev_timestamp_count_reg = *hm2->encoder.timestamp_count_reg; // all the encoders start "stopped where they are" for (i = 0; i < hm2->encoder.num_instances; i ++) { u16 count; count = hm2_encoder_get_reg_count(hm2, i); *hm2->encoder.instance[i].hal.pin.rawcounts = count; *hm2->encoder.instance[i].hal.pin.rawlatch = count; *hm2->encoder.instance[i].hal.pin.count = 0; *hm2->encoder.instance[i].hal.pin.count_latch = 0; *hm2->encoder.instance[i].hal.pin.position = 0.0; *hm2->encoder.instance[i].hal.pin.position_latch = 0.0; *hm2->encoder.instance[i].hal.pin.velocity = 0.0; *hm2->encoder.instance[i].hal.pin.quadrature_error = 0; hm2->encoder.instance[i].zero_offset = count; hm2->encoder.instance[i].prev_reg_count = count; hm2->encoder.instance[i].state = HM2_ENCODER_STOPPED; // Note: we dont initialize the other internal state variables, // because in the "Stopped" state they're not used } } /** * @brief Updates the encoder's rawcounts accumulator, and checks for index pulse if appropriate. * * Sets these variables: * * hal.pin.rawcounts * .prev_reg_count * (maybe) .index_enabled and .zero_offset * * This function expects the TRAM read to have just finished, so that * counter_reg[i] is up-to-date. * * May read the Latch register (if searching for the Index pulse). * * @param hm2 The hostmot2 structure being worked on. * * @param instance The index of the encoder instance. */ static void hm2_encoder_instance_update_rawcounts_and_handle_index(hostmot2_t *hm2, int instance) { u16 reg_count; s32 reg_count_diff; s32 prev_rawcounts; hm2_encoder_instance_t *e; e = &hm2->encoder.instance[instance]; prev_rawcounts = *e->hal.pin.rawcounts; // // figure out current rawcounts accumulated by the driver // reg_count = hm2_encoder_get_reg_count(hm2, instance); reg_count_diff = (s32)reg_count - (s32)e->prev_reg_count; if (reg_count_diff > 32768) reg_count_diff -= 65536; if (reg_count_diff < -32768) reg_count_diff += 65536; *e->hal.pin.rawcounts += reg_count_diff; // // if we've told the FPGA we're looking for an index pulse: // read the latch/ctrl register // if it's triggered set zero_offset to the rawcounts version of the latched count // if (e->prev_control & HM2_ENCODER_LATCH_ON_INDEX) { u32 latch_ctrl; hm2->llio->read( hm2->llio, hm2->encoder.latch_control_addr + (instance * sizeof(u32)), &latch_ctrl, sizeof(u32) ); if (0 == (latch_ctrl & HM2_ENCODER_LATCH_ON_INDEX)) { // hm2 reports index event occurred u16 latched_count; latched_count = (latch_ctrl >> 16) & 0xffff; reg_count_diff = (s32)latched_count - (s32)e->prev_reg_count; if (reg_count_diff > 32768) reg_count_diff -= 65536; if (reg_count_diff < -32768) reg_count_diff += 65536; e->zero_offset = prev_rawcounts + reg_count_diff; *e->hal.pin.index_enable = 0; } } else if(e->prev_control & HM2_ENCODER_LATCH_ON_PROBE) { u32 latch_ctrl; hm2->llio->read( hm2->llio, hm2->encoder.latch_control_addr + (instance * sizeof(u32)), &latch_ctrl, sizeof(u32) ); if (0 == (latch_ctrl & HM2_ENCODER_LATCH_ON_PROBE)) { // hm2 reports probe event occurred u16 latched_count; latched_count = (latch_ctrl >> 16) & 0xffff; reg_count_diff = (s32)latched_count - (s32)e->prev_reg_count; if (reg_count_diff > 32768) reg_count_diff -= 65536; if (reg_count_diff < -32768) reg_count_diff += 65536; *(e->hal.pin.rawlatch) = prev_rawcounts + reg_count_diff; // *e->hal.pin.latch_enable = 0; } } e->prev_reg_count = reg_count; } /** * @brief Updates the driver's idea of the encoder's position. * * Sets these variables: * * hal.pin.count * hal.pin.position * .prev_reg_count * .zero_offset (if hal.pin.reset is True) * * This function expects the TRAM read and (if appropriate) the * hm2_encoder_instance_update_rawcounts_and_check_for_index() function to * have just finished, so that counter_reg[i], rawcounts, and zero_offset * are all up-to-date. * * This function optionally sets .zero_offset (if .reset is True), then * computes .counts and .position from .rawcounts, .scale, and * .zero_offset. * * @param hm2 The hostmot2 structure being worked on. * * @param instance The index of the encoder instance. */ static void hm2_encoder_instance_update_position(hostmot2_t *hm2, int instance) { hm2_encoder_instance_t *e; e = &hm2->encoder.instance[instance]; // // reset count if the user wants us to (by just setting the zero offset to the current rawcounts) // if (*e->hal.pin.reset) { e->zero_offset = *e->hal.pin.rawcounts; } // // Now we know the current rawcounts and zero_offset, which together // tell us the current count. // // From that we easily compute count and scaled position. // *e->hal.pin.count = *e->hal.pin.rawcounts - e->zero_offset; *e->hal.pin.count_latch = *e->hal.pin.rawlatch - e->zero_offset; // // Now we know the current current .count, from this we easily compute // the scaled position. // *e->hal.pin.position = *e->hal.pin.count / e->hal.param.scale; *e->hal.pin.position_latch = *e->hal.pin.count_latch / e->hal.param.scale; } /** * @brief Processes the information received from the QuadratureCounter * (encoder) instance to produce an estimate of position & velocity. */ static void hm2_encoder_instance_process_tram_read(hostmot2_t *hm2, int instance) { hm2_encoder_instance_t *e; e = &hm2->encoder.instance[instance]; // sanity check if (e->hal.param.scale == 0.0) { HM2_ERR("encoder.%02d.scale == 0.0, bogus, setting to 1.0\n", instance); e->hal.param.scale = 1.0; } hm2_encoder_read_control_register(hm2); switch (e->state) { case HM2_ENCODER_STOPPED: { u16 reg_count; // the count currently in the register // get current count from the FPGA (already read) reg_count = hm2_encoder_get_reg_count(hm2, instance); if (reg_count == e->prev_reg_count && !(e->prev_control & (HM2_ENCODER_LATCH_ON_INDEX | HM2_ENCODER_LATCH_ON_PROBE))) { // still not moving, but .reset can change the position hm2_encoder_instance_update_position(hm2, instance); return; } // if we get here, we *were* stopped but now we're moving // or we are looking for index or probe hm2_encoder_instance_update_rawcounts_and_handle_index(hm2, instance); hm2_encoder_instance_update_position(hm2, instance); e->prev_event_rawcounts = *e->hal.pin.rawcounts; e->prev_event_reg_timestamp = hm2_encoder_get_reg_timestamp(hm2, instance); e->prev_dS_counts = 0; e->tsc_num_rollovers = 0; e->prev_time_of_interest = e->prev_event_reg_timestamp; e->state = HM2_ENCODER_MOVING; break; } case HM2_ENCODER_MOVING: { u16 reg_count; // the count currently in the register u16 time_of_interest; // terrible variable name, sorry s32 dT_clocks; double dT_s; s32 dS_counts; double dS_pos_units; // these are just for debugging the encoder.vel NaN problem // reported by micges, remove when the bug is fixed static s32 prev_update_dS_counts; static s32 prev_update_dT_clocks; // get current count from the FPGA (already read) reg_count = hm2_encoder_get_reg_count(hm2, instance); if (reg_count == e->prev_reg_count) { double vel; // // we're moving, but so slow that we didnt get an event // since last time we checked // // .reset can still change the position hm2_encoder_instance_update_position(hm2, instance); time_of_interest = hm2_encoder_get_reg_tsc(hm2); if (time_of_interest < e->prev_time_of_interest) { // tsc rollover! e->tsc_num_rollovers ++; } 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; if (dT_s >= e->hal.param.vel_timeout) { *e->hal.pin.velocity = 0.0; e->state = HM2_ENCODER_STOPPED; break; } if ((*e->hal.pin.velocity * e->hal.param.scale) > 0.0) { dS_counts = 1; } else { dS_counts = -1; } dS_pos_units = dS_counts / e->hal.param.scale; // FIXME: There's a bug somewhere in this code that // sometimes makes encoder.velocity NaN. Observed by // micges, but never reproduced. if (dT_clocks < 1) { HM2_PRINT("(%s:%d) uh-oh, encoder vel is broken when slow\n", __FILE__, __LINE__); HM2_PRINT(" please email a bug report to the linuxcnc-devel list!\n"); HM2_PRINT(" dS_counts=%d, dT_clocks=%d\n", dS_counts, dT_clocks); HM2_PRINT(" prev_update_dS_counts=%d, prev_update_dT_clocks=%d\n", prev_update_dS_counts, prev_update_dT_clocks); } else { // we know the encoder velocity is not faster than this vel = dS_pos_units / dT_s; if (fabs(vel) < fabs(*e->hal.pin.velocity)) { *e->hal.pin.velocity = vel; } } prev_update_dS_counts = dS_counts; prev_update_dT_clocks = dT_clocks; // if waiting for index or latch, we can't shirk our duty just // because no pulses arrived if(e->prev_control & (HM2_ENCODER_LATCH_ON_INDEX | HM2_ENCODER_LATCH_ON_PROBE)) { hm2_encoder_instance_update_rawcounts_and_handle_index(hm2, instance); } e->prev_time_of_interest = time_of_interest; } else { // // we got a new event! // hm2_encoder_instance_update_rawcounts_and_handle_index(hm2, instance); hm2_encoder_instance_update_position(hm2, instance); time_of_interest = hm2_encoder_get_reg_timestamp(hm2, instance); if (time_of_interest < e->prev_time_of_interest) { // tsc rollover! e->tsc_num_rollovers ++; } dS_counts = *e->hal.pin.rawcounts - e->prev_event_rawcounts; // If we reversed direction and moved exactly one edge, // we can't reliably estimate velocity. The encoder might // be "balancing on an edge", which may lead to a very // small dT between adjacent edges, leading to misleadingly // large velocity estimates. So we just call it 0. if ( (((*e->hal.pin.rawcounts - e->prev_event_rawcounts) == 1) && (e->prev_dS_counts < 0)) || (((*e->hal.pin.rawcounts - e->prev_event_rawcounts) == -1) && (e->prev_dS_counts > 0)) ) { *e->hal.pin.velocity = 0.0; prev_update_dT_clocks = -1; // magic value meaning "i ignored dT_clocks last time" } 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; dS_pos_units = dS_counts / e->hal.param.scale; // FIXME: There's a bug somewhere in this code that // sometimes makes encoder.velocity NaN. Observed by // micges, but never reproduced. if (dT_clocks < 1) { HM2_PRINT("(%s:%d) uh-oh, encoder vel is broken with an edge\n", __FILE__, __LINE__); HM2_PRINT(" please email a bug report to the linuxcnc-devel list!\n"); HM2_PRINT(" dS_counts=%d, dT_clocks=%d\n", dS_counts, dT_clocks); HM2_PRINT(" prev_update_dS_counts=%d, prev_update_dT_clocks=%d\n", prev_update_dS_counts, prev_update_dT_clocks); } else { // finally time to do Relative-Time Velocity Estimation *e->hal.pin.velocity = dS_pos_units / dT_s; } prev_update_dT_clocks = dT_clocks; } prev_update_dS_counts = dS_counts; e->tsc_num_rollovers = 0; // we're "using up" the rollovers now e->prev_dS_counts = dS_counts; e->prev_event_rawcounts = *e->hal.pin.rawcounts; e->prev_event_reg_timestamp = time_of_interest; e->prev_time_of_interest = time_of_interest; } break; } default: { HM2_ERR("encoder %d has invalid state %d, resetting to Stopped!\n", instance, e->state); e->state = HM2_ENCODER_STOPPED; break; } } } void hm2_encoder_process_tram_read(hostmot2_t *hm2, long l_period_ns) { int i; if (hm2->encoder.num_instances <= 0) return; // process each encoder instance independently for (i = 0; i < hm2->encoder.num_instances; i ++) { hm2_encoder_instance_process_tram_read(hm2, i); } } void hm2_encoder_cleanup(hostmot2_t *hm2) { if (hm2->encoder.num_instances <= 0) return; kfree(hm2->encoder.control_reg); } void hm2_encoder_print_module(hostmot2_t *hm2) { int i; HM2_PRINT("Encoders: %d\n", hm2->encoder.num_instances); if (hm2->encoder.num_instances <= 0) return; HM2_PRINT(" clock_frequency: %d Hz (%s MHz)\n", hm2->encoder.clock_frequency, hm2_hz_to_mhz(hm2->encoder.clock_frequency)); HM2_PRINT(" version: %d\n", hm2->encoder.version); HM2_PRINT(" counter_addr: 0x%04X\n", hm2->encoder.counter_addr); HM2_PRINT(" latch_control_addr: 0x%04X\n", hm2->encoder.latch_control_addr); HM2_PRINT(" timestamp_div_addr: 0x%04X\n", hm2->encoder.timestamp_div_addr); HM2_PRINT(" timestamp_count_addr: 0x%04X\n", hm2->encoder.timestamp_count_addr); HM2_PRINT(" filter_rate_addr: 0x%04X\n", hm2->encoder.filter_rate_addr); HM2_PRINT(" timestamp_div: 0x%04X\n", (u16)hm2->encoder.timestamp_div_reg); for (i = 0; i < hm2->encoder.num_instances; i ++) { HM2_PRINT(" instance %d:\n", i); HM2_PRINT(" hw:\n"); HM2_PRINT(" counter = %04x.%04x\n", (hm2->encoder.counter_reg[i] >> 16), (hm2->encoder.counter_reg[i] & 0xffff)); HM2_PRINT(" latch/control = %04x.%04x\n", (hm2->encoder.control_reg[i] >> 16), (hm2->encoder.control_reg[i] & 0xffff)); HM2_PRINT(" prev_control = %04x.%04x\n", (hm2->encoder.instance[i].prev_control >> 16), (hm2->encoder.instance[i].prev_control & 0xffff)); } }