xenc_vel.c

xenc_vel.c

#include "rtapi.h"              /* RTAPI realtime OS API */
#include "rtapi_app.h"          /* RTAPI realtime module decls */
#include "rtapi_string.h"
#include "hal.h"                /* HAL public API decls */

/* module information */
MODULE_AUTHOR("Jesse Schoch");
MODULE_DESCRIPTION("Encoder Velocity for RIO encoder with z ");
MODULE_LICENSE("GPL");

static int num_chan;
static int default_num_chan=1;
static int howmany;
RTAPI_MP_INT(num_chan, "RIO ENC number of encoder channels");

#define MAX_CHAN 8
char *names[MAX_CHAN] = {0,};
RTAPI_MP_ARRAY_STRING(names, MAX_CHAN, "names of encoder");

/***********************************************************************
*                STRUCTURES AND GLOBAL VARIABLES                       *
************************************************************************/

/* data that is atomically passed from fast function to slow one */

typedef struct {
    char count_detected;
    char index_detected;
    hal_s32_t raw_count;
    rtapi_u32 timestamp;
    rtapi_s32 index_count;
} atomic;

/* this structure contains the runtime data for a single counter
   u:rw means update() reads and writes the
   c:w  means capture() writes the field
   c:s u:rc means capture() sets (to 1), update() reads and clears
*/

typedef struct {
    hal_s32_t raw_count;                /* c:rw captured raw_count */
    hal_s32_t *raw_count_in;
    hal_s32_t *prev_raw_count;
    rtapi_u32 timestamp;                /* c:rw captured timestamp */
    hal_s32_t *count;           /* c:w captured binary count value */
    hal_float_t *min_speed;     /* c:r minimum velocity to estimate nonzero */
    hal_float_t *vel;           /* c:w scaled velocity (floating point) */
    hal_float_t *vel_rpm;   /* rps * 60 for convenience */
    hal_float_t *pos_scale;     /* c:r pin: scaling factor for pos */
    hal_s32_t *updates;
    double scale;               /* c:rw reciprocal value used for scaling */
    int counts_since_timeout;   /* c:rw used for velocity calcs */
} counter_t;

static rtapi_u32 timebase;              /* master timestamp for all counters */

/* pointer to array of counter_t structs in shmem, 1 per counter */
static counter_t *counter_array;


/* other globals */
static int comp_id;             /* component ID */

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/

static int export_encoder(counter_t * addr,char * prefix);
static void update(void *arg, long period);

/***********************************************************************
*                       INIT AND EXIT CODE                             *
************************************************************************/

int rtapi_app_main(void){

    int n, retval, i;
    counter_t *cntr;

    // fuck with stupid index and array howmany counter nightmare
    //
    //
    //
    if(num_chan && names[0]) {
        rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n");
        return -EINVAL;
    }
    if(!num_chan && !names[0]) num_chan = default_num_chan;

    if(num_chan) {
        howmany = num_chan;
    } else {
        howmany = 0;
        for (i = 0; i < MAX_CHAN; i++) {
            if ( (names[i] == NULL) || (*names[i] == 0) ){
                break;
            }
            howmany = i + 1;
        }
    }
    comp_id = hal_init("xenc_vel");
    if (comp_id < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, "ENCODER: ERROR: hal_init() failed\n");
        return -1;
    }
    /* allocate shared memory for counter data */
    counter_array = hal_malloc(howmany * sizeof(counter_t));
    if (counter_array == 0) {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "ENCODER: ERROR: hal_malloc() failed\n");
        hal_exit(comp_id);
        return -1;
    }
    /* init master timestamp counter */
    timebase = 0;

    // export all the variables for each counter
    i = 0; // for names= items
    for (n = 0; n < howmany; n++) {
        // point to struct
        cntr = &(counter_array[n]);
        // export all vars
        if(num_chan) {
            char buf[HAL_NAME_LEN + 1];
            rtapi_snprintf(buf, sizeof(buf), "encoder.%d", n);
            retval = export_encoder(cntr,buf);
        } else {
            retval = export_encoder(cntr,names[i++]);
        }
        if (retval != 0) {
            rtapi_print_msg(RTAPI_MSG_ERR,
                "ENCODER: ERROR: counter %d var export failed\n", n);
            hal_exit(comp_id);
            return -1;
        }

        // init counter
        *(cntr->raw_count_in) = 0;
        *(cntr->prev_raw_count) = 0;
        *(cntr->updates) = -1;
        cntr->timestamp = 0;
        //*(cntr->vel) = 0.0;

    }




    retval = hal_export_funct("enc_vel.update-counters", update,
        counter_array, 0, 0, comp_id);
    if (retval != 0) {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "ENCODER: ERROR: count funct export failed\n");
        hal_exit(comp_id);
        return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
        "ENCODER: installed %d encoder counters\n", howmany);
    hal_ready(comp_id);
    return 0;
}


void rtapi_app_exit(void)
{
    hal_exit(comp_id);
}

/***********************************************************************
*            REALTIME ENCODER COUNTING AND UPDATE FUNCTIONS            *
************************************************************************/

static void update( void *arg, long period){
        int n;
        counter_t *cntr;
        cntr = arg;
        rtapi_s32 delta_counts;
        rtapi_u32 delta_time;
        double vel;


        for(n=0;n < howmany; n++){
                if(  *(cntr->prev_raw_count) != *(cntr->raw_count_in)) {
                        // encoder changed, do some updates

                        //delta_time = buf->timestamp - cntr->timestamp;
                        delta_time = timebase - cntr->timestamp;
                        //delta_counts = buf->raw_count - cntr->raw_count;
                        delta_counts = *(cntr->prev_raw_count) - *(cntr->raw_count_in);
                        cntr->timestamp = timebase + period;
                        vel = delta_counts / (delta_time * 1e-9);
                        rtapi_print_msg(RTAPI_MSG_INFO,
                                "vel %f delta_counts: %i delta_time %u\n", vel,delta_counts,delta_time);

                        *(cntr->updates)= *(cntr->updates) + 1;
                        *(cntr->prev_raw_count) = *(cntr->raw_count_in);
                        //*(cntr->vel) = vel;

                }

                // go to next encoder
                cntr++;
        }


        // update main timestamp thing
        timebase += period;

}




/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/
static int export_encoder(counter_t * addr,char * prefix)
{
    int retval, msg;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_INFO);

    retval = hal_pin_s32_newf(HAL_OUT, &(addr->prev_raw_count), comp_id,
                    "%s.prev_raw_count",prefix);
    if(retval != 0){
            return retval;
    }
    retval = hal_pin_s32_newf(HAL_OUT, &(addr->updates), comp_id,
                    "%s.updates",prefix);
    if(retval != 0){
            return retval;
    }
    /*

    retval = hal_pin_s32_newf(HAL_OUT, &(addr->delta_time), comp_id,
                    "%s.delta_time",prefix);
    if(retval != 0){
            return retval;
    }

    retval = hal_pin_s32_newf(HAL_OUT, &(addr->delta_count), comp_id,
                    "%s.delta_count",prefix);
    if(retval != 0){
            return retval;
    }
        */


    retval = hal_pin_s32_newf(HAL_IN, &(addr->raw_count_in), comp_id,
         "%s.raw_count_in",prefix);
   if (retval != 0){
        rtapi_print("Enc Vel in probelm");
        return retval;
   }
   return 0;
}