// Generated by rio-generator #include #include #include #include #include #include #include #include #include #include #include #include #include MODULE_AUTHOR("Oliver Dippel"); MODULE_DESCRIPTION("Driver for RIO FPGA boards"); MODULE_LICENSE("GPL v2"); #define MODNAME "rio" #define PREFIX "rio" #define JOINTS 3 #define BUFFER_SIZE 32 #define OSC_CLOCK 54000000 #define UDP_IP "192.168.10.194" #define SRC_PORT 2391 #define DST_PORT 2390 #define SERIAL_PORT "/dev/ttyUSB1" #define SERIAL_BAUD B1000000 #define SPI_PIN_MOSI 10 #define SPI_PIN_MISO 9 #define SPI_PIN_CLK 11 #define SPI_PIN_CS 8 #define SPI_SPEED BCM2835_SPI_CLOCK_DIVIDER_256 static int comp_id; static const char *modname = MODNAME; static const char *prefix = PREFIX; uint32_t pkg_counter = 0; uint32_t err_total = 0; uint32_t err_counter = 0; long stamp_last = 0; float fpga_stamp_last = 0; uint32_t fpga_timestamp = 0; void rio_readwrite(void *inst, long period); int error_handler(int retval); typedef struct { // hal variables hal_bit_t *sys_enable; hal_bit_t *sys_enable_request; hal_bit_t *sys_status; hal_bit_t *machine_on; hal_bit_t *sys_simulation; hal_u32_t *fpga_timestamp; hal_float_t *duration; hal_float_t *SIGOUT_STEPPER_0_VELOCITY; hal_float_t *SIGOUT_STEPPER_0_VELOCITY_SCALE; hal_float_t *SIGOUT_STEPPER_0_VELOCITY_OFFSET; hal_float_t *SIGIN_STEPPER_0_POSITION; hal_float_t *SIGIN_STEPPER_0_POSITION_ABS; hal_s32_t *SIGIN_STEPPER_0_POSITION_S32; hal_u32_t *SIGIN_STEPPER_0_POSITION_U32_ABS; hal_float_t *SIGIN_STEPPER_0_POSITION_SCALE; hal_float_t *SIGIN_STEPPER_0_POSITION_OFFSET; hal_bit_t *SIGOUT_STEPPER_0_ENABLE; hal_float_t *SIGOUT_STEPPER_1_VELOCITY; hal_float_t *SIGOUT_STEPPER_1_VELOCITY_SCALE; hal_float_t *SIGOUT_STEPPER_1_VELOCITY_OFFSET; hal_float_t *SIGIN_STEPPER_1_POSITION; hal_float_t *SIGIN_STEPPER_1_POSITION_ABS; hal_s32_t *SIGIN_STEPPER_1_POSITION_S32; hal_u32_t *SIGIN_STEPPER_1_POSITION_U32_ABS; hal_float_t *SIGIN_STEPPER_1_POSITION_SCALE; hal_float_t *SIGIN_STEPPER_1_POSITION_OFFSET; hal_bit_t *SIGOUT_STEPPER_1_ENABLE; hal_float_t *SIGOUT_STEPPER_2_VELOCITY; hal_float_t *SIGOUT_STEPPER_2_VELOCITY_SCALE; hal_float_t *SIGOUT_STEPPER_2_VELOCITY_OFFSET; hal_float_t *SIGIN_STEPPER_2_POSITION; hal_float_t *SIGIN_STEPPER_2_POSITION_ABS; hal_s32_t *SIGIN_STEPPER_2_POSITION_S32; hal_u32_t *SIGIN_STEPPER_2_POSITION_U32_ABS; hal_float_t *SIGIN_STEPPER_2_POSITION_SCALE; hal_float_t *SIGIN_STEPPER_2_POSITION_OFFSET; hal_bit_t *SIGOUT_STEPPER_2_ENABLE; hal_float_t *SIGOUT_STEPPER_3_VELOCITY; hal_float_t *SIGOUT_STEPPER_3_VELOCITY_SCALE; hal_float_t *SIGOUT_STEPPER_3_VELOCITY_OFFSET; hal_float_t *SIGIN_STEPPER_3_POSITION; hal_float_t *SIGIN_STEPPER_3_POSITION_ABS; hal_s32_t *SIGIN_STEPPER_3_POSITION_S32; hal_u32_t *SIGIN_STEPPER_3_POSITION_U32_ABS; hal_float_t *SIGIN_STEPPER_3_POSITION_SCALE; hal_float_t *SIGIN_STEPPER_3_POSITION_OFFSET; hal_bit_t *SIGOUT_STEPPER_3_ENABLE; hal_float_t *SIGOUT_STEPPER_4_VELOCITY; hal_float_t *SIGOUT_STEPPER_4_VELOCITY_SCALE; hal_float_t *SIGOUT_STEPPER_4_VELOCITY_OFFSET; hal_float_t *SIGIN_STEPPER_4_POSITION; hal_float_t *SIGIN_STEPPER_4_POSITION_ABS; hal_s32_t *SIGIN_STEPPER_4_POSITION_S32; hal_u32_t *SIGIN_STEPPER_4_POSITION_U32_ABS; hal_float_t *SIGIN_STEPPER_4_POSITION_SCALE; hal_float_t *SIGIN_STEPPER_4_POSITION_OFFSET; hal_bit_t *SIGOUT_STEPPER_4_ENABLE; hal_float_t *SIGOUT_STEPPER_5_VELOCITY; hal_float_t *SIGOUT_STEPPER_5_VELOCITY_SCALE; hal_float_t *SIGOUT_STEPPER_5_VELOCITY_OFFSET; hal_float_t *SIGIN_STEPPER_5_POSITION; hal_float_t *SIGIN_STEPPER_5_POSITION_ABS; hal_s32_t *SIGIN_STEPPER_5_POSITION_S32; hal_u32_t *SIGIN_STEPPER_5_POSITION_U32_ABS; hal_float_t *SIGIN_STEPPER_5_POSITION_SCALE; hal_float_t *SIGIN_STEPPER_5_POSITION_OFFSET; hal_bit_t *SIGOUT_STEPPER_5_ENABLE; // raw variables int32_t VAROUT32_STEPDIR0_VELOCITY; int32_t VARIN32_STEPDIR0_POSITION; int32_t VAROUT32_STEPDIR1_VELOCITY; int32_t VARIN32_STEPDIR1_POSITION; int32_t VAROUT32_STEPDIR2_VELOCITY; int32_t VARIN32_STEPDIR2_POSITION; int32_t VAROUT32_STEPDIR3_VELOCITY; int32_t VARIN32_STEPDIR3_POSITION; int32_t VAROUT32_STEPDIR4_VELOCITY; int32_t VARIN32_STEPDIR4_POSITION; int32_t VAROUT32_STEPDIR5_VELOCITY; int32_t VARIN32_STEPDIR5_POSITION; bool VAROUT1_STEPDIR0_ENABLE; bool VAROUT1_STEPDIR1_ENABLE; bool VAROUT1_STEPDIR2_ENABLE; bool VAROUT1_STEPDIR3_ENABLE; bool VAROUT1_STEPDIR4_ENABLE; bool VAROUT1_STEPDIR5_ENABLE; } data_t; static data_t *data; void register_signals(void) { int retval = 0; data->VAROUT32_STEPDIR0_VELOCITY = 0; data->VARIN32_STEPDIR0_POSITION = 0; data->VAROUT32_STEPDIR1_VELOCITY = 0; data->VARIN32_STEPDIR1_POSITION = 0; data->VAROUT32_STEPDIR2_VELOCITY = 0; data->VARIN32_STEPDIR2_POSITION = 0; data->VAROUT32_STEPDIR3_VELOCITY = 0; data->VARIN32_STEPDIR3_POSITION = 0; data->VAROUT32_STEPDIR4_VELOCITY = 0; data->VARIN32_STEPDIR4_POSITION = 0; data->VAROUT32_STEPDIR5_VELOCITY = 0; data->VARIN32_STEPDIR5_POSITION = 0; data->VAROUT1_STEPDIR0_ENABLE = 0; data->VAROUT1_STEPDIR1_ENABLE = 0; data->VAROUT1_STEPDIR2_ENABLE = 0; data->VAROUT1_STEPDIR3_ENABLE = 0; data->VAROUT1_STEPDIR4_ENABLE = 0; data->VAROUT1_STEPDIR5_ENABLE = 0; if (retval = hal_pin_bit_newf(HAL_OUT, &(data->sys_status), comp_id, "%s.sys-status", prefix) != 0) error_handler(retval); if (retval = hal_pin_bit_newf(HAL_IN, &(data->sys_enable), comp_id, "%s.sys-enable", prefix) != 0) error_handler(retval); if (retval = hal_pin_bit_newf(HAL_IN, &(data->sys_enable_request), comp_id, "%s.sys-enable-request", prefix) != 0) error_handler(retval); if (retval = hal_pin_bit_newf(HAL_IN, &(data->machine_on), comp_id, "%s.machine-on", prefix) != 0) error_handler(retval); if (retval = hal_pin_bit_newf(HAL_IN, &(data->sys_simulation), comp_id, "%s.sys-simulation", prefix) != 0) error_handler(retval); if (retval = hal_pin_float_newf(HAL_OUT, &(data->duration), comp_id, "%s.duration", prefix) != 0) error_handler(retval); *data->duration = rtapi_get_time(); if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_0_VELOCITY_SCALE), comp_id, "%s.stepper_0.velocity-scale", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_0_VELOCITY_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_0_VELOCITY_OFFSET), comp_id, "%s.stepper_0.velocity-offset", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_0_VELOCITY_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_0_VELOCITY), comp_id, "%s.stepper_0.velocity", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_0_VELOCITY = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_0_POSITION_SCALE), comp_id, "%s.stepper_0.position-scale", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_0_POSITION_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_0_POSITION_OFFSET), comp_id, "%s.stepper_0.position-offset", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_0_POSITION_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_0_POSITION), comp_id, "%s.stepper_0.position", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_0_POSITION = 0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_0_POSITION_ABS), comp_id, "%s.stepper_0.position-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_0_POSITION_ABS = 0; if (retval = hal_pin_s32_newf(HAL_OUT, &(data->SIGIN_STEPPER_0_POSITION_S32), comp_id, "%s.stepper_0.position-s32", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_0_POSITION_S32 = 0; if (retval = hal_pin_u32_newf(HAL_OUT, &(data->SIGIN_STEPPER_0_POSITION_U32_ABS), comp_id, "%s.stepper_0.position-u32-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_0_POSITION_U32_ABS = 0; if (retval = hal_pin_bit_newf(HAL_IN, &(data->SIGOUT_STEPPER_0_ENABLE), comp_id, "%s.stepper_0.enable", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_0_ENABLE = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_1_VELOCITY_SCALE), comp_id, "%s.stepper_1.velocity-scale", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_1_VELOCITY_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_1_VELOCITY_OFFSET), comp_id, "%s.stepper_1.velocity-offset", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_1_VELOCITY_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_1_VELOCITY), comp_id, "%s.stepper_1.velocity", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_1_VELOCITY = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_1_POSITION_SCALE), comp_id, "%s.stepper_1.position-scale", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_1_POSITION_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_1_POSITION_OFFSET), comp_id, "%s.stepper_1.position-offset", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_1_POSITION_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_1_POSITION), comp_id, "%s.stepper_1.position", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_1_POSITION = 0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_1_POSITION_ABS), comp_id, "%s.stepper_1.position-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_1_POSITION_ABS = 0; if (retval = hal_pin_s32_newf(HAL_OUT, &(data->SIGIN_STEPPER_1_POSITION_S32), comp_id, "%s.stepper_1.position-s32", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_1_POSITION_S32 = 0; if (retval = hal_pin_u32_newf(HAL_OUT, &(data->SIGIN_STEPPER_1_POSITION_U32_ABS), comp_id, "%s.stepper_1.position-u32-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_1_POSITION_U32_ABS = 0; if (retval = hal_pin_bit_newf(HAL_IN, &(data->SIGOUT_STEPPER_1_ENABLE), comp_id, "%s.stepper_1.enable", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_1_ENABLE = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_2_VELOCITY_SCALE), comp_id, "%s.stepper_2.velocity-scale", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_2_VELOCITY_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_2_VELOCITY_OFFSET), comp_id, "%s.stepper_2.velocity-offset", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_2_VELOCITY_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_2_VELOCITY), comp_id, "%s.stepper_2.velocity", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_2_VELOCITY = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_2_POSITION_SCALE), comp_id, "%s.stepper_2.position-scale", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_2_POSITION_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_2_POSITION_OFFSET), comp_id, "%s.stepper_2.position-offset", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_2_POSITION_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_2_POSITION), comp_id, "%s.stepper_2.position", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_2_POSITION = 0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_2_POSITION_ABS), comp_id, "%s.stepper_2.position-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_2_POSITION_ABS = 0; if (retval = hal_pin_s32_newf(HAL_OUT, &(data->SIGIN_STEPPER_2_POSITION_S32), comp_id, "%s.stepper_2.position-s32", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_2_POSITION_S32 = 0; if (retval = hal_pin_u32_newf(HAL_OUT, &(data->SIGIN_STEPPER_2_POSITION_U32_ABS), comp_id, "%s.stepper_2.position-u32-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_2_POSITION_U32_ABS = 0; if (retval = hal_pin_bit_newf(HAL_IN, &(data->SIGOUT_STEPPER_2_ENABLE), comp_id, "%s.stepper_2.enable", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_2_ENABLE = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_3_VELOCITY_SCALE), comp_id, "%s.stepper_3.velocity-scale", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_3_VELOCITY_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_3_VELOCITY_OFFSET), comp_id, "%s.stepper_3.velocity-offset", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_3_VELOCITY_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_3_VELOCITY), comp_id, "%s.stepper_3.velocity", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_3_VELOCITY = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_3_POSITION_SCALE), comp_id, "%s.stepper_3.position-scale", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_3_POSITION_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_3_POSITION_OFFSET), comp_id, "%s.stepper_3.position-offset", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_3_POSITION_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_3_POSITION), comp_id, "%s.stepper_3.position", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_3_POSITION = 0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_3_POSITION_ABS), comp_id, "%s.stepper_3.position-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_3_POSITION_ABS = 0; if (retval = hal_pin_s32_newf(HAL_OUT, &(data->SIGIN_STEPPER_3_POSITION_S32), comp_id, "%s.stepper_3.position-s32", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_3_POSITION_S32 = 0; if (retval = hal_pin_u32_newf(HAL_OUT, &(data->SIGIN_STEPPER_3_POSITION_U32_ABS), comp_id, "%s.stepper_3.position-u32-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_3_POSITION_U32_ABS = 0; if (retval = hal_pin_bit_newf(HAL_IN, &(data->SIGOUT_STEPPER_3_ENABLE), comp_id, "%s.stepper_3.enable", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_3_ENABLE = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_4_VELOCITY_SCALE), comp_id, "%s.stepper_4.velocity-scale", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_4_VELOCITY_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_4_VELOCITY_OFFSET), comp_id, "%s.stepper_4.velocity-offset", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_4_VELOCITY_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_4_VELOCITY), comp_id, "%s.stepper_4.velocity", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_4_VELOCITY = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_4_POSITION_SCALE), comp_id, "%s.stepper_4.position-scale", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_4_POSITION_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_4_POSITION_OFFSET), comp_id, "%s.stepper_4.position-offset", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_4_POSITION_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_4_POSITION), comp_id, "%s.stepper_4.position", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_4_POSITION = 0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_4_POSITION_ABS), comp_id, "%s.stepper_4.position-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_4_POSITION_ABS = 0; if (retval = hal_pin_s32_newf(HAL_OUT, &(data->SIGIN_STEPPER_4_POSITION_S32), comp_id, "%s.stepper_4.position-s32", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_4_POSITION_S32 = 0; if (retval = hal_pin_u32_newf(HAL_OUT, &(data->SIGIN_STEPPER_4_POSITION_U32_ABS), comp_id, "%s.stepper_4.position-u32-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_4_POSITION_U32_ABS = 0; if (retval = hal_pin_bit_newf(HAL_IN, &(data->SIGOUT_STEPPER_4_ENABLE), comp_id, "%s.stepper_4.enable", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_4_ENABLE = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_5_VELOCITY_SCALE), comp_id, "%s.stepper_5.velocity-scale", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_5_VELOCITY_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_5_VELOCITY_OFFSET), comp_id, "%s.stepper_5.velocity-offset", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_5_VELOCITY_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGOUT_STEPPER_5_VELOCITY), comp_id, "%s.stepper_5.velocity", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_5_VELOCITY = 0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_5_POSITION_SCALE), comp_id, "%s.stepper_5.position-scale", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_5_POSITION_SCALE = 1.0; if (retval = hal_pin_float_newf(HAL_IN, &(data->SIGIN_STEPPER_5_POSITION_OFFSET), comp_id, "%s.stepper_5.position-offset", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_5_POSITION_OFFSET = 0.0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_5_POSITION), comp_id, "%s.stepper_5.position", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_5_POSITION = 0; if (retval = hal_pin_float_newf(HAL_OUT, &(data->SIGIN_STEPPER_5_POSITION_ABS), comp_id, "%s.stepper_5.position-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_5_POSITION_ABS = 0; if (retval = hal_pin_s32_newf(HAL_OUT, &(data->SIGIN_STEPPER_5_POSITION_S32), comp_id, "%s.stepper_5.position-s32", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_5_POSITION_S32 = 0; if (retval = hal_pin_u32_newf(HAL_OUT, &(data->SIGIN_STEPPER_5_POSITION_U32_ABS), comp_id, "%s.stepper_5.position-u32-abs", prefix) != 0) error_handler(retval); *data->SIGIN_STEPPER_5_POSITION_U32_ABS = 0; if (retval = hal_pin_bit_newf(HAL_IN, &(data->SIGOUT_STEPPER_5_ENABLE), comp_id, "%s.stepper_5.enable", prefix) != 0) error_handler(retval); *data->SIGOUT_STEPPER_5_ENABLE = 0; } /* interface: SPI */ #include #include int spifd; static uint8_t mode = SPI_MODE_0; static uint8_t bits = 8; static uint32_t speed = 3000000; int spi_init(void) { rtapi_print("Info: Initialize SPI5 connection\n"); spifd = open("/dev/spidev0.0", O_RDWR); if (spifd < 0) { rtapi_print_msg(RTAPI_MSG_ERR,"Failed to open SPI device\n"); return -1; } // Set SPI mode if (ioctl(spifd, SPI_IOC_WR_MODE, &mode) == -1) { rtapi_print_msg(RTAPI_MSG_ERR,"Failed to set SPI mode\n"); close(spifd); return -1; } // Set bits per word if (ioctl(spifd, SPI_IOC_WR_BITS_PER_WORD, &bits) == -1) { rtapi_print_msg(RTAPI_MSG_ERR,"Failed to set bits per word\n"); close(spifd); return -1; } // Set max speed if (ioctl(spifd, SPI_IOC_WR_MAX_SPEED_HZ, &speed) == -1) { rtapi_print_msg(RTAPI_MSG_ERR,"Failed to set max speed\n"); close(spifd); return -1; } } void spi_exit(void) { close(spifd); } int spi_trx(uint8_t *txBuffer, uint8_t *rxBuffer, uint16_t size) { struct spi_ioc_transfer tr = { .tx_buf = (uint64_t)txBuffer, .rx_buf = (uint64_t)rxBuffer, .len = size, .speed_hz = speed, .delay_usecs = 0, .bits_per_word = bits, }; // Perform SPI transfer if (ioctl(spifd, SPI_IOC_MESSAGE(1), &tr) == -1) { rtapi_print_msg(RTAPI_MSG_ERR,"Failed to perform SPI transfer\n"); } return 1; } int interface_init(void) { spi_init(); return 0; } void interface_exit(void) { spi_exit(); } /* hal functions */ /*********************************************************************** * HELPER FUNCTIONS * ************************************************************************/ uint16_t crc16_update(uint16_t crc, uint8_t a) { int i; crc ^= (uint16_t)a; for (i = 0; i < 8; ++i) { if (crc & 1) crc = (crc >> 1) ^ 0xA001; else crc = (crc >> 1); } return crc; } int error_handler(int retval) { if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: pin export failed with err=%i\n", modname, retval); hal_exit(comp_id); return -1; } } int rtapi_app_main(void) { char name[HAL_NAME_LEN + 1]; int retval = 0; int n = 0; data = hal_malloc(sizeof(data_t)); comp_id = hal_init(modname); if (comp_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s ERROR: hal_init() failed \n", modname); return -1; } register_signals(); rtapi_snprintf(name, sizeof(name), "%s.update-freq", prefix); rtapi_snprintf(name, sizeof(name), "%s.readwrite", prefix); retval = hal_export_funct(name, rio_readwrite, data, 1, 0, comp_id); if (retval < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: read function export failed\n", modname); hal_exit(comp_id); return -1; } rtapi_print_msg(RTAPI_MSG_INFO, "%s: installed driver\n", modname); hal_ready(comp_id); interface_init(); rio_readwrite(NULL, 0); return 0; } void rtapi_app_exit(void) { interface_exit(); hal_exit(comp_id); } /***********************************************************************/ /*********************************************************************** * PLUGIN GLOBALS * ************************************************************************/ /***********************************************************************/ // Generated by component_signal_converter() // output: SIGOUT -> calc -> VAROUT -> txBuffer void convert_varout32_stepdir0_velocity(data_t *data){ float value = *data->SIGOUT_STEPPER_0_VELOCITY; value = value * *data->SIGOUT_STEPPER_0_VELOCITY_SCALE; value = value + *data->SIGOUT_STEPPER_0_VELOCITY_OFFSET; if (value != 0) { value = OSC_CLOCK / value / 2; } data->VAROUT32_STEPDIR0_VELOCITY = value; } void convert_varout1_stepdir0_enable(data_t *data){ bool value = *data->SIGOUT_STEPPER_0_ENABLE; data->VAROUT1_STEPDIR0_ENABLE = value; } void convert_varout32_stepdir1_velocity(data_t *data){ float value = *data->SIGOUT_STEPPER_1_VELOCITY; value = value * *data->SIGOUT_STEPPER_1_VELOCITY_SCALE; value = value + *data->SIGOUT_STEPPER_1_VELOCITY_OFFSET; if (value != 0) { value = OSC_CLOCK / value / 2; } data->VAROUT32_STEPDIR1_VELOCITY = value; } void convert_varout1_stepdir1_enable(data_t *data){ bool value = *data->SIGOUT_STEPPER_1_ENABLE; data->VAROUT1_STEPDIR1_ENABLE = value; } void convert_varout32_stepdir2_velocity(data_t *data){ float value = *data->SIGOUT_STEPPER_2_VELOCITY; value = value * *data->SIGOUT_STEPPER_2_VELOCITY_SCALE; value = value + *data->SIGOUT_STEPPER_2_VELOCITY_OFFSET; if (value != 0) { value = OSC_CLOCK / value / 2; } data->VAROUT32_STEPDIR2_VELOCITY = value; } void convert_varout1_stepdir2_enable(data_t *data){ bool value = *data->SIGOUT_STEPPER_2_ENABLE; data->VAROUT1_STEPDIR2_ENABLE = value; } void convert_varout32_stepdir3_velocity(data_t *data){ float value = *data->SIGOUT_STEPPER_3_VELOCITY; value = value * *data->SIGOUT_STEPPER_3_VELOCITY_SCALE; value = value + *data->SIGOUT_STEPPER_3_VELOCITY_OFFSET; if (value != 0) { value = OSC_CLOCK / value / 2; } data->VAROUT32_STEPDIR3_VELOCITY = value; } void convert_varout1_stepdir3_enable(data_t *data){ bool value = *data->SIGOUT_STEPPER_3_ENABLE; data->VAROUT1_STEPDIR3_ENABLE = value; } void convert_varout32_stepdir4_velocity(data_t *data){ float value = *data->SIGOUT_STEPPER_4_VELOCITY; value = value * *data->SIGOUT_STEPPER_4_VELOCITY_SCALE; value = value + *data->SIGOUT_STEPPER_4_VELOCITY_OFFSET; if (value != 0) { value = OSC_CLOCK / value / 2; } data->VAROUT32_STEPDIR4_VELOCITY = value; } void convert_varout1_stepdir4_enable(data_t *data){ bool value = *data->SIGOUT_STEPPER_4_ENABLE; data->VAROUT1_STEPDIR4_ENABLE = value; } void convert_varout32_stepdir5_velocity(data_t *data){ float value = *data->SIGOUT_STEPPER_5_VELOCITY; value = value * *data->SIGOUT_STEPPER_5_VELOCITY_SCALE; value = value + *data->SIGOUT_STEPPER_5_VELOCITY_OFFSET; if (value != 0) { value = OSC_CLOCK / value / 2; } data->VAROUT32_STEPDIR5_VELOCITY = value; } void convert_varout1_stepdir5_enable(data_t *data){ bool value = *data->SIGOUT_STEPPER_5_ENABLE; data->VAROUT1_STEPDIR5_ENABLE = value; } // input: rxBuffer -> VAROUT -> calc -> SIGOUT void convert_sigin_stepper_0_position(data_t *data) { float value = data->VARIN32_STEPDIR0_POSITION; float offset = *data->SIGIN_STEPPER_0_POSITION_OFFSET; float scale = *data->SIGIN_STEPPER_0_POSITION_SCALE; float last_value = *data->SIGIN_STEPPER_0_POSITION; static float last_raw_value = 0.0; float raw_value = value; value = value + offset; value = value / scale; if (*data->sys_simulation == 1) { value = *data->SIGIN_STEPPER_0_POSITION + *data->SIGOUT_STEPPER_0_VELOCITY / 1000.0; } *data->SIGIN_STEPPER_0_POSITION_ABS = abs(value); *data->SIGIN_STEPPER_0_POSITION_S32 = value; *data->SIGIN_STEPPER_0_POSITION_U32_ABS = abs(value); *data->SIGIN_STEPPER_0_POSITION = value; last_raw_value = raw_value; } void convert_sigin_stepper_1_position(data_t *data) { float value = data->VARIN32_STEPDIR1_POSITION; float offset = *data->SIGIN_STEPPER_1_POSITION_OFFSET; float scale = *data->SIGIN_STEPPER_1_POSITION_SCALE; float last_value = *data->SIGIN_STEPPER_1_POSITION; static float last_raw_value = 0.0; float raw_value = value; value = value + offset; value = value / scale; if (*data->sys_simulation == 1) { value = *data->SIGIN_STEPPER_1_POSITION + *data->SIGOUT_STEPPER_1_VELOCITY / 1000.0; } *data->SIGIN_STEPPER_1_POSITION_ABS = abs(value); *data->SIGIN_STEPPER_1_POSITION_S32 = value; *data->SIGIN_STEPPER_1_POSITION_U32_ABS = abs(value); *data->SIGIN_STEPPER_1_POSITION = value; last_raw_value = raw_value; } void convert_sigin_stepper_2_position(data_t *data) { float value = data->VARIN32_STEPDIR2_POSITION; float offset = *data->SIGIN_STEPPER_2_POSITION_OFFSET; float scale = *data->SIGIN_STEPPER_2_POSITION_SCALE; float last_value = *data->SIGIN_STEPPER_2_POSITION; static float last_raw_value = 0.0; float raw_value = value; value = value + offset; value = value / scale; if (*data->sys_simulation == 1) { value = *data->SIGIN_STEPPER_2_POSITION + *data->SIGOUT_STEPPER_2_VELOCITY / 1000.0; } *data->SIGIN_STEPPER_2_POSITION_ABS = abs(value); *data->SIGIN_STEPPER_2_POSITION_S32 = value; *data->SIGIN_STEPPER_2_POSITION_U32_ABS = abs(value); *data->SIGIN_STEPPER_2_POSITION = value; last_raw_value = raw_value; } void convert_sigin_stepper_3_position(data_t *data) { float value = data->VARIN32_STEPDIR3_POSITION; float offset = *data->SIGIN_STEPPER_3_POSITION_OFFSET; float scale = *data->SIGIN_STEPPER_3_POSITION_SCALE; float last_value = *data->SIGIN_STEPPER_3_POSITION; static float last_raw_value = 0.0; float raw_value = value; value = value + offset; value = value / scale; if (*data->sys_simulation == 1) { value = *data->SIGIN_STEPPER_3_POSITION + *data->SIGOUT_STEPPER_3_VELOCITY / 1000.0; } *data->SIGIN_STEPPER_3_POSITION_ABS = abs(value); *data->SIGIN_STEPPER_3_POSITION_S32 = value; *data->SIGIN_STEPPER_3_POSITION_U32_ABS = abs(value); *data->SIGIN_STEPPER_3_POSITION = value; last_raw_value = raw_value; } void convert_sigin_stepper_4_position(data_t *data) { float value = data->VARIN32_STEPDIR4_POSITION; float offset = *data->SIGIN_STEPPER_4_POSITION_OFFSET; float scale = *data->SIGIN_STEPPER_4_POSITION_SCALE; float last_value = *data->SIGIN_STEPPER_4_POSITION; static float last_raw_value = 0.0; float raw_value = value; value = value + offset; value = value / scale; if (*data->sys_simulation == 1) { value = *data->SIGIN_STEPPER_4_POSITION + *data->SIGOUT_STEPPER_4_VELOCITY / 1000.0; } *data->SIGIN_STEPPER_4_POSITION_ABS = abs(value); *data->SIGIN_STEPPER_4_POSITION_S32 = value; *data->SIGIN_STEPPER_4_POSITION_U32_ABS = abs(value); *data->SIGIN_STEPPER_4_POSITION = value; last_raw_value = raw_value; } void convert_sigin_stepper_5_position(data_t *data) { float value = data->VARIN32_STEPDIR5_POSITION; float offset = *data->SIGIN_STEPPER_5_POSITION_OFFSET; float scale = *data->SIGIN_STEPPER_5_POSITION_SCALE; float last_value = *data->SIGIN_STEPPER_5_POSITION; static float last_raw_value = 0.0; float raw_value = value; value = value + offset; value = value / scale; if (*data->sys_simulation == 1) { value = *data->SIGIN_STEPPER_5_POSITION + *data->SIGOUT_STEPPER_5_VELOCITY / 1000.0; } *data->SIGIN_STEPPER_5_POSITION_ABS = abs(value); *data->SIGIN_STEPPER_5_POSITION_S32 = value; *data->SIGIN_STEPPER_5_POSITION_U32_ABS = abs(value); *data->SIGIN_STEPPER_5_POSITION = value; last_raw_value = raw_value; } // Generated by component_buffer_converter() void convert_outputs(void) { // output loop: SIGOUT -> calc -> VAROUT -> txBuffer convert_varout32_stepdir0_velocity(data); convert_varout1_stepdir0_enable(data); convert_varout32_stepdir1_velocity(data); convert_varout1_stepdir1_enable(data); convert_varout32_stepdir2_velocity(data); convert_varout1_stepdir2_enable(data); convert_varout32_stepdir3_velocity(data); convert_varout1_stepdir3_enable(data); convert_varout32_stepdir4_velocity(data); convert_varout1_stepdir4_enable(data); convert_varout32_stepdir5_velocity(data); convert_varout1_stepdir5_enable(data); } void convert_inputs(void) { // input: rxBuffer -> VAROUT -> calc -> SIGOUT convert_sigin_stepper_0_position(data); convert_sigin_stepper_1_position(data); convert_sigin_stepper_2_position(data); convert_sigin_stepper_3_position(data); convert_sigin_stepper_4_position(data); convert_sigin_stepper_5_position(data); } // Generated by component_buffer() void write_txbuffer(uint8_t *txBuffer) { // PC -> FPGA (230 + 26) int i = 0; for (i = 0; i < BUFFER_SIZE; i++) { txBuffer[i] = 0; } // raw vars to txBuffer txBuffer[0] = 0x74; // 256 txBuffer[1] = 0x69; // 248 txBuffer[2] = 0x72; // 240 txBuffer[3] = 0x77; // 232 memcpy(&txBuffer[4], &data->VAROUT32_STEPDIR0_VELOCITY, 4); // 224 memcpy(&txBuffer[8], &data->VAROUT32_STEPDIR1_VELOCITY, 4); // 192 memcpy(&txBuffer[12], &data->VAROUT32_STEPDIR2_VELOCITY, 4); // 160 memcpy(&txBuffer[16], &data->VAROUT32_STEPDIR3_VELOCITY, 4); // 128 memcpy(&txBuffer[20], &data->VAROUT32_STEPDIR4_VELOCITY, 4); // 96 memcpy(&txBuffer[24], &data->VAROUT32_STEPDIR5_VELOCITY, 4); // 64 txBuffer[28] |= (data->VAROUT1_STEPDIR0_ENABLE<<7); // 32 txBuffer[28] |= (data->VAROUT1_STEPDIR1_ENABLE<<6); // 31 txBuffer[28] |= (data->VAROUT1_STEPDIR2_ENABLE<<5); // 30 txBuffer[28] |= (data->VAROUT1_STEPDIR3_ENABLE<<4); // 29 txBuffer[28] |= (data->VAROUT1_STEPDIR4_ENABLE<<3); // 28 txBuffer[28] |= (data->VAROUT1_STEPDIR5_ENABLE<<2); // 27 // FILL: 26 } void read_rxbuffer(uint8_t *rxBuffer) { // FPGA -> PC (256 + 0) // memcpy(&header, &rxBuffer[0], 4) // 256; memcpy(&fpga_timestamp, &rxBuffer[4], 4); // 224 memcpy(&data->VARIN32_STEPDIR0_POSITION, &rxBuffer[8], 4); // 192 memcpy(&data->VARIN32_STEPDIR1_POSITION, &rxBuffer[12], 4); // 160 memcpy(&data->VARIN32_STEPDIR2_POSITION, &rxBuffer[16], 4); // 128 memcpy(&data->VARIN32_STEPDIR3_POSITION, &rxBuffer[20], 4); // 96 memcpy(&data->VARIN32_STEPDIR4_POSITION, &rxBuffer[24], 4); // 64 memcpy(&data->VARIN32_STEPDIR5_POSITION, &rxBuffer[28], 4); // 32 // FILL: 0 } void rio_readwrite(void *inst, long period) { int ret = 0; uint8_t i = 0; uint8_t rxBuffer[BUFFER_SIZE * 2]; uint8_t txBuffer[BUFFER_SIZE * 2]; if (*data->sys_enable_request == 1) { *data->sys_status = 1; } long stamp_new = rtapi_get_time(); float duration2 = (stamp_new - stamp_last) / 1000.0; stamp_last = stamp_new; float timestamp = (float)fpga_timestamp / (float)OSC_CLOCK; *data->duration = timestamp - fpga_stamp_last; fpga_stamp_last = timestamp; if (*data->sys_enable == 1 && *data->sys_status == 1) { pkg_counter += 1; convert_outputs(); if (*data->sys_simulation != 1) { write_txbuffer(txBuffer); spi_trx(txBuffer, rxBuffer, BUFFER_SIZE); if (rxBuffer[0] == 97 && rxBuffer[1] == 116 && rxBuffer[2] == 97 && rxBuffer[3] == 100) { if (err_counter > 0) { err_counter = 0; rtapi_print("recovered..\n"); } read_rxbuffer(rxBuffer); convert_inputs(); } else { err_counter += 1; err_total += 1; rtapi_print("wronng data (%i/3): ", err_counter); for (i = 0; i < BUFFER_SIZE; i++) { rtapi_print("%d ",rxBuffer[i]); } rtapi_print("\n"); if (err_counter > 3) { rtapi_print("too many errors..\n"); *data->sys_status = 0; } } } else { convert_inputs(); } } else { *data->sys_status = 0; } }