int hpg_encoder_init(hal_pru_generic_t *hpg){
int r,i;
if (hpg->config.num_encoders <= 0)
return 0;
rtapi_print("hpg_encoder_init\n");
// FIXME: Support multiple encoder tasks like so: num_encoders=3,4,2,5
// hpg->encoder.num_instances = hpg->config.num_encoders;
hpg->encoder.num_instances = 1;
// Allocate HAL shared memory for instance state data
hpg->encoder.instance = (hpg_encoder_instance_t *) hal_malloc(sizeof(hpg_encoder_instance_t) * hpg->encoder.num_instances);
if (hpg->encoder.instance == 0) {
HPG_ERR("ERROR: hal_malloc() failed\n");
return -1;
}
rtapi_print("malloc: hpg_encoder_instance_t = %p\n",hpg->encoder.instance);
// Clear memory
memset(hpg->encoder.instance, 0, (sizeof(hpg_encoder_instance_t) * hpg->encoder.num_instances) );
for (i=0; i < hpg->encoder.num_instances; i++) {
// FIXME: Support multiple instances like so: num_encoders=3,4,2,5
hpg->encoder.instance[i].num_channels = hpg->config.num_encoders;
// Allocate HAL shared memory for channel state data
hpg->encoder.instance[i].chan = (hpg_encoder_channel_instance_t *) hal_malloc(sizeof(hpg_encoder_channel_instance_t) * hpg->encoder.instance[i].num_channels);
if (hpg->encoder.instance[i].chan == 0) {
HPG_ERR("ERROR: hal_malloc() failed\n");
return -1;
}
rtapi_print("malloc: hpg_encoder_channel_instance_t = %p\n",hpg->encoder.instance[i].chan);
int len = sizeof(hpg->encoder.instance[i].pru) + (sizeof(PRU_encoder_chan_t) * hpg->encoder.instance[i].num_channels);
hpg->encoder.instance[i].task.addr = pru_malloc(hpg, len);
hpg->encoder.instance[i].pru.task.hdr.mode = eMODE_ENCODER;
hpg->encoder.instance[i].LUT = pru_malloc(hpg, sizeof(Counter_LUT));
pru_task_add(hpg, &(hpg->encoder.instance[i].task));
if ((r = export_encoder(hpg,i)) != 0){
HPG_ERR("ERROR: failed to export encoder %i: %i\n",i,r);
return -1;
}
}
return 0;
}
/*---------------------
Local functions
---------------------*/
static int setup_eQEP(eqep_t *eqep)
{
export_encoder(eqep);
*(eqep->raw_counts) = 0;
*(eqep->count) = 0;
*(eqep->min_speed) = 1.0;
*(eqep->pos) = 0.0;
*(eqep->pos_scale) = 1.0;
*(eqep->vel) = 0.0;
*(eqep->phase_error_count) = 0;
*(eqep->counter_mode) = false;
*(eqep->x2_mode) = false;
*(eqep->invertA) = false;
*(eqep->invertB) = false;
*(eqep->invertZ) = false;
*(eqep->capture_period) = 0;
*(eqep->capture_threshold) = 100;
*(eqep->capture_overflow_count) = 0;
*(eqep->capture_dir_change_count) = 0;
eqep->old_raw_count = 0;
eqep->old_scale = 1.0;
eqep->raw_count = 0;
eqep->timestamp = 0;
eqep->index_count = 0;
eqep->counts_since_timeout = 0;
eqep->scale = 1.0 / *(eqep->pos_scale);
eqep->reverse = false;
eqep->old_vel = 0.0;
eqep->old_counter_mode = false;
eqep->old_x2_mode = false;
eqep->old_invertA = false;
eqep->old_invertB = false;
eqep->old_invertZ = false;
eqep->old_capture_pre = 255u;
eqep->capture_freq = 0.0;
eqep->eqep_reg->QDECCTL = XCR; /* start in x1 resolution */
eqep->eqep_reg->QPOSINIT = 0;
eqep->eqep_reg->QPOSMAX = -1;
eqep->eqep_reg->QEINT |= (IEL | PHE);
eqep->eqep_reg->QEPCTL = PHEN | IEL0 | IEL1 | SWI |PCRM0;
eqep->eqep_reg->QCAPCTL = 0u; // reset to prevent prescaler problems
eqep->eqep_reg->QCAPCTL |= CEN; // enable eQEP capture
rtapi_print("%s: REVID = %#x\n",modname, eqep->eqep_reg->QREVID);
return 0;
}
