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; }