multigcs / riocore

riocore
GNU General Public License v2.0
8 stars 3 forks source link

How to contribute a module (documentation) #9

Closed jschoch closed 2 days ago

jschoch commented 4 months ago

Let's say I wanted to integrate some fairly complex module code that also needs it's own hal config. How do I go about integrating that and getting it merged in?

We can use my crappy encoder code, it has a clash on some of the hal pin stuff which makes it an interesting integration example.

Module code

#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 */
#include "rtapi_math.h"

/* 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;
    long long 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;
        }
    }
    rtapi_print_msg(RTAPI_MSG_ERR, "ENC_vel: init comp\n");
    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
        rtapi_print_msg(RTAPI_MSG_ERR, "ENC_vel: export vars\n");
        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
        rtapi_print_msg(RTAPI_MSG_ERR, "ENC_vel: init values\n");
        *(cntr->raw_count_in) = 0;
        *(cntr->prev_raw_count) = 0;
        *(cntr->updates) = -1;
        cntr->timestamp = 0;
        *(cntr->vel) = 0.0;
        *(cntr->vel_rpm) = 0.0;
        cntr->counts_since_timeout = 0;
        //cntr->scale = 1.0/1440;
        cntr->scale = 1.0;
        rtapi_print_msg(RTAPI_MSG_ERR, "ENC_vel: init done\n");

    }

    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;
        long long delta_time;
        double vel;
        long long current_time = rtapi_get_time();  // Get timestamp in seconds

        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 = current_time - cntr->timestamp;
                        //delta_counts = buf->raw_count - cntr->raw_count;
                        delta_counts = *(cntr->prev_raw_count) - *(cntr->raw_count_in);
                        cntr->timestamp = current_time;  // Get timestamp in seconds

                        // count is usually 1 or -1 since this is wired to the encoder's raw out
                        //  this uses the encoder's ppr to extrapolate revolutions per second
                        // nanoseconds converted to seconds via 1e-9
                        vel = delta_counts * (1.0/ (delta_time * 1e-9 * 360));
                        *(cntr->vel_rpm) = vel * 60;

                        //rtapi_print_msg(RTAPI_MSG_INFO,
                                //"vel %f delta_counts: %i delta_time %u period: %i timebase: %i\n", vel,delta_counts,delta_time,period,timebase);

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

                        if(cntr->counts_since_timeout < 2){
                                cntr->counts_since_timeout++;
                        }else{
                                *(cntr->vel) = vel;
                        }

                }else{
                        // No updates, see if we shoudl timeout
                        if(cntr->counts_since_timeout){
                                delta_time = timebase - cntr->timestamp;
                                if(delta_time < 1e9 / (1 / cntr->scale)){
                                        // estimate
                                }else{
                                        cntr->counts_since_timeout = 0;
                                        *(cntr->vel) = 0;
                                }
                        }else{
                                // already timed out;
                                *(cntr->vel) = 0;
                                //cntr->vel_rpm = 0;
                        }

                }

                // 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;
    }
    */
    rtapi_print("Enc Vel export RPM");
    retval = hal_pin_float_newf(HAL_OUT, &(addr->vel_rpm), comp_id,
                    "%s.velocity_rpm",prefix);
    if(retval != 0){
                rtapi_print("Enc Vel vel_rpm probelm");
            return retval;
    }
    rtapi_print("Enc Vel rpm done");

   /* export pin for scaled velocity captured by capture() */
    retval = hal_pin_float_newf(HAL_OUT, &(addr->vel), comp_id,
            "%s.velocity", 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 raw_count_in ");
        return retval;
   }
        /*
   retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id,
         "%s.scale",prefix);
   if (retval != 0){
        rtapi_print("Enc Vel in probelm");
        return retval;
   }
        */

   return 0;
}

hal code

#Encoder RIO play
#

loadrt xenc_vel num_chan=1

addf enc_vel.update-counters servo-thread

net  raw_count_in rio.spindle_encoder-s32 => encoder.0.raw_count_in

net spindle-velocity encoder.0.velocity => spindle.0.speed-in
multigcs commented 4 months ago

rio can not build external hal-component's, in this case, you can add install your hal-component with halcompile and add your hal code to 'postgui_call_list.hal'

but you can use your code inside the quadencoderz plugin or make a new one:

cp -a riocore/plugins/quadencoderz/ riocore/plugins/quadencoderz_jschoch/

and change the plugin.pyy file to something like this (not working / have to fix some variables)

from riocore.plugins import PluginBase

class Plugin(PluginBase):

    my_vel_code = """
    static uint32_t timestamp = 0;
    static uint32_t prev_raw_count = 0;
    if(  prev_raw_count != value) {
        // encoder changed, do some updates

        //delta_time = buf->timestamp - timestamp;
        delta_time = current_time - timestamp;

        //delta_counts = buf->raw_count - cntr->raw_count;
        delta_counts = prev_raw_count - value;
        timestamp = current_time;  // Get timestamp in seconds

        // count is usually 1 or -1 since this is wired to the encoder's raw out
        //  this uses the encoder's ppr to extrapolate revolutions per second
        // nanoseconds converted to seconds via 1e-9
        vel = delta_counts * (1.0/ (delta_time * 1e-9 * 360));
        *(cntr->vel_rpm) = vel * 60;

        //rtapi_print_msg(RTAPI_MSG_INFO,
        //"vel %f delta_counts: %i delta_time %u period: %i timebase: %i\\n", vel,delta_counts,delta_time,period,timebase);
        *(cntr->updates)= *(cntr->updates) + 1;
        prev_raw_count = value;

        if(cntr->counts_since_timeout < 2) {
            cntr->counts_since_timeout++;
        } else {
            cntr->vel = vel;
        }

    } else {
        // No updates, see if we shoudl timeout
        if(cntr->counts_since_timeout) {
            delta_time = timebase - timestamp;
            if(delta_time < 1e9 / (1 / cntr->scale)) {
                // estimate
            } else {
                cntr->counts_since_timeout = 0;
                cntr->vel = 0;
            }
        } else {
            // already timed out;
            cntr->vel = 0;
            //cntr->vel_rpm = 0;
        }
    }

    // update main timestamp thing
    timebase += period;
    """

    def setup(self):
        self.NAME = "quadencoderz_jschoch"
        self.VERILOGS = ["quadencoderz.v"]
        self.PINDEFAULTS = {
            "a": {
                "direction": "input",
                "invert": False,
                "pullup": False,
            },
            "b": {
                "direction": "input",
                "invert": False,
                "pullup": False,
            },
            "z": {
                "description": "index pin",
                "direction": "input",
                "invert": False,
                "pullup": False,
            },
        }
        self.OPTIONS = {
            "quad_type": {
                "default": 2,
                "type": int,
                "min": 1,
                "max": 4,
                "description": "encoder type",
            },
        }
        self.INTERFACE = {
            "indexenable": {
                "size": 1,
                "direction": "output",
            },
            "indexout": {
                "size": 1,
                "direction": "input",
            },
            "position": {
                "size": 32,
                "direction": "input",
            },
        }
        self.SIGNALS = {
            "indexenable": {
                "is_index_enable": True,
                "direction": "inout",
                "bool": True,
            },
            "indexout": {
                "is_index_out": True,
                "direction": "input",
                "bool": True,
            },
            "position": {
                "is_index_position": True,
                "direction": "input",
                "targets": {
                    "vel": self.my_vel_code,
                    "vel_rpm": "value_vel_rpm = value_vel * 60.0;",
                },
                "description": "position feedback in steps",
            },
            "rps": {
                "direction": "input",
                "source": "position",
                "description": "calculates revolutions per second",
            },
        }
        self.INFO = "quadencoder with index pin"
        self.DESCRIPTION = "usable as spindle-encoder for rigid tapping and thread cutting"

    def gateware_instances(self):
        instances = self.gateware_instances_base()
        instance = instances[self.instances_name]
        instance_predefines = instance["predefines"]
        instance_parameter = instance["parameter"]
        instance_arguments = instance["arguments"]
        quad_type = self.plugin_setup.get("quad_type", self.OPTIONS["quad_type"]["default"])
        instance_parameter["QUAD_TYPE"] = quad_type
        return instances

rename the pluginname in your config, generate and look at rio.c

multigcs commented 4 months ago

sorry, forget to rename the signal names:

        self.SIGNALS = {
.....
            "vel": {
                "direction": "input",
                "source": "position",
                "description": "calculates revolutions per second",
            },
            "vel_rpm": {
                "direction": "input",
                "source": "position",
                "description": "calculates revolutions per minute",
            },
        }