NavdataMessageDefinitions.h
Go to the documentation of this file.
1 
25 // Autogenerated from source-files using the CreateNavdataFormat.py script
26 
27 #ifdef NAVDATA_STRUCTS_INCLUDES
28 #include "ardrone_autonomy/navdata_demo.h"
29 #include "ardrone_autonomy/navdata_time.h"
30 #include "ardrone_autonomy/navdata_raw_measures.h"
31 #include "ardrone_autonomy/navdata_phys_measures.h"
32 #include "ardrone_autonomy/navdata_gyros_offsets.h"
33 #include "ardrone_autonomy/navdata_euler_angles.h"
34 #include "ardrone_autonomy/navdata_references.h"
35 #include "ardrone_autonomy/navdata_trims.h"
36 #include "ardrone_autonomy/navdata_rc_references.h"
37 #include "ardrone_autonomy/navdata_pwm.h"
38 #include "ardrone_autonomy/navdata_altitude.h"
39 #include "ardrone_autonomy/navdata_vision_raw.h"
40 #include "ardrone_autonomy/navdata_vision_of.h"
41 #include "ardrone_autonomy/navdata_vision.h"
42 #include "ardrone_autonomy/navdata_vision_perf.h"
43 #include "ardrone_autonomy/navdata_trackers_send.h"
44 #include "ardrone_autonomy/navdata_vision_detect.h"
45 #include "ardrone_autonomy/navdata_watchdog.h"
46 #include "ardrone_autonomy/navdata_adc_data_frame.h"
47 #include "ardrone_autonomy/navdata_video_stream.h"
48 #include "ardrone_autonomy/navdata_games.h"
49 #include "ardrone_autonomy/navdata_pressure_raw.h"
50 #include "ardrone_autonomy/navdata_magneto.h"
51 #include "ardrone_autonomy/navdata_wind_speed.h"
52 #include "ardrone_autonomy/navdata_kalman_pressure.h"
53 #include "ardrone_autonomy/navdata_hdvideo_stream.h"
54 #include "ardrone_autonomy/navdata_wifi.h"
55 #include "ardrone_autonomy/navdata_zimmu_3000.h"
56 #include <std_msgs/UInt16.h>
57 #include <std_msgs/UInt32.h>
58 #include <std_msgs/Float32.h>
59 #include <std_msgs/Int32.h>
60 #include <std_msgs/Int16.h>
61 #include <std_msgs/UInt8.h>
62 #include <ardrone_autonomy/vector31.h>
63 #include <ardrone_autonomy/vector21.h>
64 #include <ardrone_autonomy/matrix33.h>
65 #endif
66 
67 #ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
68  void PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received);
69 #endif
70 
71 #ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
72  ros::Publisher pub_navdata_demo;
73  bool enabled_navdata_demo;
74  ardrone_autonomy::navdata_demo navdata_demo_msg;
75  ros::Publisher pub_navdata_time;
76  bool enabled_navdata_time;
77  ardrone_autonomy::navdata_time navdata_time_msg;
78  ros::Publisher pub_navdata_raw_measures;
79  bool enabled_navdata_raw_measures;
80  ardrone_autonomy::navdata_raw_measures navdata_raw_measures_msg;
81  ros::Publisher pub_navdata_phys_measures;
82  bool enabled_navdata_phys_measures;
83  ardrone_autonomy::navdata_phys_measures navdata_phys_measures_msg;
84  ros::Publisher pub_navdata_gyros_offsets;
85  bool enabled_navdata_gyros_offsets;
86  ardrone_autonomy::navdata_gyros_offsets navdata_gyros_offsets_msg;
87  ros::Publisher pub_navdata_euler_angles;
88  bool enabled_navdata_euler_angles;
89  ardrone_autonomy::navdata_euler_angles navdata_euler_angles_msg;
90  ros::Publisher pub_navdata_references;
91  bool enabled_navdata_references;
92  ardrone_autonomy::navdata_references navdata_references_msg;
93  ros::Publisher pub_navdata_trims;
94  bool enabled_navdata_trims;
95  ardrone_autonomy::navdata_trims navdata_trims_msg;
96  ros::Publisher pub_navdata_rc_references;
97  bool enabled_navdata_rc_references;
98  ardrone_autonomy::navdata_rc_references navdata_rc_references_msg;
99  ros::Publisher pub_navdata_pwm;
100  bool enabled_navdata_pwm;
101  ardrone_autonomy::navdata_pwm navdata_pwm_msg;
102  ros::Publisher pub_navdata_altitude;
103  bool enabled_navdata_altitude;
104  ardrone_autonomy::navdata_altitude navdata_altitude_msg;
105  ros::Publisher pub_navdata_vision_raw;
106  bool enabled_navdata_vision_raw;
107  ardrone_autonomy::navdata_vision_raw navdata_vision_raw_msg;
108  ros::Publisher pub_navdata_vision_of;
109  bool enabled_navdata_vision_of;
110  ardrone_autonomy::navdata_vision_of navdata_vision_of_msg;
111  ros::Publisher pub_navdata_vision;
112  bool enabled_navdata_vision;
113  ardrone_autonomy::navdata_vision navdata_vision_msg;
114  ros::Publisher pub_navdata_vision_perf;
115  bool enabled_navdata_vision_perf;
116  ardrone_autonomy::navdata_vision_perf navdata_vision_perf_msg;
117  ros::Publisher pub_navdata_trackers_send;
118  bool enabled_navdata_trackers_send;
119  ardrone_autonomy::navdata_trackers_send navdata_trackers_send_msg;
120  ros::Publisher pub_navdata_vision_detect;
121  bool enabled_navdata_vision_detect;
122  ardrone_autonomy::navdata_vision_detect navdata_vision_detect_msg;
123  ros::Publisher pub_navdata_watchdog;
124  bool enabled_navdata_watchdog;
125  ardrone_autonomy::navdata_watchdog navdata_watchdog_msg;
126  ros::Publisher pub_navdata_adc_data_frame;
127  bool enabled_navdata_adc_data_frame;
128  ardrone_autonomy::navdata_adc_data_frame navdata_adc_data_frame_msg;
129  ros::Publisher pub_navdata_video_stream;
130  bool enabled_navdata_video_stream;
131  ardrone_autonomy::navdata_video_stream navdata_video_stream_msg;
132  ros::Publisher pub_navdata_games;
133  bool enabled_navdata_games;
134  ardrone_autonomy::navdata_games navdata_games_msg;
135  ros::Publisher pub_navdata_pressure_raw;
136  bool enabled_navdata_pressure_raw;
137  ardrone_autonomy::navdata_pressure_raw navdata_pressure_raw_msg;
138  ros::Publisher pub_navdata_magneto;
139  bool enabled_navdata_magneto;
140  ardrone_autonomy::navdata_magneto navdata_magneto_msg;
141  ros::Publisher pub_navdata_wind_speed;
142  bool enabled_navdata_wind_speed;
143  ardrone_autonomy::navdata_wind_speed navdata_wind_speed_msg;
144  ros::Publisher pub_navdata_kalman_pressure;
145  bool enabled_navdata_kalman_pressure;
146  ardrone_autonomy::navdata_kalman_pressure navdata_kalman_pressure_msg;
147  ros::Publisher pub_navdata_hdvideo_stream;
148  bool enabled_navdata_hdvideo_stream;
149  ardrone_autonomy::navdata_hdvideo_stream navdata_hdvideo_stream_msg;
150  ros::Publisher pub_navdata_wifi;
151  bool enabled_navdata_wifi;
152  ardrone_autonomy::navdata_wifi navdata_wifi_msg;
153  ros::Publisher pub_navdata_zimmu_3000;
154  bool enabled_navdata_zimmu_3000;
155  ardrone_autonomy::navdata_zimmu_3000 navdata_zimmu_3000_msg;
156 
157  bool enabled_legacy_navdata;
158 
159  bool initialized_navdata_publishers;
160 #endif
161 
162 #ifdef NAVDATA_STRUCTS_INITIALIZE
163  if(!initialized_navdata_publishers)
164  {
165  initialized_navdata_publishers = true;
166 
167  ros::param::param("~enable_legacy_navdata", enabled_legacy_navdata, true);
168  if(enabled_legacy_navdata)
169  {
170  navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 200);
171  imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 200);
172  mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 200);
173  }
174  odo_pub = node_handle.advertise<nav_msgs::Odometry>("ardrone/odometry", 200);
175 
176  //-------------------------
177 
178  ros::param::param("~enable_navdata_demo", enabled_navdata_demo, false);
179  if(enabled_navdata_demo)
180  {
181  pub_navdata_demo = node_handle.advertise<ardrone_autonomy::navdata_demo>("ardrone/navdata_demo", NAVDATA_QUEUE_SIZE);
182  }
183 
184  //-------------------------
185 
186  ros::param::param("~enable_navdata_time", enabled_navdata_time, false);
187  if(enabled_navdata_time)
188  {
189  pub_navdata_time = node_handle.advertise<ardrone_autonomy::navdata_time>("ardrone/navdata_time", NAVDATA_QUEUE_SIZE);
190  }
191 
192  //-------------------------
193 
194  ros::param::param("~enable_navdata_raw_measures", enabled_navdata_raw_measures, false);
195  if(enabled_navdata_raw_measures)
196  {
197  pub_navdata_raw_measures = node_handle.advertise<ardrone_autonomy::navdata_raw_measures>("ardrone/navdata_raw_measures", NAVDATA_QUEUE_SIZE);
198  }
199 
200  //-------------------------
201 
202  ros::param::param("~enable_navdata_phys_measures", enabled_navdata_phys_measures, false);
203  if(enabled_navdata_phys_measures)
204  {
205  pub_navdata_phys_measures = node_handle.advertise<ardrone_autonomy::navdata_phys_measures>("ardrone/navdata_phys_measures", NAVDATA_QUEUE_SIZE);
206  }
207 
208  //-------------------------
209 
210  ros::param::param("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets, false);
211  if(enabled_navdata_gyros_offsets)
212  {
213  pub_navdata_gyros_offsets = node_handle.advertise<ardrone_autonomy::navdata_gyros_offsets>("ardrone/navdata_gyros_offsets", NAVDATA_QUEUE_SIZE);
214  }
215 
216  //-------------------------
217 
218  ros::param::param("~enable_navdata_euler_angles", enabled_navdata_euler_angles, false);
219  if(enabled_navdata_euler_angles)
220  {
221  pub_navdata_euler_angles = node_handle.advertise<ardrone_autonomy::navdata_euler_angles>("ardrone/navdata_euler_angles", NAVDATA_QUEUE_SIZE);
222  }
223 
224  //-------------------------
225 
226  ros::param::param("~enable_navdata_references", enabled_navdata_references, false);
227  if(enabled_navdata_references)
228  {
229  pub_navdata_references = node_handle.advertise<ardrone_autonomy::navdata_references>("ardrone/navdata_references", NAVDATA_QUEUE_SIZE);
230  }
231 
232  //-------------------------
233 
234  ros::param::param("~enable_navdata_trims", enabled_navdata_trims, false);
235  if(enabled_navdata_trims)
236  {
237  pub_navdata_trims = node_handle.advertise<ardrone_autonomy::navdata_trims>("ardrone/navdata_trims", NAVDATA_QUEUE_SIZE);
238  }
239 
240  //-------------------------
241 
242  ros::param::param("~enable_navdata_rc_references", enabled_navdata_rc_references, false);
243  if(enabled_navdata_rc_references)
244  {
245  pub_navdata_rc_references = node_handle.advertise<ardrone_autonomy::navdata_rc_references>("ardrone/navdata_rc_references", NAVDATA_QUEUE_SIZE);
246  }
247 
248  //-------------------------
249 
250  ros::param::param("~enable_navdata_pwm", enabled_navdata_pwm, false);
251  if(enabled_navdata_pwm)
252  {
253  pub_navdata_pwm = node_handle.advertise<ardrone_autonomy::navdata_pwm>("ardrone/navdata_pwm", NAVDATA_QUEUE_SIZE);
254  }
255 
256  //-------------------------
257 
258  ros::param::param("~enable_navdata_altitude", enabled_navdata_altitude, false);
259  if(enabled_navdata_altitude)
260  {
261  pub_navdata_altitude = node_handle.advertise<ardrone_autonomy::navdata_altitude>("ardrone/navdata_altitude", NAVDATA_QUEUE_SIZE);
262  }
263 
264  //-------------------------
265 
266  ros::param::param("~enable_navdata_vision_raw", enabled_navdata_vision_raw, false);
267  if(enabled_navdata_vision_raw)
268  {
269  pub_navdata_vision_raw = node_handle.advertise<ardrone_autonomy::navdata_vision_raw>("ardrone/navdata_vision_raw", NAVDATA_QUEUE_SIZE);
270  }
271 
272  //-------------------------
273 
274  ros::param::param("~enable_navdata_vision_of", enabled_navdata_vision_of, false);
275  if(enabled_navdata_vision_of)
276  {
277  pub_navdata_vision_of = node_handle.advertise<ardrone_autonomy::navdata_vision_of>("ardrone/navdata_vision_of", NAVDATA_QUEUE_SIZE);
278  }
279 
280  //-------------------------
281 
282  ros::param::param("~enable_navdata_vision", enabled_navdata_vision, false);
283  if(enabled_navdata_vision)
284  {
285  pub_navdata_vision = node_handle.advertise<ardrone_autonomy::navdata_vision>("ardrone/navdata_vision", NAVDATA_QUEUE_SIZE);
286  }
287 
288  //-------------------------
289 
290  ros::param::param("~enable_navdata_vision_perf", enabled_navdata_vision_perf, false);
291  if(enabled_navdata_vision_perf)
292  {
293  pub_navdata_vision_perf = node_handle.advertise<ardrone_autonomy::navdata_vision_perf>("ardrone/navdata_vision_perf", NAVDATA_QUEUE_SIZE);
294  }
295 
296  //-------------------------
297 
298  ros::param::param("~enable_navdata_trackers_send", enabled_navdata_trackers_send, false);
299  if(enabled_navdata_trackers_send)
300  {
301  pub_navdata_trackers_send = node_handle.advertise<ardrone_autonomy::navdata_trackers_send>("ardrone/navdata_trackers_send", NAVDATA_QUEUE_SIZE);
302  }
303 
304  //-------------------------
305 
306  ros::param::param("~enable_navdata_vision_detect", enabled_navdata_vision_detect, false);
307  if(enabled_navdata_vision_detect)
308  {
309  pub_navdata_vision_detect = node_handle.advertise<ardrone_autonomy::navdata_vision_detect>("ardrone/navdata_vision_detect", NAVDATA_QUEUE_SIZE);
310  }
311 
312  //-------------------------
313 
314  ros::param::param("~enable_navdata_watchdog", enabled_navdata_watchdog, false);
315  if(enabled_navdata_watchdog)
316  {
317  pub_navdata_watchdog = node_handle.advertise<ardrone_autonomy::navdata_watchdog>("ardrone/navdata_watchdog", NAVDATA_QUEUE_SIZE);
318  }
319 
320  //-------------------------
321 
322  ros::param::param("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame, false);
323  if(enabled_navdata_adc_data_frame)
324  {
325  pub_navdata_adc_data_frame = node_handle.advertise<ardrone_autonomy::navdata_adc_data_frame>("ardrone/navdata_adc_data_frame", NAVDATA_QUEUE_SIZE);
326  }
327 
328  //-------------------------
329 
330  ros::param::param("~enable_navdata_video_stream", enabled_navdata_video_stream, false);
331  if(enabled_navdata_video_stream)
332  {
333  pub_navdata_video_stream = node_handle.advertise<ardrone_autonomy::navdata_video_stream>("ardrone/navdata_video_stream", NAVDATA_QUEUE_SIZE);
334  }
335 
336  //-------------------------
337 
338  ros::param::param("~enable_navdata_games", enabled_navdata_games, false);
339  if(enabled_navdata_games)
340  {
341  pub_navdata_games = node_handle.advertise<ardrone_autonomy::navdata_games>("ardrone/navdata_games", NAVDATA_QUEUE_SIZE);
342  }
343 
344  //-------------------------
345 
346  ros::param::param("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw, false);
347  if(enabled_navdata_pressure_raw)
348  {
349  pub_navdata_pressure_raw = node_handle.advertise<ardrone_autonomy::navdata_pressure_raw>("ardrone/navdata_pressure_raw", NAVDATA_QUEUE_SIZE);
350  }
351 
352  //-------------------------
353 
354  ros::param::param("~enable_navdata_magneto", enabled_navdata_magneto, false);
355  if(enabled_navdata_magneto)
356  {
357  pub_navdata_magneto = node_handle.advertise<ardrone_autonomy::navdata_magneto>("ardrone/navdata_magneto", NAVDATA_QUEUE_SIZE);
358  }
359 
360  //-------------------------
361 
362  ros::param::param("~enable_navdata_wind_speed", enabled_navdata_wind_speed, false);
363  if(enabled_navdata_wind_speed)
364  {
365  pub_navdata_wind_speed = node_handle.advertise<ardrone_autonomy::navdata_wind_speed>("ardrone/navdata_wind_speed", NAVDATA_QUEUE_SIZE);
366  }
367 
368  //-------------------------
369 
370  ros::param::param("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure, false);
371  if(enabled_navdata_kalman_pressure)
372  {
373  pub_navdata_kalman_pressure = node_handle.advertise<ardrone_autonomy::navdata_kalman_pressure>("ardrone/navdata_kalman_pressure", NAVDATA_QUEUE_SIZE);
374  }
375 
376  //-------------------------
377 
378  ros::param::param("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream, false);
379  if(enabled_navdata_hdvideo_stream)
380  {
381  pub_navdata_hdvideo_stream = node_handle.advertise<ardrone_autonomy::navdata_hdvideo_stream>("ardrone/navdata_hdvideo_stream", NAVDATA_QUEUE_SIZE);
382  }
383 
384  //-------------------------
385 
386  ros::param::param("~enable_navdata_wifi", enabled_navdata_wifi, false);
387  if(enabled_navdata_wifi)
388  {
389  pub_navdata_wifi = node_handle.advertise<ardrone_autonomy::navdata_wifi>("ardrone/navdata_wifi", NAVDATA_QUEUE_SIZE);
390  }
391 
392  //-------------------------
393 
394  ros::param::param("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000, false);
395  if(enabled_navdata_zimmu_3000)
396  {
397  pub_navdata_zimmu_3000 = node_handle.advertise<ardrone_autonomy::navdata_zimmu_3000>("ardrone/navdata_zimmu_3000", NAVDATA_QUEUE_SIZE);
398  }
399 
400  //-------------------------
401 
402  }
403 #endif
404 
405 #ifdef NAVDATA_STRUCTS_SOURCE
406 void ARDroneDriver::PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received)
407 {
408  if(initialized_navdata_publishers)
409  {
410  if(enabled_navdata_demo && pub_navdata_demo.getNumSubscribers()>0)
411  {
412  navdata_demo_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
413  navdata_demo_msg.header.stamp = received;
414  navdata_demo_msg.header.frame_id = drone_frame_base;
415 
416  {
417  uint16_t c = n.navdata_demo.tag;
418  uint16_t m;
419  m = c;
420 
421  navdata_demo_msg.tag = m;
422  }
423 
424  {
425  uint16_t c = n.navdata_demo.size;
426  uint16_t m;
427  m = c;
428 
429  navdata_demo_msg.size = m;
430  }
431 
432  {
433  uint32_t c = n.navdata_demo.ctrl_state;
434  uint32_t m;
435  m = c;
436 
437  navdata_demo_msg.ctrl_state = m;
438  }
439 
440  {
441  uint32_t c = n.navdata_demo.vbat_flying_percentage;
442  uint32_t m;
443  m = c;
444 
445  navdata_demo_msg.vbat_flying_percentage = m;
446  }
447 
448  {
449  float32_t c = n.navdata_demo.theta;
450  float32_t m;
451  m = c;
452 
453  navdata_demo_msg.theta = m;
454  }
455 
456  {
457  float32_t c = n.navdata_demo.phi;
458  float32_t m;
459  m = c;
460 
461  navdata_demo_msg.phi = m;
462  }
463 
464  {
465  float32_t c = n.navdata_demo.psi;
466  float32_t m;
467  m = c;
468 
469  navdata_demo_msg.psi = m;
470  }
471 
472  {
473  int32_t c = n.navdata_demo.altitude;
474  int32_t m;
475  m = c;
476 
477  navdata_demo_msg.altitude = m;
478  }
479 
480  {
481  float32_t c = n.navdata_demo.vx;
482  float32_t m;
483  m = c;
484 
485  navdata_demo_msg.vx = m;
486  }
487 
488  {
489  float32_t c = n.navdata_demo.vy;
490  float32_t m;
491  m = c;
492 
493  navdata_demo_msg.vy = m;
494  }
495 
496  {
497  float32_t c = n.navdata_demo.vz;
498  float32_t m;
499  m = c;
500 
501  navdata_demo_msg.vz = m;
502  }
503 
504  {
505  uint32_t c = n.navdata_demo.num_frames;
506  uint32_t m;
507  m = c;
508 
509  navdata_demo_msg.num_frames = m;
510  }
511 
512  {
513  uint32_t c = n.navdata_demo.detection_camera_type;
514  uint32_t m;
515  m = c;
516 
517  navdata_demo_msg.detection_camera_type = m;
518  }
519 
520  pub_navdata_demo.publish(navdata_demo_msg);
521  }
522 
523  //-------------------------
524 
525  if(enabled_navdata_time && pub_navdata_time.getNumSubscribers()>0)
526  {
527  navdata_time_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
528  navdata_time_msg.header.stamp = received;
529  navdata_time_msg.header.frame_id = drone_frame_base;
530 
531  {
532  uint16_t c = n.navdata_time.tag;
533  uint16_t m;
534  m = c;
535 
536  navdata_time_msg.tag = m;
537  }
538 
539  {
540  uint16_t c = n.navdata_time.size;
541  uint16_t m;
542  m = c;
543 
544  navdata_time_msg.size = m;
545  }
546 
547  {
548  uint32_t c = n.navdata_time.time;
549  uint32_t m;
550  m = c;
551 
552  navdata_time_msg.time = m;
553  }
554 
555  pub_navdata_time.publish(navdata_time_msg);
556  }
557 
558  //-------------------------
559 
560  if(enabled_navdata_raw_measures && pub_navdata_raw_measures.getNumSubscribers()>0)
561  {
562  navdata_raw_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
563  navdata_raw_measures_msg.header.stamp = received;
564  navdata_raw_measures_msg.header.frame_id = drone_frame_base;
565 
566  {
567  uint16_t c = n.navdata_raw_measures.tag;
568  uint16_t m;
569  m = c;
570 
571  navdata_raw_measures_msg.tag = m;
572  }
573 
574  {
575  uint16_t c = n.navdata_raw_measures.size;
576  uint16_t m;
577  m = c;
578 
579  navdata_raw_measures_msg.size = m;
580  }
581 
582  navdata_raw_measures_msg.raw_gyros.clear();
583  for(int i=0; i<NB_GYROS; i++)
584  {
585  int16_t c = n.navdata_raw_measures.raw_gyros[i];
586  int16_t m;
587  m = c;
588 
589  navdata_raw_measures_msg.raw_gyros.push_back(m);
590  }
591 
592  navdata_raw_measures_msg.raw_gyros_110.clear();
593  for(int i=0; i<2; i++)
594  {
595  int16_t c = n.navdata_raw_measures.raw_gyros_110[i];
596  int16_t m;
597  m = c;
598 
599  navdata_raw_measures_msg.raw_gyros_110.push_back(m);
600  }
601 
602  {
603  uint32_t c = n.navdata_raw_measures.vbat_raw;
604  uint32_t m;
605  m = c;
606 
607  navdata_raw_measures_msg.vbat_raw = m;
608  }
609 
610  {
611  uint16_t c = n.navdata_raw_measures.us_debut_echo;
612  uint16_t m;
613  m = c;
614 
615  navdata_raw_measures_msg.us_debut_echo = m;
616  }
617 
618  {
619  uint16_t c = n.navdata_raw_measures.us_fin_echo;
620  uint16_t m;
621  m = c;
622 
623  navdata_raw_measures_msg.us_fin_echo = m;
624  }
625 
626  {
627  uint16_t c = n.navdata_raw_measures.us_association_echo;
628  uint16_t m;
629  m = c;
630 
631  navdata_raw_measures_msg.us_association_echo = m;
632  }
633 
634  {
635  uint16_t c = n.navdata_raw_measures.us_distance_echo;
636  uint16_t m;
637  m = c;
638 
639  navdata_raw_measures_msg.us_distance_echo = m;
640  }
641 
642  {
643  uint16_t c = n.navdata_raw_measures.us_courbe_temps;
644  uint16_t m;
645  m = c;
646 
647  navdata_raw_measures_msg.us_courbe_temps = m;
648  }
649 
650  {
651  uint16_t c = n.navdata_raw_measures.us_courbe_valeur;
652  uint16_t m;
653  m = c;
654 
655  navdata_raw_measures_msg.us_courbe_valeur = m;
656  }
657 
658  {
659  uint16_t c = n.navdata_raw_measures.us_courbe_ref;
660  uint16_t m;
661  m = c;
662 
663  navdata_raw_measures_msg.us_courbe_ref = m;
664  }
665 
666  {
667  uint16_t c = n.navdata_raw_measures.flag_echo_ini;
668  uint16_t m;
669  m = c;
670 
671  navdata_raw_measures_msg.flag_echo_ini = m;
672  }
673 
674  {
675  uint16_t c = n.navdata_raw_measures.nb_echo;
676  uint16_t m;
677  m = c;
678 
679  navdata_raw_measures_msg.nb_echo = m;
680  }
681 
682  {
683  uint32_t c = n.navdata_raw_measures.sum_echo;
684  uint32_t m;
685  m = c;
686 
687  navdata_raw_measures_msg.sum_echo = m;
688  }
689 
690  {
691  int32_t c = n.navdata_raw_measures.alt_temp_raw;
692  int32_t m;
693  m = c;
694 
695  navdata_raw_measures_msg.alt_temp_raw = m;
696  }
697 
698  {
699  int16_t c = n.navdata_raw_measures.gradient;
700  int16_t m;
701  m = c;
702 
703  navdata_raw_measures_msg.gradient = m;
704  }
705 
706  pub_navdata_raw_measures.publish(navdata_raw_measures_msg);
707  }
708 
709  //-------------------------
710 
711  if(enabled_navdata_phys_measures && pub_navdata_phys_measures.getNumSubscribers()>0)
712  {
713  navdata_phys_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
714  navdata_phys_measures_msg.header.stamp = received;
715  navdata_phys_measures_msg.header.frame_id = drone_frame_base;
716 
717  {
718  uint16_t c = n.navdata_phys_measures.tag;
719  uint16_t m;
720  m = c;
721 
722  navdata_phys_measures_msg.tag = m;
723  }
724 
725  {
726  uint16_t c = n.navdata_phys_measures.size;
727  uint16_t m;
728  m = c;
729 
730  navdata_phys_measures_msg.size = m;
731  }
732 
733  {
734  float32_t c = n.navdata_phys_measures.accs_temp;
735  float32_t m;
736  m = c;
737 
738  navdata_phys_measures_msg.accs_temp = m;
739  }
740 
741  {
742  uint16_t c = n.navdata_phys_measures.gyro_temp;
743  uint16_t m;
744  m = c;
745 
746  navdata_phys_measures_msg.gyro_temp = m;
747  }
748 
749  navdata_phys_measures_msg.phys_accs.clear();
750  for(int i=0; i<NB_ACCS; i++)
751  {
752  float32_t c = n.navdata_phys_measures.phys_accs[i];
753  float32_t m;
754  m = c;
755 
756  navdata_phys_measures_msg.phys_accs.push_back(m);
757  }
758 
759  navdata_phys_measures_msg.phys_gyros.clear();
760  for(int i=0; i<NB_GYROS; i++)
761  {
762  float32_t c = n.navdata_phys_measures.phys_gyros[i];
763  float32_t m;
764  m = c;
765 
766  navdata_phys_measures_msg.phys_gyros.push_back(m);
767  }
768 
769  {
770  uint32_t c = n.navdata_phys_measures.alim3V3;
771  uint32_t m;
772  m = c;
773 
774  navdata_phys_measures_msg.alim3V3 = m;
775  }
776 
777  {
778  uint32_t c = n.navdata_phys_measures.vrefEpson;
779  uint32_t m;
780  m = c;
781 
782  navdata_phys_measures_msg.vrefEpson = m;
783  }
784 
785  {
786  uint32_t c = n.navdata_phys_measures.vrefIDG;
787  uint32_t m;
788  m = c;
789 
790  navdata_phys_measures_msg.vrefIDG = m;
791  }
792 
793  pub_navdata_phys_measures.publish(navdata_phys_measures_msg);
794  }
795 
796  //-------------------------
797 
798  if(enabled_navdata_gyros_offsets && pub_navdata_gyros_offsets.getNumSubscribers()>0)
799  {
800  navdata_gyros_offsets_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
801  navdata_gyros_offsets_msg.header.stamp = received;
802  navdata_gyros_offsets_msg.header.frame_id = drone_frame_base;
803 
804  {
805  uint16_t c = n.navdata_gyros_offsets.tag;
806  uint16_t m;
807  m = c;
808 
809  navdata_gyros_offsets_msg.tag = m;
810  }
811 
812  {
813  uint16_t c = n.navdata_gyros_offsets.size;
814  uint16_t m;
815  m = c;
816 
817  navdata_gyros_offsets_msg.size = m;
818  }
819 
820  navdata_gyros_offsets_msg.offset_g.clear();
821  for(int i=0; i<NB_GYROS; i++)
822  {
823  float32_t c = n.navdata_gyros_offsets.offset_g[i];
824  float32_t m;
825  m = c;
826 
827  navdata_gyros_offsets_msg.offset_g.push_back(m);
828  }
829 
830  pub_navdata_gyros_offsets.publish(navdata_gyros_offsets_msg);
831  }
832 
833  //-------------------------
834 
835  if(enabled_navdata_euler_angles && pub_navdata_euler_angles.getNumSubscribers()>0)
836  {
837  navdata_euler_angles_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
838  navdata_euler_angles_msg.header.stamp = received;
839  navdata_euler_angles_msg.header.frame_id = drone_frame_base;
840 
841  {
842  uint16_t c = n.navdata_euler_angles.tag;
843  uint16_t m;
844  m = c;
845 
846  navdata_euler_angles_msg.tag = m;
847  }
848 
849  {
850  uint16_t c = n.navdata_euler_angles.size;
851  uint16_t m;
852  m = c;
853 
854  navdata_euler_angles_msg.size = m;
855  }
856 
857  {
858  float32_t c = n.navdata_euler_angles.theta_a;
859  float32_t m;
860  m = c;
861 
862  navdata_euler_angles_msg.theta_a = m;
863  }
864 
865  {
866  float32_t c = n.navdata_euler_angles.phi_a;
867  float32_t m;
868  m = c;
869 
870  navdata_euler_angles_msg.phi_a = m;
871  }
872 
873  pub_navdata_euler_angles.publish(navdata_euler_angles_msg);
874  }
875 
876  //-------------------------
877 
878  if(enabled_navdata_references && pub_navdata_references.getNumSubscribers()>0)
879  {
880  navdata_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
881  navdata_references_msg.header.stamp = received;
882  navdata_references_msg.header.frame_id = drone_frame_base;
883 
884  {
885  uint16_t c = n.navdata_references.tag;
886  uint16_t m;
887  m = c;
888 
889  navdata_references_msg.tag = m;
890  }
891 
892  {
893  uint16_t c = n.navdata_references.size;
894  uint16_t m;
895  m = c;
896 
897  navdata_references_msg.size = m;
898  }
899 
900  {
901  int32_t c = n.navdata_references.ref_theta;
902  int32_t m;
903  m = c;
904 
905  navdata_references_msg.ref_theta = m;
906  }
907 
908  {
909  int32_t c = n.navdata_references.ref_phi;
910  int32_t m;
911  m = c;
912 
913  navdata_references_msg.ref_phi = m;
914  }
915 
916  {
917  int32_t c = n.navdata_references.ref_theta_I;
918  int32_t m;
919  m = c;
920 
921  navdata_references_msg.ref_theta_I = m;
922  }
923 
924  {
925  int32_t c = n.navdata_references.ref_phi_I;
926  int32_t m;
927  m = c;
928 
929  navdata_references_msg.ref_phi_I = m;
930  }
931 
932  {
933  int32_t c = n.navdata_references.ref_pitch;
934  int32_t m;
935  m = c;
936 
937  navdata_references_msg.ref_pitch = m;
938  }
939 
940  {
941  int32_t c = n.navdata_references.ref_roll;
942  int32_t m;
943  m = c;
944 
945  navdata_references_msg.ref_roll = m;
946  }
947 
948  {
949  int32_t c = n.navdata_references.ref_yaw;
950  int32_t m;
951  m = c;
952 
953  navdata_references_msg.ref_yaw = m;
954  }
955 
956  {
957  int32_t c = n.navdata_references.ref_psi;
958  int32_t m;
959  m = c;
960 
961  navdata_references_msg.ref_psi = m;
962  }
963 
964  {
965  float32_t c = n.navdata_references.vx_ref;
966  float32_t m;
967  m = c;
968 
969  navdata_references_msg.vx_ref = m;
970  }
971 
972  {
973  float32_t c = n.navdata_references.vy_ref;
974  float32_t m;
975  m = c;
976 
977  navdata_references_msg.vy_ref = m;
978  }
979 
980  {
981  float32_t c = n.navdata_references.theta_mod;
982  float32_t m;
983  m = c;
984 
985  navdata_references_msg.theta_mod = m;
986  }
987 
988  {
989  float32_t c = n.navdata_references.phi_mod;
990  float32_t m;
991  m = c;
992 
993  navdata_references_msg.phi_mod = m;
994  }
995 
996  {
997  float32_t c = n.navdata_references.k_v_x;
998  float32_t m;
999  m = c;
1000 
1001  navdata_references_msg.k_v_x = m;
1002  }
1003 
1004  {
1005  float32_t c = n.navdata_references.k_v_y;
1006  float32_t m;
1007  m = c;
1008 
1009  navdata_references_msg.k_v_y = m;
1010  }
1011 
1012  {
1013  uint32_t c = n.navdata_references.k_mode;
1014  uint32_t m;
1015  m = c;
1016 
1017  navdata_references_msg.k_mode = m;
1018  }
1019 
1020  {
1021  float32_t c = n.navdata_references.ui_time;
1022  float32_t m;
1023  m = c;
1024 
1025  navdata_references_msg.ui_time = m;
1026  }
1027 
1028  {
1029  float32_t c = n.navdata_references.ui_theta;
1030  float32_t m;
1031  m = c;
1032 
1033  navdata_references_msg.ui_theta = m;
1034  }
1035 
1036  {
1037  float32_t c = n.navdata_references.ui_phi;
1038  float32_t m;
1039  m = c;
1040 
1041  navdata_references_msg.ui_phi = m;
1042  }
1043 
1044  {
1045  float32_t c = n.navdata_references.ui_psi;
1046  float32_t m;
1047  m = c;
1048 
1049  navdata_references_msg.ui_psi = m;
1050  }
1051 
1052  {
1053  float32_t c = n.navdata_references.ui_psi_accuracy;
1054  float32_t m;
1055  m = c;
1056 
1057  navdata_references_msg.ui_psi_accuracy = m;
1058  }
1059 
1060  {
1061  int32_t c = n.navdata_references.ui_seq;
1062  int32_t m;
1063  m = c;
1064 
1065  navdata_references_msg.ui_seq = m;
1066  }
1067 
1068  pub_navdata_references.publish(navdata_references_msg);
1069  }
1070 
1071  //-------------------------
1072 
1073  if(enabled_navdata_trims && pub_navdata_trims.getNumSubscribers()>0)
1074  {
1075  navdata_trims_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1076  navdata_trims_msg.header.stamp = received;
1077  navdata_trims_msg.header.frame_id = drone_frame_base;
1078 
1079  {
1080  uint16_t c = n.navdata_trims.tag;
1081  uint16_t m;
1082  m = c;
1083 
1084  navdata_trims_msg.tag = m;
1085  }
1086 
1087  {
1088  uint16_t c = n.navdata_trims.size;
1089  uint16_t m;
1090  m = c;
1091 
1092  navdata_trims_msg.size = m;
1093  }
1094 
1095  {
1096  float32_t c = n.navdata_trims.angular_rates_trim_r;
1097  float32_t m;
1098  m = c;
1099 
1100  navdata_trims_msg.angular_rates_trim_r = m;
1101  }
1102 
1103  {
1104  float32_t c = n.navdata_trims.euler_angles_trim_theta;
1105  float32_t m;
1106  m = c;
1107 
1108  navdata_trims_msg.euler_angles_trim_theta = m;
1109  }
1110 
1111  {
1112  float32_t c = n.navdata_trims.euler_angles_trim_phi;
1113  float32_t m;
1114  m = c;
1115 
1116  navdata_trims_msg.euler_angles_trim_phi = m;
1117  }
1118 
1119  pub_navdata_trims.publish(navdata_trims_msg);
1120  }
1121 
1122  //-------------------------
1123 
1124  if(enabled_navdata_rc_references && pub_navdata_rc_references.getNumSubscribers()>0)
1125  {
1126  navdata_rc_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1127  navdata_rc_references_msg.header.stamp = received;
1128  navdata_rc_references_msg.header.frame_id = drone_frame_base;
1129 
1130  {
1131  uint16_t c = n.navdata_rc_references.tag;
1132  uint16_t m;
1133  m = c;
1134 
1135  navdata_rc_references_msg.tag = m;
1136  }
1137 
1138  {
1139  uint16_t c = n.navdata_rc_references.size;
1140  uint16_t m;
1141  m = c;
1142 
1143  navdata_rc_references_msg.size = m;
1144  }
1145 
1146  {
1147  int32_t c = n.navdata_rc_references.rc_ref_pitch;
1148  int32_t m;
1149  m = c;
1150 
1151  navdata_rc_references_msg.rc_ref_pitch = m;
1152  }
1153 
1154  {
1155  int32_t c = n.navdata_rc_references.rc_ref_roll;
1156  int32_t m;
1157  m = c;
1158 
1159  navdata_rc_references_msg.rc_ref_roll = m;
1160  }
1161 
1162  {
1163  int32_t c = n.navdata_rc_references.rc_ref_yaw;
1164  int32_t m;
1165  m = c;
1166 
1167  navdata_rc_references_msg.rc_ref_yaw = m;
1168  }
1169 
1170  {
1171  int32_t c = n.navdata_rc_references.rc_ref_gaz;
1172  int32_t m;
1173  m = c;
1174 
1175  navdata_rc_references_msg.rc_ref_gaz = m;
1176  }
1177 
1178  {
1179  int32_t c = n.navdata_rc_references.rc_ref_ag;
1180  int32_t m;
1181  m = c;
1182 
1183  navdata_rc_references_msg.rc_ref_ag = m;
1184  }
1185 
1186  pub_navdata_rc_references.publish(navdata_rc_references_msg);
1187  }
1188 
1189  //-------------------------
1190 
1191  if(enabled_navdata_pwm && pub_navdata_pwm.getNumSubscribers()>0)
1192  {
1193  navdata_pwm_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1194  navdata_pwm_msg.header.stamp = received;
1195  navdata_pwm_msg.header.frame_id = drone_frame_base;
1196 
1197  {
1198  uint16_t c = n.navdata_pwm.tag;
1199  uint16_t m;
1200  m = c;
1201 
1202  navdata_pwm_msg.tag = m;
1203  }
1204 
1205  {
1206  uint16_t c = n.navdata_pwm.size;
1207  uint16_t m;
1208  m = c;
1209 
1210  navdata_pwm_msg.size = m;
1211  }
1212 
1213  {
1214  uint8_t c = n.navdata_pwm.motor1;
1215  uint8_t m;
1216  m = c;
1217 
1218  navdata_pwm_msg.motor1 = m;
1219  }
1220 
1221  {
1222  uint8_t c = n.navdata_pwm.motor2;
1223  uint8_t m;
1224  m = c;
1225 
1226  navdata_pwm_msg.motor2 = m;
1227  }
1228 
1229  {
1230  uint8_t c = n.navdata_pwm.motor3;
1231  uint8_t m;
1232  m = c;
1233 
1234  navdata_pwm_msg.motor3 = m;
1235  }
1236 
1237  {
1238  uint8_t c = n.navdata_pwm.motor4;
1239  uint8_t m;
1240  m = c;
1241 
1242  navdata_pwm_msg.motor4 = m;
1243  }
1244 
1245  {
1246  uint8_t c = n.navdata_pwm.sat_motor1;
1247  uint8_t m;
1248  m = c;
1249 
1250  navdata_pwm_msg.sat_motor1 = m;
1251  }
1252 
1253  {
1254  uint8_t c = n.navdata_pwm.sat_motor2;
1255  uint8_t m;
1256  m = c;
1257 
1258  navdata_pwm_msg.sat_motor2 = m;
1259  }
1260 
1261  {
1262  uint8_t c = n.navdata_pwm.sat_motor3;
1263  uint8_t m;
1264  m = c;
1265 
1266  navdata_pwm_msg.sat_motor3 = m;
1267  }
1268 
1269  {
1270  uint8_t c = n.navdata_pwm.sat_motor4;
1271  uint8_t m;
1272  m = c;
1273 
1274  navdata_pwm_msg.sat_motor4 = m;
1275  }
1276 
1277  {
1278  float32_t c = n.navdata_pwm.gaz_feed_forward;
1279  float32_t m;
1280  m = c;
1281 
1282  navdata_pwm_msg.gaz_feed_forward = m;
1283  }
1284 
1285  {
1286  float32_t c = n.navdata_pwm.gaz_altitude;
1287  float32_t m;
1288  m = c;
1289 
1290  navdata_pwm_msg.gaz_altitude = m;
1291  }
1292 
1293  {
1294  float32_t c = n.navdata_pwm.altitude_integral;
1295  float32_t m;
1296  m = c;
1297 
1298  navdata_pwm_msg.altitude_integral = m;
1299  }
1300 
1301  {
1302  float32_t c = n.navdata_pwm.vz_ref;
1303  float32_t m;
1304  m = c;
1305 
1306  navdata_pwm_msg.vz_ref = m;
1307  }
1308 
1309  {
1310  int32_t c = n.navdata_pwm.u_pitch;
1311  int32_t m;
1312  m = c;
1313 
1314  navdata_pwm_msg.u_pitch = m;
1315  }
1316 
1317  {
1318  int32_t c = n.navdata_pwm.u_roll;
1319  int32_t m;
1320  m = c;
1321 
1322  navdata_pwm_msg.u_roll = m;
1323  }
1324 
1325  {
1326  int32_t c = n.navdata_pwm.u_yaw;
1327  int32_t m;
1328  m = c;
1329 
1330  navdata_pwm_msg.u_yaw = m;
1331  }
1332 
1333  {
1334  float32_t c = n.navdata_pwm.yaw_u_I;
1335  float32_t m;
1336  m = c;
1337 
1338  navdata_pwm_msg.yaw_u_I = m;
1339  }
1340 
1341  {
1342  int32_t c = n.navdata_pwm.u_pitch_planif;
1343  int32_t m;
1344  m = c;
1345 
1346  navdata_pwm_msg.u_pitch_planif = m;
1347  }
1348 
1349  {
1350  int32_t c = n.navdata_pwm.u_roll_planif;
1351  int32_t m;
1352  m = c;
1353 
1354  navdata_pwm_msg.u_roll_planif = m;
1355  }
1356 
1357  {
1358  int32_t c = n.navdata_pwm.u_yaw_planif;
1359  int32_t m;
1360  m = c;
1361 
1362  navdata_pwm_msg.u_yaw_planif = m;
1363  }
1364 
1365  {
1366  float32_t c = n.navdata_pwm.u_gaz_planif;
1367  float32_t m;
1368  m = c;
1369 
1370  navdata_pwm_msg.u_gaz_planif = m;
1371  }
1372 
1373  {
1374  uint16_t c = n.navdata_pwm.current_motor1;
1375  uint16_t m;
1376  m = c;
1377 
1378  navdata_pwm_msg.current_motor1 = m;
1379  }
1380 
1381  {
1382  uint16_t c = n.navdata_pwm.current_motor2;
1383  uint16_t m;
1384  m = c;
1385 
1386  navdata_pwm_msg.current_motor2 = m;
1387  }
1388 
1389  {
1390  uint16_t c = n.navdata_pwm.current_motor3;
1391  uint16_t m;
1392  m = c;
1393 
1394  navdata_pwm_msg.current_motor3 = m;
1395  }
1396 
1397  {
1398  uint16_t c = n.navdata_pwm.current_motor4;
1399  uint16_t m;
1400  m = c;
1401 
1402  navdata_pwm_msg.current_motor4 = m;
1403  }
1404 
1405  {
1406  float32_t c = n.navdata_pwm.altitude_der;
1407  float32_t m;
1408  m = c;
1409 
1410  navdata_pwm_msg.altitude_der = m;
1411  }
1412 
1413  pub_navdata_pwm.publish(navdata_pwm_msg);
1414  }
1415 
1416  //-------------------------
1417 
1418  if(enabled_navdata_altitude && pub_navdata_altitude.getNumSubscribers()>0)
1419  {
1420  navdata_altitude_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1421  navdata_altitude_msg.header.stamp = received;
1422  navdata_altitude_msg.header.frame_id = drone_frame_base;
1423 
1424  {
1425  uint16_t c = n.navdata_altitude.tag;
1426  uint16_t m;
1427  m = c;
1428 
1429  navdata_altitude_msg.tag = m;
1430  }
1431 
1432  {
1433  uint16_t c = n.navdata_altitude.size;
1434  uint16_t m;
1435  m = c;
1436 
1437  navdata_altitude_msg.size = m;
1438  }
1439 
1440  {
1441  int32_t c = n.navdata_altitude.altitude_vision;
1442  int32_t m;
1443  m = c;
1444 
1445  navdata_altitude_msg.altitude_vision = m;
1446  }
1447 
1448  {
1449  float32_t c = n.navdata_altitude.altitude_vz;
1450  float32_t m;
1451  m = c;
1452 
1453  navdata_altitude_msg.altitude_vz = m;
1454  }
1455 
1456  {
1457  int32_t c = n.navdata_altitude.altitude_ref;
1458  int32_t m;
1459  m = c;
1460 
1461  navdata_altitude_msg.altitude_ref = m;
1462  }
1463 
1464  {
1465  int32_t c = n.navdata_altitude.altitude_raw;
1466  int32_t m;
1467  m = c;
1468 
1469  navdata_altitude_msg.altitude_raw = m;
1470  }
1471 
1472  {
1473  float32_t c = n.navdata_altitude.obs_accZ;
1474  float32_t m;
1475  m = c;
1476 
1477  navdata_altitude_msg.obs_accZ = m;
1478  }
1479 
1480  {
1481  float32_t c = n.navdata_altitude.obs_alt;
1482  float32_t m;
1483  m = c;
1484 
1485  navdata_altitude_msg.obs_alt = m;
1486  }
1487 
1488  {
1489  vector31_t c = n.navdata_altitude.obs_x;
1490  ardrone_autonomy::vector31 m;
1491  m.x = c.x;
1492  m.y = c.y;
1493  m.z = c.z;
1494 
1495  navdata_altitude_msg.obs_x = m;
1496  }
1497 
1498  {
1499  uint32_t c = n.navdata_altitude.obs_state;
1500  uint32_t m;
1501  m = c;
1502 
1503  navdata_altitude_msg.obs_state = m;
1504  }
1505 
1506  {
1507  vector21_t c = n.navdata_altitude.est_vb;
1508  ardrone_autonomy::vector21 m;
1509  m.x = c.x;
1510  m.y = c.y;
1511 
1512  navdata_altitude_msg.est_vb = m;
1513  }
1514 
1515  {
1516  uint32_t c = n.navdata_altitude.est_state;
1517  uint32_t m;
1518  m = c;
1519 
1520  navdata_altitude_msg.est_state = m;
1521  }
1522 
1523  pub_navdata_altitude.publish(navdata_altitude_msg);
1524  }
1525 
1526  //-------------------------
1527 
1528  if(enabled_navdata_vision_raw && pub_navdata_vision_raw.getNumSubscribers()>0)
1529  {
1530  navdata_vision_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1531  navdata_vision_raw_msg.header.stamp = received;
1532  navdata_vision_raw_msg.header.frame_id = drone_frame_base;
1533 
1534  {
1535  uint16_t c = n.navdata_vision_raw.tag;
1536  uint16_t m;
1537  m = c;
1538 
1539  navdata_vision_raw_msg.tag = m;
1540  }
1541 
1542  {
1543  uint16_t c = n.navdata_vision_raw.size;
1544  uint16_t m;
1545  m = c;
1546 
1547  navdata_vision_raw_msg.size = m;
1548  }
1549 
1550  {
1551  float32_t c = n.navdata_vision_raw.vision_tx_raw;
1552  float32_t m;
1553  m = c;
1554 
1555  navdata_vision_raw_msg.vision_tx_raw = m;
1556  }
1557 
1558  {
1559  float32_t c = n.navdata_vision_raw.vision_ty_raw;
1560  float32_t m;
1561  m = c;
1562 
1563  navdata_vision_raw_msg.vision_ty_raw = m;
1564  }
1565 
1566  {
1567  float32_t c = n.navdata_vision_raw.vision_tz_raw;
1568  float32_t m;
1569  m = c;
1570 
1571  navdata_vision_raw_msg.vision_tz_raw = m;
1572  }
1573 
1574  pub_navdata_vision_raw.publish(navdata_vision_raw_msg);
1575  }
1576 
1577  //-------------------------
1578 
1579  if(enabled_navdata_vision_of && pub_navdata_vision_of.getNumSubscribers()>0)
1580  {
1581  navdata_vision_of_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1582  navdata_vision_of_msg.header.stamp = received;
1583  navdata_vision_of_msg.header.frame_id = drone_frame_base;
1584 
1585  {
1586  uint16_t c = n.navdata_vision_of.tag;
1587  uint16_t m;
1588  m = c;
1589 
1590  navdata_vision_of_msg.tag = m;
1591  }
1592 
1593  {
1594  uint16_t c = n.navdata_vision_of.size;
1595  uint16_t m;
1596  m = c;
1597 
1598  navdata_vision_of_msg.size = m;
1599  }
1600 
1601  navdata_vision_of_msg.of_dx.clear();
1602  for(int i=0; i<5; i++)
1603  {
1604  float32_t c = n.navdata_vision_of.of_dx[i];
1605  float32_t m;
1606  m = c;
1607 
1608  navdata_vision_of_msg.of_dx.push_back(m);
1609  }
1610 
1611  navdata_vision_of_msg.of_dy.clear();
1612  for(int i=0; i<5; i++)
1613  {
1614  float32_t c = n.navdata_vision_of.of_dy[i];
1615  float32_t m;
1616  m = c;
1617 
1618  navdata_vision_of_msg.of_dy.push_back(m);
1619  }
1620 
1621  pub_navdata_vision_of.publish(navdata_vision_of_msg);
1622  }
1623 
1624  //-------------------------
1625 
1626  if(enabled_navdata_vision && pub_navdata_vision.getNumSubscribers()>0)
1627  {
1628  navdata_vision_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1629  navdata_vision_msg.header.stamp = received;
1630  navdata_vision_msg.header.frame_id = drone_frame_base;
1631 
1632  {
1633  uint16_t c = n.navdata_vision.tag;
1634  uint16_t m;
1635  m = c;
1636 
1637  navdata_vision_msg.tag = m;
1638  }
1639 
1640  {
1641  uint16_t c = n.navdata_vision.size;
1642  uint16_t m;
1643  m = c;
1644 
1645  navdata_vision_msg.size = m;
1646  }
1647 
1648  {
1649  uint32_t c = n.navdata_vision.vision_state;
1650  uint32_t m;
1651  m = c;
1652 
1653  navdata_vision_msg.vision_state = m;
1654  }
1655 
1656  {
1657  int32_t c = n.navdata_vision.vision_misc;
1658  int32_t m;
1659  m = c;
1660 
1661  navdata_vision_msg.vision_misc = m;
1662  }
1663 
1664  {
1665  float32_t c = n.navdata_vision.vision_phi_trim;
1666  float32_t m;
1667  m = c;
1668 
1669  navdata_vision_msg.vision_phi_trim = m;
1670  }
1671 
1672  {
1673  float32_t c = n.navdata_vision.vision_phi_ref_prop;
1674  float32_t m;
1675  m = c;
1676 
1677  navdata_vision_msg.vision_phi_ref_prop = m;
1678  }
1679 
1680  {
1681  float32_t c = n.navdata_vision.vision_theta_trim;
1682  float32_t m;
1683  m = c;
1684 
1685  navdata_vision_msg.vision_theta_trim = m;
1686  }
1687 
1688  {
1689  float32_t c = n.navdata_vision.vision_theta_ref_prop;
1690  float32_t m;
1691  m = c;
1692 
1693  navdata_vision_msg.vision_theta_ref_prop = m;
1694  }
1695 
1696  {
1697  int32_t c = n.navdata_vision.new_raw_picture;
1698  int32_t m;
1699  m = c;
1700 
1701  navdata_vision_msg.new_raw_picture = m;
1702  }
1703 
1704  {
1705  float32_t c = n.navdata_vision.theta_capture;
1706  float32_t m;
1707  m = c;
1708 
1709  navdata_vision_msg.theta_capture = m;
1710  }
1711 
1712  {
1713  float32_t c = n.navdata_vision.phi_capture;
1714  float32_t m;
1715  m = c;
1716 
1717  navdata_vision_msg.phi_capture = m;
1718  }
1719 
1720  {
1721  float32_t c = n.navdata_vision.psi_capture;
1722  float32_t m;
1723  m = c;
1724 
1725  navdata_vision_msg.psi_capture = m;
1726  }
1727 
1728  {
1729  int32_t c = n.navdata_vision.altitude_capture;
1730  int32_t m;
1731  m = c;
1732 
1733  navdata_vision_msg.altitude_capture = m;
1734  }
1735 
1736  {
1737  uint32_t c = n.navdata_vision.time_capture;
1738  uint32_t m;
1739  m = c;
1740 
1741  navdata_vision_msg.time_capture = m;
1742  }
1743 
1744  {
1745  velocities_t c = n.navdata_vision.body_v;
1746  ardrone_autonomy::vector31 m;
1747  m.x = c.x;
1748  m.y = c.y;
1749  m.z = c.z;
1750 
1751  navdata_vision_msg.body_v = m;
1752  }
1753 
1754  {
1755  float32_t c = n.navdata_vision.delta_phi;
1756  float32_t m;
1757  m = c;
1758 
1759  navdata_vision_msg.delta_phi = m;
1760  }
1761 
1762  {
1763  float32_t c = n.navdata_vision.delta_theta;
1764  float32_t m;
1765  m = c;
1766 
1767  navdata_vision_msg.delta_theta = m;
1768  }
1769 
1770  {
1771  float32_t c = n.navdata_vision.delta_psi;
1772  float32_t m;
1773  m = c;
1774 
1775  navdata_vision_msg.delta_psi = m;
1776  }
1777 
1778  {
1779  uint32_t c = n.navdata_vision.gold_defined;
1780  uint32_t m;
1781  m = c;
1782 
1783  navdata_vision_msg.gold_defined = m;
1784  }
1785 
1786  {
1787  uint32_t c = n.navdata_vision.gold_reset;
1788  uint32_t m;
1789  m = c;
1790 
1791  navdata_vision_msg.gold_reset = m;
1792  }
1793 
1794  {
1795  float32_t c = n.navdata_vision.gold_x;
1796  float32_t m;
1797  m = c;
1798 
1799  navdata_vision_msg.gold_x = m;
1800  }
1801 
1802  {
1803  float32_t c = n.navdata_vision.gold_y;
1804  float32_t m;
1805  m = c;
1806 
1807  navdata_vision_msg.gold_y = m;
1808  }
1809 
1810  pub_navdata_vision.publish(navdata_vision_msg);
1811  }
1812 
1813  //-------------------------
1814 
1815  if(enabled_navdata_vision_perf && pub_navdata_vision_perf.getNumSubscribers()>0)
1816  {
1817  navdata_vision_perf_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1818  navdata_vision_perf_msg.header.stamp = received;
1819  navdata_vision_perf_msg.header.frame_id = drone_frame_base;
1820 
1821  {
1822  uint16_t c = n.navdata_vision_perf.tag;
1823  uint16_t m;
1824  m = c;
1825 
1826  navdata_vision_perf_msg.tag = m;
1827  }
1828 
1829  {
1830  uint16_t c = n.navdata_vision_perf.size;
1831  uint16_t m;
1832  m = c;
1833 
1834  navdata_vision_perf_msg.size = m;
1835  }
1836 
1837  {
1838  float32_t c = n.navdata_vision_perf.time_corners;
1839  float32_t m;
1840  m = c;
1841 
1842  navdata_vision_perf_msg.time_corners = m;
1843  }
1844 
1845  {
1846  float32_t c = n.navdata_vision_perf.time_compute;
1847  float32_t m;
1848  m = c;
1849 
1850  navdata_vision_perf_msg.time_compute = m;
1851  }
1852 
1853  {
1854  float32_t c = n.navdata_vision_perf.time_tracking;
1855  float32_t m;
1856  m = c;
1857 
1858  navdata_vision_perf_msg.time_tracking = m;
1859  }
1860 
1861  {
1862  float32_t c = n.navdata_vision_perf.time_trans;
1863  float32_t m;
1864  m = c;
1865 
1866  navdata_vision_perf_msg.time_trans = m;
1867  }
1868 
1869  {
1870  float32_t c = n.navdata_vision_perf.time_update;
1871  float32_t m;
1872  m = c;
1873 
1874  navdata_vision_perf_msg.time_update = m;
1875  }
1876 
1877  navdata_vision_perf_msg.time_custom.clear();
1878  for(int i=0; i<NAVDATA_MAX_CUSTOM_TIME_SAVE; i++)
1879  {
1880  float32_t c = n.navdata_vision_perf.time_custom[i];
1881  float32_t m;
1882  m = c;
1883 
1884  navdata_vision_perf_msg.time_custom.push_back(m);
1885  }
1886 
1887  pub_navdata_vision_perf.publish(navdata_vision_perf_msg);
1888  }
1889 
1890  //-------------------------
1891 
1892  if(enabled_navdata_trackers_send && pub_navdata_trackers_send.getNumSubscribers()>0)
1893  {
1894  navdata_trackers_send_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1895  navdata_trackers_send_msg.header.stamp = received;
1896  navdata_trackers_send_msg.header.frame_id = drone_frame_base;
1897 
1898  {
1899  uint16_t c = n.navdata_trackers_send.tag;
1900  uint16_t m;
1901  m = c;
1902 
1903  navdata_trackers_send_msg.tag = m;
1904  }
1905 
1906  {
1907  uint16_t c = n.navdata_trackers_send.size;
1908  uint16_t m;
1909  m = c;
1910 
1911  navdata_trackers_send_msg.size = m;
1912  }
1913 
1914  navdata_trackers_send_msg.locked.clear();
1915  for(int i=0; i<DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT; i++)
1916  {
1917  int32_t c = n.navdata_trackers_send.locked[i];
1918  int32_t m;
1919  m = c;
1920 
1921  navdata_trackers_send_msg.locked.push_back(m);
1922  }
1923 
1924  navdata_trackers_send_msg.point.clear();
1925  for(int i=0; i<DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT; i++)
1926  {
1927  screen_point_t c = n.navdata_trackers_send.point[i];
1928  ardrone_autonomy::vector21 m;
1929  m.x = c.x;
1930  m.y = c.y;
1931 
1932  navdata_trackers_send_msg.point.push_back(m);
1933  }
1934 
1935  pub_navdata_trackers_send.publish(navdata_trackers_send_msg);
1936  }
1937 
1938  //-------------------------
1939 
1940  if(enabled_navdata_vision_detect && pub_navdata_vision_detect.getNumSubscribers()>0)
1941  {
1942  navdata_vision_detect_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
1943  navdata_vision_detect_msg.header.stamp = received;
1944  navdata_vision_detect_msg.header.frame_id = drone_frame_base;
1945 
1946  {
1947  uint16_t c = n.navdata_vision_detect.tag;
1948  uint16_t m;
1949  m = c;
1950 
1951  navdata_vision_detect_msg.tag = m;
1952  }
1953 
1954  {
1955  uint16_t c = n.navdata_vision_detect.size;
1956  uint16_t m;
1957  m = c;
1958 
1959  navdata_vision_detect_msg.size = m;
1960  }
1961 
1962  {
1963  uint32_t c = n.navdata_vision_detect.nb_detected;
1964  uint32_t m;
1965  m = c;
1966 
1967  navdata_vision_detect_msg.nb_detected = m;
1968  }
1969 
1970  navdata_vision_detect_msg.type.clear();
1971  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
1972  {
1973  uint32_t c = n.navdata_vision_detect.type[i];
1974  uint32_t m;
1975  m = c;
1976 
1977  navdata_vision_detect_msg.type.push_back(m);
1978  }
1979 
1980  navdata_vision_detect_msg.xc.clear();
1981  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
1982  {
1983  uint32_t c = n.navdata_vision_detect.xc[i];
1984  uint32_t m;
1985  m = c;
1986 
1987  navdata_vision_detect_msg.xc.push_back(m);
1988  }
1989 
1990  navdata_vision_detect_msg.yc.clear();
1991  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
1992  {
1993  uint32_t c = n.navdata_vision_detect.yc[i];
1994  uint32_t m;
1995  m = c;
1996 
1997  navdata_vision_detect_msg.yc.push_back(m);
1998  }
1999 
2000  navdata_vision_detect_msg.width.clear();
2001  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
2002  {
2003  uint32_t c = n.navdata_vision_detect.width[i];
2004  uint32_t m;
2005  m = c;
2006 
2007  navdata_vision_detect_msg.width.push_back(m);
2008  }
2009 
2010  navdata_vision_detect_msg.height.clear();
2011  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
2012  {
2013  uint32_t c = n.navdata_vision_detect.height[i];
2014  uint32_t m;
2015  m = c;
2016 
2017  navdata_vision_detect_msg.height.push_back(m);
2018  }
2019 
2020  navdata_vision_detect_msg.dist.clear();
2021  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
2022  {
2023  uint32_t c = n.navdata_vision_detect.dist[i];
2024  uint32_t m;
2025  m = c;
2026 
2027  navdata_vision_detect_msg.dist.push_back(m);
2028  }
2029 
2030  navdata_vision_detect_msg.orientation_angle.clear();
2031  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
2032  {
2033  float32_t c = n.navdata_vision_detect.orientation_angle[i];
2034  float32_t m;
2035  m = c;
2036 
2037  navdata_vision_detect_msg.orientation_angle.push_back(m);
2038  }
2039 
2040  navdata_vision_detect_msg.rotation.clear();
2041  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
2042  {
2043  matrix33_t c = n.navdata_vision_detect.rotation[i];
2044  ardrone_autonomy::matrix33 m;
2045  m.m11 = c.m11;
2046  m.m12 = c.m12;
2047  m.m13 = c.m13;
2048  m.m21 = c.m21;
2049  m.m22 = c.m22;
2050  m.m23 = c.m23;
2051  m.m31 = c.m31;
2052  m.m32 = c.m32;
2053  m.m33 = c.m33;
2054 
2055  navdata_vision_detect_msg.rotation.push_back(m);
2056  }
2057 
2058  navdata_vision_detect_msg.translation.clear();
2059  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
2060  {
2061  vector31_t c = n.navdata_vision_detect.translation[i];
2062  ardrone_autonomy::vector31 m;
2063  m.x = c.x;
2064  m.y = c.y;
2065  m.z = c.z;
2066 
2067  navdata_vision_detect_msg.translation.push_back(m);
2068  }
2069 
2070  navdata_vision_detect_msg.camera_source.clear();
2071  for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
2072  {
2073  uint32_t c = n.navdata_vision_detect.camera_source[i];
2074  uint32_t m;
2075  m = c;
2076 
2077  navdata_vision_detect_msg.camera_source.push_back(m);
2078  }
2079 
2080  pub_navdata_vision_detect.publish(navdata_vision_detect_msg);
2081  }
2082 
2083  //-------------------------
2084 
2085  if(enabled_navdata_watchdog && pub_navdata_watchdog.getNumSubscribers()>0)
2086  {
2087  navdata_watchdog_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2088  navdata_watchdog_msg.header.stamp = received;
2089  navdata_watchdog_msg.header.frame_id = drone_frame_base;
2090 
2091  {
2092  uint16_t c = n.navdata_watchdog.tag;
2093  uint16_t m;
2094  m = c;
2095 
2096  navdata_watchdog_msg.tag = m;
2097  }
2098 
2099  {
2100  uint16_t c = n.navdata_watchdog.size;
2101  uint16_t m;
2102  m = c;
2103 
2104  navdata_watchdog_msg.size = m;
2105  }
2106 
2107  pub_navdata_watchdog.publish(navdata_watchdog_msg);
2108  }
2109 
2110  //-------------------------
2111 
2112  if(enabled_navdata_adc_data_frame && pub_navdata_adc_data_frame.getNumSubscribers()>0)
2113  {
2114  navdata_adc_data_frame_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2115  navdata_adc_data_frame_msg.header.stamp = received;
2116  navdata_adc_data_frame_msg.header.frame_id = drone_frame_base;
2117 
2118  {
2119  uint16_t c = n.navdata_adc_data_frame.tag;
2120  uint16_t m;
2121  m = c;
2122 
2123  navdata_adc_data_frame_msg.tag = m;
2124  }
2125 
2126  {
2127  uint16_t c = n.navdata_adc_data_frame.size;
2128  uint16_t m;
2129  m = c;
2130 
2131  navdata_adc_data_frame_msg.size = m;
2132  }
2133 
2134  {
2135  uint32_t c = n.navdata_adc_data_frame.version;
2136  uint32_t m;
2137  m = c;
2138 
2139  navdata_adc_data_frame_msg.version = m;
2140  }
2141 
2142  navdata_adc_data_frame_msg.data_frame.clear();
2143  for(int i=0; i<32; i++)
2144  {
2145  uint8_t c = n.navdata_adc_data_frame.data_frame[i];
2146  uint8_t m;
2147  m = c;
2148 
2149  navdata_adc_data_frame_msg.data_frame.push_back(m);
2150  }
2151 
2152  pub_navdata_adc_data_frame.publish(navdata_adc_data_frame_msg);
2153  }
2154 
2155  //-------------------------
2156 
2157  if(enabled_navdata_video_stream && pub_navdata_video_stream.getNumSubscribers()>0)
2158  {
2159  navdata_video_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2160  navdata_video_stream_msg.header.stamp = received;
2161  navdata_video_stream_msg.header.frame_id = drone_frame_base;
2162 
2163  {
2164  uint16_t c = n.navdata_video_stream.tag;
2165  uint16_t m;
2166  m = c;
2167 
2168  navdata_video_stream_msg.tag = m;
2169  }
2170 
2171  {
2172  uint16_t c = n.navdata_video_stream.size;
2173  uint16_t m;
2174  m = c;
2175 
2176  navdata_video_stream_msg.size = m;
2177  }
2178 
2179  {
2180  uint8_t c = n.navdata_video_stream.quant;
2181  uint8_t m;
2182  m = c;
2183 
2184  navdata_video_stream_msg.quant = m;
2185  }
2186 
2187  {
2188  uint32_t c = n.navdata_video_stream.frame_size;
2189  uint32_t m;
2190  m = c;
2191 
2192  navdata_video_stream_msg.frame_size = m;
2193  }
2194 
2195  {
2196  uint32_t c = n.navdata_video_stream.frame_number;
2197  uint32_t m;
2198  m = c;
2199 
2200  navdata_video_stream_msg.frame_number = m;
2201  }
2202 
2203  {
2204  uint32_t c = n.navdata_video_stream.atcmd_ref_seq;
2205  uint32_t m;
2206  m = c;
2207 
2208  navdata_video_stream_msg.atcmd_ref_seq = m;
2209  }
2210 
2211  {
2212  uint32_t c = n.navdata_video_stream.atcmd_mean_ref_gap;
2213  uint32_t m;
2214  m = c;
2215 
2216  navdata_video_stream_msg.atcmd_mean_ref_gap = m;
2217  }
2218 
2219  {
2220  float32_t c = n.navdata_video_stream.atcmd_var_ref_gap;
2221  float32_t m;
2222  m = c;
2223 
2224  navdata_video_stream_msg.atcmd_var_ref_gap = m;
2225  }
2226 
2227  {
2228  uint32_t c = n.navdata_video_stream.atcmd_ref_quality;
2229  uint32_t m;
2230  m = c;
2231 
2232  navdata_video_stream_msg.atcmd_ref_quality = m;
2233  }
2234 
2235  {
2236  uint32_t c = n.navdata_video_stream.desired_bitrate;
2237  uint32_t m;
2238  m = c;
2239 
2240  navdata_video_stream_msg.desired_bitrate = m;
2241  }
2242 
2243  {
2244  int32_t c = n.navdata_video_stream.data2;
2245  int32_t m;
2246  m = c;
2247 
2248  navdata_video_stream_msg.data2 = m;
2249  }
2250 
2251  {
2252  int32_t c = n.navdata_video_stream.data3;
2253  int32_t m;
2254  m = c;
2255 
2256  navdata_video_stream_msg.data3 = m;
2257  }
2258 
2259  {
2260  int32_t c = n.navdata_video_stream.data4;
2261  int32_t m;
2262  m = c;
2263 
2264  navdata_video_stream_msg.data4 = m;
2265  }
2266 
2267  {
2268  int32_t c = n.navdata_video_stream.data5;
2269  int32_t m;
2270  m = c;
2271 
2272  navdata_video_stream_msg.data5 = m;
2273  }
2274 
2275  {
2276  uint32_t c = n.navdata_video_stream.fifo_queue_level;
2277  uint32_t m;
2278  m = c;
2279 
2280  navdata_video_stream_msg.fifo_queue_level = m;
2281  }
2282 
2283  pub_navdata_video_stream.publish(navdata_video_stream_msg);
2284  }
2285 
2286  //-------------------------
2287 
2288  if(enabled_navdata_games && pub_navdata_games.getNumSubscribers()>0)
2289  {
2290  navdata_games_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2291  navdata_games_msg.header.stamp = received;
2292  navdata_games_msg.header.frame_id = drone_frame_base;
2293 
2294  {
2295  uint16_t c = n.navdata_games.tag;
2296  uint16_t m;
2297  m = c;
2298 
2299  navdata_games_msg.tag = m;
2300  }
2301 
2302  {
2303  uint16_t c = n.navdata_games.size;
2304  uint16_t m;
2305  m = c;
2306 
2307  navdata_games_msg.size = m;
2308  }
2309 
2310  {
2311  uint32_t c = n.navdata_games.double_tap_counter;
2312  uint32_t m;
2313  m = c;
2314 
2315  navdata_games_msg.double_tap_counter = m;
2316  }
2317 
2318  {
2319  uint32_t c = n.navdata_games.finish_line_counter;
2320  uint32_t m;
2321  m = c;
2322 
2323  navdata_games_msg.finish_line_counter = m;
2324  }
2325 
2326  pub_navdata_games.publish(navdata_games_msg);
2327  }
2328 
2329  //-------------------------
2330 
2331  if(enabled_navdata_pressure_raw && pub_navdata_pressure_raw.getNumSubscribers()>0)
2332  {
2333  navdata_pressure_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2334  navdata_pressure_raw_msg.header.stamp = received;
2335  navdata_pressure_raw_msg.header.frame_id = drone_frame_base;
2336 
2337  {
2338  uint16_t c = n.navdata_pressure_raw.tag;
2339  uint16_t m;
2340  m = c;
2341 
2342  navdata_pressure_raw_msg.tag = m;
2343  }
2344 
2345  {
2346  uint16_t c = n.navdata_pressure_raw.size;
2347  uint16_t m;
2348  m = c;
2349 
2350  navdata_pressure_raw_msg.size = m;
2351  }
2352 
2353  {
2354  int32_t c = n.navdata_pressure_raw.up;
2355  int32_t m;
2356  m = c;
2357 
2358  navdata_pressure_raw_msg.up = m;
2359  }
2360 
2361  {
2362  int16_t c = n.navdata_pressure_raw.ut;
2363  int16_t m;
2364  m = c;
2365 
2366  navdata_pressure_raw_msg.ut = m;
2367  }
2368 
2369  {
2370  int32_t c = n.navdata_pressure_raw.Temperature_meas;
2371  int32_t m;
2372  m = c;
2373 
2374  navdata_pressure_raw_msg.Temperature_meas = m;
2375  }
2376 
2377  {
2378  int32_t c = n.navdata_pressure_raw.Pression_meas;
2379  int32_t m;
2380  m = c;
2381 
2382  navdata_pressure_raw_msg.Pression_meas = m;
2383  }
2384 
2385  pub_navdata_pressure_raw.publish(navdata_pressure_raw_msg);
2386  }
2387 
2388  //-------------------------
2389 
2390  if(enabled_navdata_magneto && pub_navdata_magneto.getNumSubscribers()>0)
2391  {
2392  navdata_magneto_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2393  navdata_magneto_msg.header.stamp = received;
2394  navdata_magneto_msg.header.frame_id = drone_frame_base;
2395 
2396  {
2397  uint16_t c = n.navdata_magneto.tag;
2398  uint16_t m;
2399  m = c;
2400 
2401  navdata_magneto_msg.tag = m;
2402  }
2403 
2404  {
2405  uint16_t c = n.navdata_magneto.size;
2406  uint16_t m;
2407  m = c;
2408 
2409  navdata_magneto_msg.size = m;
2410  }
2411 
2412  {
2413  int16_t c = n.navdata_magneto.mx;
2414  int16_t m;
2415  m = c;
2416 
2417  navdata_magneto_msg.mx = m;
2418  }
2419 
2420  {
2421  int16_t c = n.navdata_magneto.my;
2422  int16_t m;
2423  m = c;
2424 
2425  navdata_magneto_msg.my = m;
2426  }
2427 
2428  {
2429  int16_t c = n.navdata_magneto.mz;
2430  int16_t m;
2431  m = c;
2432 
2433  navdata_magneto_msg.mz = m;
2434  }
2435 
2436  {
2437  vector31_t c = n.navdata_magneto.magneto_raw;
2438  ardrone_autonomy::vector31 m;
2439  m.x = c.x;
2440  m.y = c.y;
2441  m.z = c.z;
2442 
2443  navdata_magneto_msg.magneto_raw = m;
2444  }
2445 
2446  {
2447  vector31_t c = n.navdata_magneto.magneto_rectified;
2448  ardrone_autonomy::vector31 m;
2449  m.x = c.x;
2450  m.y = c.y;
2451  m.z = c.z;
2452 
2453  navdata_magneto_msg.magneto_rectified = m;
2454  }
2455 
2456  {
2457  vector31_t c = n.navdata_magneto.magneto_offset;
2458  ardrone_autonomy::vector31 m;
2459  m.x = c.x;
2460  m.y = c.y;
2461  m.z = c.z;
2462 
2463  navdata_magneto_msg.magneto_offset = m;
2464  }
2465 
2466  {
2467  float32_t c = n.navdata_magneto.heading_unwrapped;
2468  float32_t m;
2469  m = c;
2470 
2471  navdata_magneto_msg.heading_unwrapped = m;
2472  }
2473 
2474  {
2475  float32_t c = n.navdata_magneto.heading_gyro_unwrapped;
2476  float32_t m;
2477  m = c;
2478 
2479  navdata_magneto_msg.heading_gyro_unwrapped = m;
2480  }
2481 
2482  {
2483  float32_t c = n.navdata_magneto.heading_fusion_unwrapped;
2484  float32_t m;
2485  m = c;
2486 
2487  navdata_magneto_msg.heading_fusion_unwrapped = m;
2488  }
2489 
2490  {
2491  char c = n.navdata_magneto.magneto_calibration_ok;
2492  char m;
2493  m = c;
2494 
2495  navdata_magneto_msg.magneto_calibration_ok = m;
2496  }
2497 
2498  {
2499  uint32_t c = n.navdata_magneto.magneto_state;
2500  uint32_t m;
2501  m = c;
2502 
2503  navdata_magneto_msg.magneto_state = m;
2504  }
2505 
2506  {
2507  float32_t c = n.navdata_magneto.magneto_radius;
2508  float32_t m;
2509  m = c;
2510 
2511  navdata_magneto_msg.magneto_radius = m;
2512  }
2513 
2514  {
2515  float32_t c = n.navdata_magneto.error_mean;
2516  float32_t m;
2517  m = c;
2518 
2519  navdata_magneto_msg.error_mean = m;
2520  }
2521 
2522  {
2523  float32_t c = n.navdata_magneto.error_var;
2524  float32_t m;
2525  m = c;
2526 
2527  navdata_magneto_msg.error_var = m;
2528  }
2529 
2530  pub_navdata_magneto.publish(navdata_magneto_msg);
2531  }
2532 
2533  //-------------------------
2534 
2535  if(enabled_navdata_wind_speed && pub_navdata_wind_speed.getNumSubscribers()>0)
2536  {
2537  navdata_wind_speed_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2538  navdata_wind_speed_msg.header.stamp = received;
2539  navdata_wind_speed_msg.header.frame_id = drone_frame_base;
2540 
2541  {
2542  uint16_t c = n.navdata_wind_speed.tag;
2543  uint16_t m;
2544  m = c;
2545 
2546  navdata_wind_speed_msg.tag = m;
2547  }
2548 
2549  {
2550  uint16_t c = n.navdata_wind_speed.size;
2551  uint16_t m;
2552  m = c;
2553 
2554  navdata_wind_speed_msg.size = m;
2555  }
2556 
2557  {
2558  float32_t c = n.navdata_wind_speed.wind_speed;
2559  float32_t m;
2560  m = c;
2561 
2562  navdata_wind_speed_msg.wind_speed = m;
2563  }
2564 
2565  {
2566  float32_t c = n.navdata_wind_speed.wind_angle;
2567  float32_t m;
2568  m = c;
2569 
2570  navdata_wind_speed_msg.wind_angle = m;
2571  }
2572 
2573  {
2574  float32_t c = n.navdata_wind_speed.wind_compensation_theta;
2575  float32_t m;
2576  m = c;
2577 
2578  navdata_wind_speed_msg.wind_compensation_theta = m;
2579  }
2580 
2581  {
2582  float32_t c = n.navdata_wind_speed.wind_compensation_phi;
2583  float32_t m;
2584  m = c;
2585 
2586  navdata_wind_speed_msg.wind_compensation_phi = m;
2587  }
2588 
2589  {
2590  float32_t c = n.navdata_wind_speed.state_x1;
2591  float32_t m;
2592  m = c;
2593 
2594  navdata_wind_speed_msg.state_x1 = m;
2595  }
2596 
2597  {
2598  float32_t c = n.navdata_wind_speed.state_x2;
2599  float32_t m;
2600  m = c;
2601 
2602  navdata_wind_speed_msg.state_x2 = m;
2603  }
2604 
2605  {
2606  float32_t c = n.navdata_wind_speed.state_x3;
2607  float32_t m;
2608  m = c;
2609 
2610  navdata_wind_speed_msg.state_x3 = m;
2611  }
2612 
2613  {
2614  float32_t c = n.navdata_wind_speed.state_x4;
2615  float32_t m;
2616  m = c;
2617 
2618  navdata_wind_speed_msg.state_x4 = m;
2619  }
2620 
2621  {
2622  float32_t c = n.navdata_wind_speed.state_x5;
2623  float32_t m;
2624  m = c;
2625 
2626  navdata_wind_speed_msg.state_x5 = m;
2627  }
2628 
2629  {
2630  float32_t c = n.navdata_wind_speed.state_x6;
2631  float32_t m;
2632  m = c;
2633 
2634  navdata_wind_speed_msg.state_x6 = m;
2635  }
2636 
2637  {
2638  float32_t c = n.navdata_wind_speed.magneto_debug1;
2639  float32_t m;
2640  m = c;
2641 
2642  navdata_wind_speed_msg.magneto_debug1 = m;
2643  }
2644 
2645  {
2646  float32_t c = n.navdata_wind_speed.magneto_debug2;
2647  float32_t m;
2648  m = c;
2649 
2650  navdata_wind_speed_msg.magneto_debug2 = m;
2651  }
2652 
2653  {
2654  float32_t c = n.navdata_wind_speed.magneto_debug3;
2655  float32_t m;
2656  m = c;
2657 
2658  navdata_wind_speed_msg.magneto_debug3 = m;
2659  }
2660 
2661  pub_navdata_wind_speed.publish(navdata_wind_speed_msg);
2662  }
2663 
2664  //-------------------------
2665 
2666  if(enabled_navdata_kalman_pressure && pub_navdata_kalman_pressure.getNumSubscribers()>0)
2667  {
2668  navdata_kalman_pressure_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2669  navdata_kalman_pressure_msg.header.stamp = received;
2670  navdata_kalman_pressure_msg.header.frame_id = drone_frame_base;
2671 
2672  {
2673  uint16_t c = n.navdata_kalman_pressure.tag;
2674  uint16_t m;
2675  m = c;
2676 
2677  navdata_kalman_pressure_msg.tag = m;
2678  }
2679 
2680  {
2681  uint16_t c = n.navdata_kalman_pressure.size;
2682  uint16_t m;
2683  m = c;
2684 
2685  navdata_kalman_pressure_msg.size = m;
2686  }
2687 
2688  {
2689  float32_t c = n.navdata_kalman_pressure.offset_pressure;
2690  float32_t m;
2691  m = c;
2692 
2693  navdata_kalman_pressure_msg.offset_pressure = m;
2694  }
2695 
2696  {
2697  float32_t c = n.navdata_kalman_pressure.est_z;
2698  float32_t m;
2699  m = c;
2700 
2701  navdata_kalman_pressure_msg.est_z = m;
2702  }
2703 
2704  {
2705  float32_t c = n.navdata_kalman_pressure.est_zdot;
2706  float32_t m;
2707  m = c;
2708 
2709  navdata_kalman_pressure_msg.est_zdot = m;
2710  }
2711 
2712  {
2713  float32_t c = n.navdata_kalman_pressure.est_bias_PWM;
2714  float32_t m;
2715  m = c;
2716 
2717  navdata_kalman_pressure_msg.est_bias_PWM = m;
2718  }
2719 
2720  {
2721  float32_t c = n.navdata_kalman_pressure.est_biais_pression;
2722  float32_t m;
2723  m = c;
2724 
2725  navdata_kalman_pressure_msg.est_biais_pression = m;
2726  }
2727 
2728  {
2729  float32_t c = n.navdata_kalman_pressure.offset_US;
2730  float32_t m;
2731  m = c;
2732 
2733  navdata_kalman_pressure_msg.offset_US = m;
2734  }
2735 
2736  {
2737  float32_t c = n.navdata_kalman_pressure.prediction_US;
2738  float32_t m;
2739  m = c;
2740 
2741  navdata_kalman_pressure_msg.prediction_US = m;
2742  }
2743 
2744  {
2745  float32_t c = n.navdata_kalman_pressure.cov_alt;
2746  float32_t m;
2747  m = c;
2748 
2749  navdata_kalman_pressure_msg.cov_alt = m;
2750  }
2751 
2752  {
2753  float32_t c = n.navdata_kalman_pressure.cov_PWM;
2754  float32_t m;
2755  m = c;
2756 
2757  navdata_kalman_pressure_msg.cov_PWM = m;
2758  }
2759 
2760  {
2761  float32_t c = n.navdata_kalman_pressure.cov_vitesse;
2762  float32_t m;
2763  m = c;
2764 
2765  navdata_kalman_pressure_msg.cov_vitesse = m;
2766  }
2767 
2768  {
2769  bool_t c = n.navdata_kalman_pressure.bool_effet_sol;
2770  bool_t m;
2771  m = c;
2772 
2773  navdata_kalman_pressure_msg.bool_effet_sol = m;
2774  }
2775 
2776  {
2777  float32_t c = n.navdata_kalman_pressure.somme_inno;
2778  float32_t m;
2779  m = c;
2780 
2781  navdata_kalman_pressure_msg.somme_inno = m;
2782  }
2783 
2784  {
2785  bool_t c = n.navdata_kalman_pressure.flag_rejet_US;
2786  bool_t m;
2787  m = c;
2788 
2789  navdata_kalman_pressure_msg.flag_rejet_US = m;
2790  }
2791 
2792  {
2793  float32_t c = n.navdata_kalman_pressure.u_multisinus;
2794  float32_t m;
2795  m = c;
2796 
2797  navdata_kalman_pressure_msg.u_multisinus = m;
2798  }
2799 
2800  {
2801  float32_t c = n.navdata_kalman_pressure.gaz_altitude;
2802  float32_t m;
2803  m = c;
2804 
2805  navdata_kalman_pressure_msg.gaz_altitude = m;
2806  }
2807 
2808  {
2809  bool_t c = n.navdata_kalman_pressure.Flag_multisinus;
2810  bool_t m;
2811  m = c;
2812 
2813  navdata_kalman_pressure_msg.Flag_multisinus = m;
2814  }
2815 
2816  {
2817  bool_t c = n.navdata_kalman_pressure.Flag_multisinus_debut;
2818  bool_t m;
2819  m = c;
2820 
2821  navdata_kalman_pressure_msg.Flag_multisinus_debut = m;
2822  }
2823 
2824  pub_navdata_kalman_pressure.publish(navdata_kalman_pressure_msg);
2825  }
2826 
2827  //-------------------------
2828 
2829  if(enabled_navdata_hdvideo_stream && pub_navdata_hdvideo_stream.getNumSubscribers()>0)
2830  {
2831  navdata_hdvideo_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2832  navdata_hdvideo_stream_msg.header.stamp = received;
2833  navdata_hdvideo_stream_msg.header.frame_id = drone_frame_base;
2834 
2835  {
2836  uint16_t c = n.navdata_hdvideo_stream.tag;
2837  uint16_t m;
2838  m = c;
2839 
2840  navdata_hdvideo_stream_msg.tag = m;
2841  }
2842 
2843  {
2844  uint16_t c = n.navdata_hdvideo_stream.size;
2845  uint16_t m;
2846  m = c;
2847 
2848  navdata_hdvideo_stream_msg.size = m;
2849  }
2850 
2851  {
2852  uint32_t c = n.navdata_hdvideo_stream.hdvideo_state;
2853  uint32_t m;
2854  m = c;
2855 
2856  navdata_hdvideo_stream_msg.hdvideo_state = m;
2857  }
2858 
2859  {
2860  uint32_t c = n.navdata_hdvideo_stream.storage_fifo_nb_packets;
2861  uint32_t m;
2862  m = c;
2863 
2864  navdata_hdvideo_stream_msg.storage_fifo_nb_packets = m;
2865  }
2866 
2867  {
2868  uint32_t c = n.navdata_hdvideo_stream.storage_fifo_size;
2869  uint32_t m;
2870  m = c;
2871 
2872  navdata_hdvideo_stream_msg.storage_fifo_size = m;
2873  }
2874 
2875  {
2876  uint32_t c = n.navdata_hdvideo_stream.usbkey_size;
2877  uint32_t m;
2878  m = c;
2879 
2880  navdata_hdvideo_stream_msg.usbkey_size = m;
2881  }
2882 
2883  {
2884  uint32_t c = n.navdata_hdvideo_stream.usbkey_freespace;
2885  uint32_t m;
2886  m = c;
2887 
2888  navdata_hdvideo_stream_msg.usbkey_freespace = m;
2889  }
2890 
2891  {
2892  uint32_t c = n.navdata_hdvideo_stream.frame_number;
2893  uint32_t m;
2894  m = c;
2895 
2896  navdata_hdvideo_stream_msg.frame_number = m;
2897  }
2898 
2899  {
2900  uint32_t c = n.navdata_hdvideo_stream.usbkey_remaining_time;
2901  uint32_t m;
2902  m = c;
2903 
2904  navdata_hdvideo_stream_msg.usbkey_remaining_time = m;
2905  }
2906 
2907  pub_navdata_hdvideo_stream.publish(navdata_hdvideo_stream_msg);
2908  }
2909 
2910  //-------------------------
2911 
2912  if(enabled_navdata_wifi && pub_navdata_wifi.getNumSubscribers()>0)
2913  {
2914  navdata_wifi_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2915  navdata_wifi_msg.header.stamp = received;
2916  navdata_wifi_msg.header.frame_id = drone_frame_base;
2917 
2918  {
2919  uint16_t c = n.navdata_wifi.tag;
2920  uint16_t m;
2921  m = c;
2922 
2923  navdata_wifi_msg.tag = m;
2924  }
2925 
2926  {
2927  uint16_t c = n.navdata_wifi.size;
2928  uint16_t m;
2929  m = c;
2930 
2931  navdata_wifi_msg.size = m;
2932  }
2933 
2934  {
2935  uint32_t c = n.navdata_wifi.link_quality;
2936  uint32_t m;
2937  m = c;
2938 
2939  navdata_wifi_msg.link_quality = m;
2940  }
2941 
2942  pub_navdata_wifi.publish(navdata_wifi_msg);
2943  }
2944 
2945  //-------------------------
2946 
2947  if(enabled_navdata_zimmu_3000 && pub_navdata_zimmu_3000.getNumSubscribers()>0)
2948  {
2949  navdata_zimmu_3000_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
2950  navdata_zimmu_3000_msg.header.stamp = received;
2951  navdata_zimmu_3000_msg.header.frame_id = drone_frame_base;
2952 
2953  {
2954  uint16_t c = n.navdata_zimmu_3000.tag;
2955  uint16_t m;
2956  m = c;
2957 
2958  navdata_zimmu_3000_msg.tag = m;
2959  }
2960 
2961  {
2962  uint16_t c = n.navdata_zimmu_3000.size;
2963  uint16_t m;
2964  m = c;
2965 
2966  navdata_zimmu_3000_msg.size = m;
2967  }
2968 
2969  {
2970  int32_t c = n.navdata_zimmu_3000.vzimmuLSB;
2971  int32_t m;
2972  m = c;
2973 
2974  navdata_zimmu_3000_msg.vzimmuLSB = m;
2975  }
2976 
2977  {
2978  float32_t c = n.navdata_zimmu_3000.vzfind;
2979  float32_t m;
2980  m = c;
2981 
2982  navdata_zimmu_3000_msg.vzfind = m;
2983  }
2984 
2985  pub_navdata_zimmu_3000.publish(navdata_zimmu_3000_msg);
2986  }
2987 
2988  //-------------------------
2989 
2990  }
2991 }
2992 #endif
ros::param::param
bool param(const std::string &param_name, T &param_val, const T &default_val)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ARDroneDriver::drone_frame_base
std::string drone_frame_base
Definition: ardrone_driver.h:133
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
NAVDATA_QUEUE_SIZE
#define NAVDATA_QUEUE_SIZE
Definition: ardrone_driver.h:64

ardrone_autonomy
Author(s): Mani Monajjemi, Mani Monajjemi
autogenerated on Mon Jun 10 2019 12:39:49