Драйверы программы Machinekit для работы c RISC сопроцессором внутри процессоров Allwinner H2+, H3 и H5
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

arisc.stepgen.c 8.5KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326
  1. #include "rtapi.h"
  2. #include "rtapi_app.h"
  3. #include "hal.h"
  4. #include <math.h>
  5. #include <float.h>
  6. #include <stdlib.h>
  7. #include "rtapi_math.h"
  8. #include "allwinner_CPU.h"
  9. #include "msg_api.h"
  10. #include "gpio_api.h"
  11. #include "stepgen_api.h"
  12. #include "arisc.stepgen.h"
  13. #if !defined(TARGET_PLATFORM_ALLWINNER)
  14. //#error "This driver is for the Allwinner platform only"
  15. #endif
  16. MODULE_DESCRIPTION("stepgen driver for the Allwinner ARISC firmware");
  17. MODULE_LICENSE("GPL");
  18. #define g *sg[ch]
  19. #define gp sg[ch]
  20. static int32_t comp_id;
  21. static const uint8_t * comp_name = "arisc.stepgen";
  22. static uint8_t cpu_id = ALLWINNER_H3;
  23. static char *CPU = "H3";
  24. RTAPI_MP_STRING(CPU, "Allwinner CPU name");
  25. static char *ctrl_type = "";
  26. RTAPI_MP_STRING(ctrl_type, "channels control type, comma separated");
  27. static stepgen_ch_t *sg;
  28. static uint8_t sg_cnt = 0;
  29. static long last_period = 1000000;
  30. // TOOLS
  31. static void update_pos_scale(uint8_t ch)
  32. {
  33. if ( g.pos_scale < 0 || (g.pos_scale < 1e-20 && g.pos_scale > -1e-20) ) g.pos_scale = 1.0;
  34. }
  35. static void update_pins(uint8_t ch)
  36. {
  37. if ( g.step_port != gp.step_port_old ||
  38. g.step_pin != gp.step_pin_old ||
  39. g.step_inv != gp.step_inv_old )
  40. {
  41. if ( g.step_port < GPIO_PORTS_CNT && g.step_pin < GPIO_PINS_CNT )
  42. {
  43. stepgen_pin_setup(ch, STEP, g.step_port, g.step_pin, g.step_inv);
  44. }
  45. gp.step_port_old = g.step_port;
  46. gp.step_pin_old = g.step_pin;
  47. gp.step_inv_old = g.step_inv;
  48. }
  49. if ( g.dir_port != gp.dir_port_old ||
  50. g.dir_pin != gp.dir_pin_old ||
  51. g.dir_inv != gp.dir_inv_old )
  52. {
  53. if ( g.dir_port < GPIO_PORTS_CNT && g.dir_pin < GPIO_PINS_CNT )
  54. {
  55. stepgen_pin_setup(ch, DIR, g.dir_port, g.dir_pin, g.dir_inv);
  56. }
  57. gp.dir_port_old = g.dir_port;
  58. gp.dir_pin_old = g.dir_pin;
  59. gp.dir_inv_old = g.dir_inv;
  60. }
  61. }
  62. static void update_dir_times(uint8_t ch)
  63. {
  64. if ( g.dir_setup != gp.dir_setup_old ||
  65. g.dir_hold != gp.dir_hold_old )
  66. {
  67. if ( (g.dir_setup + g.dir_hold) > last_period )
  68. {
  69. g.dir_setup = last_period / 16;
  70. g.dir_hold = g.dir_setup;
  71. }
  72. stepgen_time_setup(ch, DIR, g.dir_setup, g.dir_hold);
  73. gp.dir_setup_old = g.dir_setup;
  74. gp.dir_hold_old = g.dir_hold;
  75. }
  76. }
  77. // HAL functions
  78. static void capture_pos(void *arg, long period)
  79. {
  80. static uint8_t ch;
  81. for ( ch = sg_cnt; ch--; )
  82. {
  83. if ( !g.enable ) continue;
  84. update_pos_scale(ch);
  85. g.counts = stepgen_pos_get(ch);
  86. if ( gp.ctrl_type ) // velocity mode
  87. {
  88. g.pos_fb = ((hal_float_t)g.counts) / g.pos_scale;
  89. }
  90. else // position mode
  91. {
  92. g.pos_fb = (g.counts == gp.step_pos_cmd) ?
  93. g.pos_cmd :
  94. ((hal_float_t)g.counts) / g.pos_scale ;
  95. }
  96. }
  97. }
  98. static void update_freq(void *arg, long period)
  99. {
  100. static uint8_t ch;
  101. static int32_t steps;
  102. static uint32_t step_space, step_period, step_period_min, step_len;
  103. static hal_float_t period_s;
  104. last_period = period;
  105. period_s = ((hal_float_t)period) / ((hal_float_t)1000000000);
  106. for ( ch = sg_cnt; ch--; )
  107. {
  108. if ( !g.enable ) { g.freq = 0; continue; }
  109. update_pins(ch);
  110. update_dir_times(ch);
  111. update_pos_scale(ch);
  112. if ( gp.ctrl_type ) // velocity mode
  113. {
  114. // stop any movement if velocity value is too small
  115. if ( g.vel_cmd < 1e-20 && g.vel_cmd > -1e-20 ) { g.freq = 0; continue; }
  116. g.freq = g.pos_scale * g.vel_cmd;
  117. gp.step_pos_cmd = g.counts + (int32_t)(g.freq * period_s);
  118. steps = gp.step_pos_cmd - g.counts;
  119. }
  120. else // position mode
  121. {
  122. gp.step_pos_cmd = (int32_t)(g.pos_scale * g.pos_cmd);
  123. steps = gp.step_pos_cmd - g.counts;
  124. // stop any movement if we have the nearest position to the target
  125. if ( !steps ) { g.freq = 0; continue; }
  126. g.freq = (g.pos_cmd - g.pos_fb) / period_s;
  127. }
  128. if ( steps )
  129. {
  130. step_period = ((uint32_t)period) / ((uint32_t)abs(steps));
  131. // use 50% duty if any of step timings == 0
  132. if ( !g.step_len || !g.step_space )
  133. {
  134. step_space = step_period / 2;
  135. step_len = step_space;
  136. }
  137. else // use step space/len parameters set by user
  138. {
  139. step_period_min = g.step_len + g.step_space;
  140. // limit the velocity using step space/len parameters
  141. // this limitation can cause "joint # following error"
  142. if ( step_period < step_period_min ) step_period = step_period_min;
  143. step_len = g.step_len;
  144. step_space = step_period - step_len;
  145. }
  146. stepgen_time_setup(ch, STEP, step_space, step_len);
  147. stepgen_task_add(ch, steps);
  148. }
  149. }
  150. }
  151. // INIT
  152. #define EXPORT_PIN(IO_TYPE,VAR_TYPE,VAL,NAME,DEFAULT) \
  153. r += hal_pin_##VAR_TYPE##_newf(IO_TYPE, &(gp.VAL), comp_id,\
  154. "%s.%d." NAME, comp_name, ch);\
  155. g.VAL = DEFAULT;
  156. #define EXPORT_FUNC(FUNC,NAME,FP) \
  157. hal_export_functf(FUNC, 0, FP, 0, comp_id, "%s."NAME, comp_name)
  158. #define PRINT_ERROR(MSG) \
  159. rtapi_print_msg(RTAPI_MSG_ERR, "%s: "MSG"\n", comp_name)
  160. #define PRINT_ERROR_AND_RETURN(MSG,RETVAL) \
  161. { PRINT_ERROR(MSG); return RETVAL; }
  162. static int32_t malloc_and_export(const char *comp_name, int32_t comp_id)
  163. {
  164. int32_t r, ch;
  165. int8_t *data = ctrl_type, *token, type[STEPGEN_CH_CNT_MAX] = {0};
  166. // get channels count and type
  167. while ( (token = strtok(data, ",")) != NULL )
  168. {
  169. if ( data != NULL ) data = NULL;
  170. if ( token[0] == 'P' || token[0] == 'p' ) type[sg_cnt++] = 0;
  171. else if ( token[0] == 'V' || token[0] == 'v' ) type[sg_cnt++] = 1;
  172. }
  173. if ( !sg_cnt ) return 0;
  174. if ( sg_cnt > STEPGEN_CH_CNT_MAX ) sg_cnt = STEPGEN_CH_CNT_MAX;
  175. // shared memory allocation
  176. sg = hal_malloc(sg_cnt * sizeof(stepgen_ch_t));
  177. if ( !sg ) PRINT_ERROR_AND_RETURN("hal_malloc() failed", -1);
  178. // export HAL pins and set default values
  179. for ( r = 0, ch = sg_cnt; ch--; )
  180. {
  181. EXPORT_PIN(HAL_IN,bit,enable,"enable", 0);
  182. EXPORT_PIN(HAL_IN,u32,step_space,"stepspace", 0);
  183. EXPORT_PIN(HAL_IN,u32,step_len,"steplen", 0);
  184. EXPORT_PIN(HAL_IN,u32,dir_setup,"dirsetup", 1000);
  185. EXPORT_PIN(HAL_IN,u32,dir_hold,"dirhold", 1000);
  186. EXPORT_PIN(HAL_IN,u32,step_port,"step-port", UINT32_MAX);
  187. EXPORT_PIN(HAL_IN,u32,step_pin,"step-pin", UINT32_MAX);
  188. EXPORT_PIN(HAL_IN,bit,step_inv,"step-invert", 0);
  189. EXPORT_PIN(HAL_IN,u32,dir_port,"dir-port", UINT32_MAX);
  190. EXPORT_PIN(HAL_IN,u32,dir_pin,"dir-pin", UINT32_MAX);
  191. EXPORT_PIN(HAL_IN,bit,dir_inv,"dir-invert", 0);
  192. EXPORT_PIN(HAL_IN,float,pos_scale,"position-scale", 1.0);
  193. EXPORT_PIN(HAL_OUT,float,pos_fb,"position-fb", 0.0);
  194. EXPORT_PIN(HAL_OUT,float,freq,"frequency", 0.0);
  195. EXPORT_PIN(HAL_OUT,s32,counts,"counts", 0);
  196. stepgen_task_add(ch,0); // abort all tasks
  197. stepgen_pos_set(ch,0);
  198. if ( type[ch] ) { EXPORT_PIN(HAL_IN,float,vel_cmd,"velocity-cmd", 0.0); }
  199. else { EXPORT_PIN(HAL_IN,float,pos_cmd,"position-cmd", 0.0); }
  200. gp.ctrl_type = type[ch];
  201. gp.step_pos_cmd = 0;
  202. gp.step_inv_old = 0;
  203. gp.step_pin_old = UINT32_MAX;
  204. gp.step_port_old = UINT32_MAX;
  205. gp.dir_inv_old = 0;
  206. gp.dir_pin_old = UINT32_MAX;
  207. gp.dir_port_old = UINT32_MAX;
  208. }
  209. if ( r ) PRINT_ERROR_AND_RETURN("HAL pins export failed", -1);
  210. // export HAL functions
  211. r = 0;
  212. r += EXPORT_FUNC(capture_pos, "capture-position", 1);
  213. r += EXPORT_FUNC(update_freq, "update-freq", 1);
  214. if ( r ) PRINT_ERROR_AND_RETURN("HAL functions export failed", -1);
  215. return 0;
  216. }
  217. int32_t rtapi_app_main(void)
  218. {
  219. // get component id
  220. if ( (comp_id = hal_init(comp_name)) < 0 )
  221. {
  222. rtapi_print_msg(RTAPI_MSG_ERR, "%s: hal_init() failed\n", comp_name);
  223. return -1;
  224. }
  225. #define EXIT { hal_exit(comp_id); return -1; }
  226. // shared memory allocation and export
  227. cpu_id = allwinner_cpu_id_get(CPU);
  228. if ( msg_mem_init(cpu_id, comp_name) ) EXIT;
  229. if ( malloc_and_export(comp_name, comp_id) ) EXIT;
  230. // driver ready to work
  231. hal_ready(comp_id);
  232. return 0;
  233. }
  234. void rtapi_app_exit(void)
  235. {
  236. msg_mem_deinit();
  237. hal_exit(comp_id);
  238. }
  239. #undef EXPORT_PIN
  240. #undef PRINT_ERROR
  241. #undef PRINT_ERROR_AND_RETURN
  242. #undef g
  243. #undef gp