API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 24th, 2017
96
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 15.88 KB | None | 0 0
raw download clone embed print report
  1. /**
  2. * @file main.c
  3. *
  4. * the control law of this file is immigrated from the original control law code of
  5. * VisionAir developed by DCAS of ISAE. Note that the hovering mode is the neutral position,
  6. * that means in this code the body frame axises is a bit different.
  7. * @author CAI Dongcai <dacid120@gmail.com>
  8. */
  9.  
  10. #include <px4_config.h>
  11. #include <stdio.h>
  12. #include <stdlib.h>
  13. #include <string.h>
  14. #include <unistd.h>
  15. #include <fcntl.h>
  16. #include <errno.h>
  17. #include <math.h>
  18. #include <poll.h>
  19. #include <time.h>
  20. #include <drivers/drv_hrt.h>
  21. #include <uORB/uORB.h>
  22. #include <uORB/topics/vehicle_global_position.h> //???
  23. #include <uORB/topics/position_setpoint_triplet.h> //??
  24. #include <uORB/topics/vehicle_attitude.h>
  25. #include <uORB/topics/vehicle_status.h>
  26. #include <uORB/topics/vehicle_attitude_setpoint.h>
  27. #include <uORB/topics/manual_control_setpoint.h>
  28. #include <uORB/topics/actuator_controls.h>
  29. #include <uORB/topics/vehicle_rates_setpoint.h>
  30. #include <uORB/topics/vehicle_local_position.h> //changed by cai
  31. #include <uORB/topics/parameter_update.h>
  32. #include <uORB/topics/control_state.h> //added by cai
  33. #include <systemlib/param/param.h>
  34. #include <systemlib/pid/pid.h>
  35. #include <geo/geo.h>
  36. #include <systemlib/perf_counter.h>
  37. #include <systemlib/systemlib.h>
  38. #include <systemlib/err.h>
  39.  
  40.  
  41.  
  42. #include "params.h"
  43.  
  44.  
  45.  
  46.  
  47. /* Prototypes */
  48.  
  49. /**
  50. * Daemon management function.
  51. *
  52. * This function allows to start / stop the background task (daemon).
  53. * The purpose of it is to be able to start the controller on the
  54. * command line, query its status and stop it, without giving up
  55. * the command line to one particular process or the need for bg/fg
  56. * ^Z support by the shell.
  57. */
  58. __EXPORT int visionair_control_main(int argc, char *argv[]);
  59.  
  60. /**
  61. * Mainloop of daemon.
  62. */
  63. int visionair_control_thread_main(int argc, char *argv[]);
  64.  
  65. /**
  66. * Print the correct usage.
  67. */
  68. static void usage(const char *reason);
  69.  
  70.  
  71.  
  72. /**
  73. * VisionAir controller,
  74. * @param pManual_sp [description]
  75. * @param pLocal_pos [description]
  76. * @param pCtrl_state [pro]
  77. * @param pAtt [description]
  78. * @param pActuators [description]
  79. */
  80. static void visionair_controller(const struct manual_control_setpoint_s *pManual_sp,
  81. const struct vehicle_local_position_s *pLocal_pos,
  82. const struct control_state_s * pCtrl_state,
  83. const struct vehicle_attitude_s *pAtt,
  84. struct actuator_controls_s *pActuators);
  85.  
  86. /* Variables */
  87. static bool thread_should_exit = false; /**< Daemon exit flag */
  88. static bool thread_running = false; /**< Daemon status flag */
  89. static int deamon_task; /**< Handle of deamon task / thread */
  90. static struct params va_p; //VisionAir parameters
  91. static struct param_handles ph;
  92.  
  93.  
  94.  
  95. //Altitude, when switch from mannual mode to altitude mode, we should remember the altitude
  96. static float Hpc = 0.0f;
  97.  
  98.  
  99.  
  100. /**
  101. * Limit angle to -PI~PI
  102. * @param angle input angle(rad)
  103. * @return out put angle(rad)
  104. */
  105. static float modAngle(float angle);
  106. static float modAngle(float angle)
  107. {
  108. #if 0 /*this is created by CAI, not work properly for some value. e.g. -1.1*M_PI_F */
  109. int i;
  110.  
  111. i = (int)((angle+M_PI_F)*0.5f/M_PI_F);
  112.  
  113. return angle - M_PI_F * (float)(i*2);
  114. #endif
  115.  
  116. //use the original mod function
  117. if (angle > M_PI_F)
  118. angle += - 2 * M_PI_F ;
  119. if (angle < - M_PI_F)
  120. angle += 2 * M_PI_F ;
  121.  
  122. return angle ;
  123. }
  124.  
  125.  
  126.  
  127.  
  128.  
  129. static void visionair_controller(const struct manual_control_setpoint_s *pManual_sp,
  130. const struct vehicle_local_position_s *pLocal_pos,
  131. const struct control_state_s * pCtrl_state,
  132. const struct vehicle_attitude_s *pAtt,
  133. struct actuator_controls_s *pActuators)
  134. {
  135.  
  136. static float SEUIL_HAUT_GAZ_MODE2 = 0.65f; //TODO: change the variables' name
  137. static float SEUIL_BAS_GAZ_MODE2 = 0.35f;
  138. static float GAZ_BASCUL = 0.0f;
  139.  
  140.  
  141. //scale the RC inputs (roll, pitch, yaw) to angles(rad)
  142. static float scaled_rc_att[3] = {0.0f};
  143.  
  144. //throttle command
  145. static float GAZC = 0.0f;
  146.  
  147.  
  148. //height integrator
  149. static float TiGaz = 0.0f;
  150.  
  151. float Hp = -pLocal_pos->z;
  152. float vz_acc = -pLocal_pos->vz;
  153.  
  154.  
  155. // get dT
  156. static uint64_t last_run = 0;
  157. float dT = (hrt_absolute_time() - last_run) / 1000000.0f;
  158. last_run = hrt_absolute_time();
  159.  
  160. /* guard against too small (< 2ms) and too large (> 20ms) dt's */
  161. if (dT < 0.002f) {
  162. dT = 0.002f;
  163. } else if (dT > 0.02f) {
  164. dT = 0.02f;
  165. }
  166.  
  167.  
  168. /*************** calculate: scaled_rc_att, GAZC ********************/
  169. // scale rc attitude control from input commands to angles(rad)
  170. scaled_rc_att[0] = pManual_sp->y * va_p.rc_cmd_k[0]; //-1~1
  171. scaled_rc_att[1] = pManual_sp->x * va_p.rc_cmd_k[1]; //change from 0~170 to -90~80 (degree)
  172. scaled_rc_att[2] += (pManual_sp->r * va_p.rc_cmd_k[2])*dT;
  173. scaled_rc_att[2] = modAngle(scaled_rc_att[2]);
  174.  
  175. //TODO: if we want to see these variables in GCS through mavlink, put
  176. //them in vehicle_rates_setpoint or similar msgs that are not used.
  177.  
  178. switch(pManual_sp->mode_switch)
  179. {
  180. //the statement below need C++ support
  181. //case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
  182. case 3: //direct control of throttle
  183. Hpc = Hp;
  184. TiGaz = 0.0f;
  185. GAZC = pManual_sp->z; // pManual_sp->z is RC throttle input. 0~1
  186. GAZ_BASCUL = pManual_sp->z;
  187. break;
  188. //case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
  189. case 2: //throttle is used to control vz speed
  190. if (pManual_sp->z > SEUIL_HAUT_GAZ_MODE2)
  191. {
  192. Hpc += (pManual_sp->z - SEUIL_HAUT_GAZ_MODE2) / (1-SEUIL_HAUT_GAZ_MODE2) * va_p.rc_cmd_k[3] * dT;
  193. }
  194. else if (pManual_sp->z < SEUIL_BAS_GAZ_MODE2)
  195. {
  196. if (GAZC > 0.1f)
  197. {
  198. Hpc -= (SEUIL_BAS_GAZ_MODE2 - pManual_sp->z)/(SEUIL_BAS_GAZ_MODE2 - 0) * va_p.rc_cmd_k[3] * dT;
  199. }
  200. }
  201. else
  202. {
  203. ;
  204. }
  205.  
  206. TiGaz += va_p.h_PID[1] * (Hpc - Hp) * dT;
  207.  
  208. GAZC = GAZ_BASCUL+ va_p.h_PID[0] * (Hpc - Hp) + TiGaz + va_p.h_PID[2] * vz_acc;
  209.  
  210. break;
  211. default : //just protection in case we switch to a third postion and don't have any throttle command
  212. Hpc = Hp;
  213. TiGaz = 0.0f;
  214. GAZC = pManual_sp->z ;
  215. GAZ_BASCUL = pManual_sp->z;
  216. break;
  217. }
  218.  
  219.  
  220.  
  221. /*************** calculate: com_att ********************/
  222. // input: euler angles, anguler rates. Using PD control
  223.  
  224. //roll pitch yaw commands (in earth frame or from pilot's view?), there is mixing in roll and yaw depeding on pitch angle
  225. float com_att[3] = {0.0f};
  226.  
  227.  
  228. //printf("r,p,y _Q = %.4f, %.4f, %.4f\n", (double)pAtt->roll, (double)pAtt->pitch, (double)pAtt->yaw);
  229.  
  230.  
  231. float sin_pitch = sinf(pAtt->pitch);
  232. float cos_pitch = cosf(pAtt->pitch);
  233.  
  234. // Roll command
  235. float roll_rate_tmp = pCtrl_state->roll_rate * cos_pitch + pCtrl_state->yaw_rate * sin_pitch ; // roll derivative, note changes of the rotation axis, and which is the positive rotation side
  236. com_att[0] = va_p.att_P[0] * (scaled_rc_att[0] - pAtt->roll) + va_p.att_D[0] * roll_rate_tmp ;
  237.  
  238. // Pitch command
  239. com_att[1] = va_p.att_P[1] * (scaled_rc_att[1] - pAtt->pitch) + va_p.att_D[1] * pCtrl_state->pitch_rate ;
  240.  
  241. // Yaw command
  242. float yaw_error = modAngle(scaled_rc_att[2] - pAtt->yaw) ;
  243.  
  244. static int sature = 0;
  245. if ( (sature == 1) && (yaw_error<0) )
  246. yaw_error += 2*M_PI_F ;
  247. if ( (sature == -1) && (yaw_error>0) )
  248. yaw_error -= 2*M_PI_F ;
  249.  
  250. if (yaw_error > M_PI_F/2) // Ecrêtage de l'écart à 90°
  251. {
  252. yaw_error = M_PI_F/2 ;
  253. sature = 1 ;
  254. }
  255. else if (yaw_error < - M_PI_F/2)
  256. {
  257. yaw_error = -M_PI_F/2 ;
  258. sature=-1 ;
  259. }
  260. else
  261. {
  262. sature = 0;
  263. }
  264.  
  265. //yaw_rate_tmp = (-pCtrl_state->roll_rate*sin_pitch + pCtrl_state->yaw_rate*cos_pitch)/cosphi ;
  266. //yaw derivative TODO, why devide by cosphi?????
  267. float yaw_rate_tmp = (-pCtrl_state->roll_rate * sin_pitch + pCtrl_state->yaw_rate * cos_pitch);
  268.  
  269. com_att[2] = va_p.att_P[2] * yaw_error + va_p.att_D[2] * yaw_rate_tmp ;
  270.  
  271.  
  272.  
  273. /*************** calculate actuator_controls_s, which will be used in mixer ********************/
  274. //COM_ROULIS, COM_TANGAGE, COM_LACET, COM_GAZ in the original visionair code
  275. //(the commands below are in body frame ?) note the different coordinate frames(pilot's commands to control)
  276. pActuators->control[0] = com_att[0] * cos_pitch - com_att[2] * sin_pitch ; // roll
  277. pActuators->control[1] = com_att[1] ; // pitch
  278. pActuators->control[2] = com_att[0] * sin_pitch + com_att[2] * cos_pitch ; // yaw
  279.  
  280. //please be aware that the code below is anti yaw (roll) disturbation,
  281. //2016.9.16 change pCtrl_state->roll_rate to pCtrl_state->yaw_rate as the body coordinates change because of using Pixhawk
  282. pActuators->control[3] = GAZC + va_p.kgp * pCtrl_state->yaw_rate; //0.02 is kgp in original VisionAir sourcecode.
  283. pActuators->control[4] = GAZC - va_p.kgp * pCtrl_state->yaw_rate;
  284.  
  285. pActuators->timestamp = hrt_absolute_time();
  286. pActuators->timestamp_sample = pActuators->timestamp;
  287. }
  288.  
  289.  
  290. /* Main Thread */
  291. int visionair_control_thread_main(int argc, char *argv[])
  292. {
  293. /* read arguments */
  294. bool verbose = false;
  295.  
  296. for (int i = 1; i < argc; i++) {
  297. if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
  298. verbose = true;
  299. }
  300. }
  301.  
  302. /* welcome user (warnx prints a line, including an appended\n, with variable arguments */
  303. warnx("[visionair control] started");
  304.  
  305. /* initialize parameters, first the handles, then the values */
  306. parameters_init(&ph);
  307. parameters_update(&ph, &va_p);
  308.  
  309.  
  310.  
  311. /*
  312. * Declare and safely initialize all structs to zero.
  313. *
  314. * These structs contain the system state and things
  315. * like attitude, position, the current waypoint, etc.
  316. */
  317. struct vehicle_attitude_s att;
  318. memset(&att, 0, sizeof(att));
  319.  
  320.  
  321.  
  322. struct vehicle_attitude_setpoint_s att_sp;
  323. memset(&att_sp, 0, sizeof(att_sp));
  324. struct vehicle_rates_setpoint_s rates_sp;
  325. memset(&rates_sp, 0, sizeof(rates_sp));
  326. struct vehicle_status_s vstatus;
  327. memset(&vstatus, 0, sizeof(vstatus));
  328. //cai TODO: the 3 msgs above may not be useful anymore
  329.  
  330.  
  331.  
  332. struct manual_control_setpoint_s manual_sp;
  333. memset(&manual_sp, 0, sizeof(manual_sp));
  334.  
  335. //added by cai
  336. struct vehicle_local_position_s local_pos;
  337. memset(&local_pos, 0, sizeof(local_pos));
  338. struct control_state_s ctrl_state;
  339. memset(&ctrl_state, 0, sizeof(ctrl_state));
  340.  
  341. /* output structs - this is what is sent to the mixer */
  342. struct actuator_controls_s actuators;
  343. memset(&actuators, 0, sizeof(actuators));
  344.  
  345.  
  346. /* publish actuator controls with zero values */
  347. for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
  348. actuators.control[i] = 0.0f;
  349. }
  350.  
  351. /*
  352. * Advertise that this controller will publish actuator
  353. * control values and the rate setpoint
  354. */
  355. orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
  356. orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
  357.  
  358. /* subscribe to topics. */
  359. int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
  360. int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
  361. //int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
  362. int param_sub = orb_subscribe(ORB_ID(parameter_update));
  363. int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
  364. int ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
  365.  
  366. /* Setup of loop */
  367.  
  368. struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
  369. { .fd = ctrl_state_sub, .events = POLLIN }
  370. };
  371.  
  372.  
  373. while (!thread_should_exit) {
  374. /*
  375. * Wait for a sensor or param update, check for exit condition every 500 ms.
  376. * This means that the execution will block here without consuming any resources,
  377. * but will continue to execute the very moment a new attitude measurement or
  378. * a param update is published. So no latency in contrast to the polling
  379. * design pattern (do not confuse the poll() system call with polling).
  380. *
  381. * This design pattern makes the controller also agnostic of the attitude
  382. * update speed - it runs as fast as the attitude updates with minimal latency.
  383. */
  384. int ret = poll(fds, 2, 100);
  385.  
  386. if (ret < 0) {
  387. /*
  388. * Poll error, this will not really happen in practice,
  389. * but its good design practice to make output an error message.
  390. */
  391. warnx("poll error");
  392. usleep(100000);
  393. continue;
  394. }
  395. if (ret == 0) {
  396. /* no return value = nothing changed for 500 ms, ignore */
  397. warnx("poll timeout visionair");
  398. continue;
  399. }
  400.  
  401.  
  402.  
  403. /* only update parameters if they changed */
  404. if (fds[0].revents & POLLIN) {
  405. /* read from param to clear updated flag (uORB API requirement) */
  406. struct parameter_update_s update;
  407. orb_copy(ORB_ID(parameter_update), param_sub, &update);
  408.  
  409. /* if a param update occured, re-read our parameters */
  410. parameters_update(&ph, &va_p);
  411. }
  412.  
  413.  
  414. /* only run controller if attitude changed */
  415. if (fds[1].revents & POLLIN) {
  416.  
  417. /* copy control state topic */
  418. orb_copy(ORB_ID(control_state), ctrl_state_sub, &ctrl_state);
  419.  
  420.  
  421. /* check for updates in other topics */
  422. bool updated = false;
  423.  
  424. orb_check(local_pos_sub, &updated);
  425. if (updated) {
  426. orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
  427. }
  428.  
  429. updated = false;
  430. orb_check(manual_sp_sub, &updated);
  431. if (updated) /* get the RC (or otherwise user based) input */
  432. {
  433. orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
  434. //cai test for read pwm input
  435. if (verbose) {
  436. warnx("manual input x=%f, y=%f, z=%f, r=%f;\n", (double)manual_sp.x, (double)manual_sp.y, (double)manual_sp.z, (double)manual_sp.r);
  437. }
  438. }
  439.  
  440. //cai: currently we don't need the two msgs below:
  441. /* get the system status and the flight mode we're in */
  442. //orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
  443.  
  444.  
  445. /* get a local copy of attitude */
  446. updated = false;
  447. orb_check(att_sub, &updated);
  448. if (updated)
  449. {
  450. orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
  451. }
  452.  
  453.  
  454. /* check if the throttle was ever more than 50% - go later only to failsafe if yes */
  455. if (isfinite(manual_sp.z) &&
  456. (manual_sp.z >= 0.6f) &&
  457. (manual_sp.z <= 1.0f)) {
  458. }
  459.  
  460. //run the controller!!
  461. visionair_controller(&manual_sp, &local_pos, &ctrl_state, &att, &actuators);
  462.  
  463.  
  464.  
  465. /* publish rates */
  466. orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
  467.  
  468. //warnx("set acuators0 = %f; 1 = %f; 2 = %f; 3 = %f\n", (double)actuators.control[0], (double)actuators.control[1],(double)actuators.control[2],(double)actuators.control[3]);
  469.  
  470. /* sanity check and publish actuator outputs */
  471. if (isfinite(actuators.control[0]) &&
  472. isfinite(actuators.control[1]) &&
  473. isfinite(actuators.control[2]) &&
  474. isfinite(actuators.control[3])) {
  475.  
  476. orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
  477.  
  478. if (verbose) {
  479. warnx("published");
  480. }
  481. }
  482. }
  483.  
  484. }
  485.  
  486. printf("[visionair_control] exiting, stopping all motors.\n");
  487. thread_running = false;
  488.  
  489. /* kill all outputs */
  490. for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
  491. actuators.control[i] = 0.0f;
  492. }
  493.  
  494. orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
  495.  
  496. fflush(stdout);
  497.  
  498. return 0;
  499. }
  500.  
  501. /* Startup Functions */
  502.  
  503. static void
  504. usage(const char *reason)
  505. {
  506. if (reason) {
  507. fprintf(stderr, "%s\n", reason);
  508. }
  509.  
  510. fprintf(stderr, "usage: visionair_control {start|stop|status}\n\n");
  511. exit(1);
  512. }
  513.  
  514. /**
  515. * The daemon app only briefly exists to start
  516. * the background job. The stack size assigned in the
  517. * Makefile does only apply to this management task.
  518. *
  519. * The actual stack size should be set in the call
  520. * to px4_task_spawn_cmd().
  521. */
  522. int visionair_control_main(int argc, char *argv[])
  523. {
  524. if (argc < 2) {
  525. usage("missing command");
  526. }
  527.  
  528. if (!strcmp(argv[1], "start")) {
  529.  
  530. if (thread_running) {
  531. printf("visionair_control already running\n");
  532. /* this is not an error */
  533. exit(0);
  534. }
  535.  
  536. thread_should_exit = false;
  537. deamon_task = px4_task_spawn_cmd("visionair_control",
  538. SCHED_DEFAULT,
  539. SCHED_PRIORITY_MAX - 20,
  540. 2048,
  541. visionair_control_thread_main,
  542. (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
  543. thread_running = true;
  544. exit(0);
  545. }
  546.  
  547. if (!strcmp(argv[1], "stop")) {
  548. thread_should_exit = true;
  549. exit(0);
  550. }
  551.  
  552. if (!strcmp(argv[1], "status")) {
  553. if (thread_running) {
  554. printf("\tvisionair_control is running\n");
  555.  
  556. } else {
  557. printf("\tvisionair_control not started\n");
  558. }
  559.  
  560. exit(0);
  561. }
  562.  
  563. usage("unrecognized command");
  564. exit(1);
  565. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!