00001
00025
00026
00027 #ifdef NAVDATA_STRUCTS_INCLUDES
00028 #include "ardrone_autonomy/navdata_demo.h"
00029 #include "ardrone_autonomy/navdata_time.h"
00030 #include "ardrone_autonomy/navdata_raw_measures.h"
00031 #include "ardrone_autonomy/navdata_phys_measures.h"
00032 #include "ardrone_autonomy/navdata_gyros_offsets.h"
00033 #include "ardrone_autonomy/navdata_euler_angles.h"
00034 #include "ardrone_autonomy/navdata_references.h"
00035 #include "ardrone_autonomy/navdata_trims.h"
00036 #include "ardrone_autonomy/navdata_rc_references.h"
00037 #include "ardrone_autonomy/navdata_pwm.h"
00038 #include "ardrone_autonomy/navdata_altitude.h"
00039 #include "ardrone_autonomy/navdata_vision_raw.h"
00040 #include "ardrone_autonomy/navdata_vision_of.h"
00041 #include "ardrone_autonomy/navdata_vision.h"
00042 #include "ardrone_autonomy/navdata_vision_perf.h"
00043 #include "ardrone_autonomy/navdata_trackers_send.h"
00044 #include "ardrone_autonomy/navdata_vision_detect.h"
00045 #include "ardrone_autonomy/navdata_watchdog.h"
00046 #include "ardrone_autonomy/navdata_adc_data_frame.h"
00047 #include "ardrone_autonomy/navdata_video_stream.h"
00048 #include "ardrone_autonomy/navdata_games.h"
00049 #include "ardrone_autonomy/navdata_pressure_raw.h"
00050 #include "ardrone_autonomy/navdata_magneto.h"
00051 #include "ardrone_autonomy/navdata_wind_speed.h"
00052 #include "ardrone_autonomy/navdata_kalman_pressure.h"
00053 #include "ardrone_autonomy/navdata_hdvideo_stream.h"
00054 #include "ardrone_autonomy/navdata_wifi.h"
00055 #include "ardrone_autonomy/navdata_zimmu_3000.h"
00056 #include <std_msgs/UInt16.h>
00057 #include <std_msgs/UInt32.h>
00058 #include <std_msgs/Float32.h>
00059 #include <std_msgs/Int32.h>
00060 #include <std_msgs/Int16.h>
00061 #include <std_msgs/UInt8.h>
00062 #include <ardrone_autonomy/vector31.h>
00063 #include <ardrone_autonomy/vector21.h>
00064 #include <ardrone_autonomy/matrix33.h>
00065 #endif
00066
00067 #ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
00068 void PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received);
00069 #endif
00070
00071 #ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
00072 ros::Publisher pub_navdata_demo;
00073 bool enabled_navdata_demo;
00074 ardrone_autonomy::navdata_demo navdata_demo_msg;
00075 ros::Publisher pub_navdata_time;
00076 bool enabled_navdata_time;
00077 ardrone_autonomy::navdata_time navdata_time_msg;
00078 ros::Publisher pub_navdata_raw_measures;
00079 bool enabled_navdata_raw_measures;
00080 ardrone_autonomy::navdata_raw_measures navdata_raw_measures_msg;
00081 ros::Publisher pub_navdata_phys_measures;
00082 bool enabled_navdata_phys_measures;
00083 ardrone_autonomy::navdata_phys_measures navdata_phys_measures_msg;
00084 ros::Publisher pub_navdata_gyros_offsets;
00085 bool enabled_navdata_gyros_offsets;
00086 ardrone_autonomy::navdata_gyros_offsets navdata_gyros_offsets_msg;
00087 ros::Publisher pub_navdata_euler_angles;
00088 bool enabled_navdata_euler_angles;
00089 ardrone_autonomy::navdata_euler_angles navdata_euler_angles_msg;
00090 ros::Publisher pub_navdata_references;
00091 bool enabled_navdata_references;
00092 ardrone_autonomy::navdata_references navdata_references_msg;
00093 ros::Publisher pub_navdata_trims;
00094 bool enabled_navdata_trims;
00095 ardrone_autonomy::navdata_trims navdata_trims_msg;
00096 ros::Publisher pub_navdata_rc_references;
00097 bool enabled_navdata_rc_references;
00098 ardrone_autonomy::navdata_rc_references navdata_rc_references_msg;
00099 ros::Publisher pub_navdata_pwm;
00100 bool enabled_navdata_pwm;
00101 ardrone_autonomy::navdata_pwm navdata_pwm_msg;
00102 ros::Publisher pub_navdata_altitude;
00103 bool enabled_navdata_altitude;
00104 ardrone_autonomy::navdata_altitude navdata_altitude_msg;
00105 ros::Publisher pub_navdata_vision_raw;
00106 bool enabled_navdata_vision_raw;
00107 ardrone_autonomy::navdata_vision_raw navdata_vision_raw_msg;
00108 ros::Publisher pub_navdata_vision_of;
00109 bool enabled_navdata_vision_of;
00110 ardrone_autonomy::navdata_vision_of navdata_vision_of_msg;
00111 ros::Publisher pub_navdata_vision;
00112 bool enabled_navdata_vision;
00113 ardrone_autonomy::navdata_vision navdata_vision_msg;
00114 ros::Publisher pub_navdata_vision_perf;
00115 bool enabled_navdata_vision_perf;
00116 ardrone_autonomy::navdata_vision_perf navdata_vision_perf_msg;
00117 ros::Publisher pub_navdata_trackers_send;
00118 bool enabled_navdata_trackers_send;
00119 ardrone_autonomy::navdata_trackers_send navdata_trackers_send_msg;
00120 ros::Publisher pub_navdata_vision_detect;
00121 bool enabled_navdata_vision_detect;
00122 ardrone_autonomy::navdata_vision_detect navdata_vision_detect_msg;
00123 ros::Publisher pub_navdata_watchdog;
00124 bool enabled_navdata_watchdog;
00125 ardrone_autonomy::navdata_watchdog navdata_watchdog_msg;
00126 ros::Publisher pub_navdata_adc_data_frame;
00127 bool enabled_navdata_adc_data_frame;
00128 ardrone_autonomy::navdata_adc_data_frame navdata_adc_data_frame_msg;
00129 ros::Publisher pub_navdata_video_stream;
00130 bool enabled_navdata_video_stream;
00131 ardrone_autonomy::navdata_video_stream navdata_video_stream_msg;
00132 ros::Publisher pub_navdata_games;
00133 bool enabled_navdata_games;
00134 ardrone_autonomy::navdata_games navdata_games_msg;
00135 ros::Publisher pub_navdata_pressure_raw;
00136 bool enabled_navdata_pressure_raw;
00137 ardrone_autonomy::navdata_pressure_raw navdata_pressure_raw_msg;
00138 ros::Publisher pub_navdata_magneto;
00139 bool enabled_navdata_magneto;
00140 ardrone_autonomy::navdata_magneto navdata_magneto_msg;
00141 ros::Publisher pub_navdata_wind_speed;
00142 bool enabled_navdata_wind_speed;
00143 ardrone_autonomy::navdata_wind_speed navdata_wind_speed_msg;
00144 ros::Publisher pub_navdata_kalman_pressure;
00145 bool enabled_navdata_kalman_pressure;
00146 ardrone_autonomy::navdata_kalman_pressure navdata_kalman_pressure_msg;
00147 ros::Publisher pub_navdata_hdvideo_stream;
00148 bool enabled_navdata_hdvideo_stream;
00149 ardrone_autonomy::navdata_hdvideo_stream navdata_hdvideo_stream_msg;
00150 ros::Publisher pub_navdata_wifi;
00151 bool enabled_navdata_wifi;
00152 ardrone_autonomy::navdata_wifi navdata_wifi_msg;
00153 ros::Publisher pub_navdata_zimmu_3000;
00154 bool enabled_navdata_zimmu_3000;
00155 ardrone_autonomy::navdata_zimmu_3000 navdata_zimmu_3000_msg;
00156
00157 bool enabled_legacy_navdata;
00158
00159 bool initialized_navdata_publishers;
00160 #endif
00161
00162 #ifdef NAVDATA_STRUCTS_INITIALIZE
00163 if(!initialized_navdata_publishers)
00164 {
00165 initialized_navdata_publishers = true;
00166
00167 ros::param::param("~enable_legacy_navdata", enabled_legacy_navdata, true);
00168 if(enabled_legacy_navdata)
00169 {
00170 navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 200);
00171 imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 200);
00172 mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 200);
00173 }
00174 odo_pub = node_handle.advertise<nav_msgs::Odometry>("ardrone/odometry", 200);
00175
00176
00177
00178 ros::param::param("~enable_navdata_demo", enabled_navdata_demo, false);
00179 if(enabled_navdata_demo)
00180 {
00181 pub_navdata_demo = node_handle.advertise<ardrone_autonomy::navdata_demo>("ardrone/navdata_demo", NAVDATA_QUEUE_SIZE);
00182 }
00183
00184
00185
00186 ros::param::param("~enable_navdata_time", enabled_navdata_time, false);
00187 if(enabled_navdata_time)
00188 {
00189 pub_navdata_time = node_handle.advertise<ardrone_autonomy::navdata_time>("ardrone/navdata_time", NAVDATA_QUEUE_SIZE);
00190 }
00191
00192
00193
00194 ros::param::param("~enable_navdata_raw_measures", enabled_navdata_raw_measures, false);
00195 if(enabled_navdata_raw_measures)
00196 {
00197 pub_navdata_raw_measures = node_handle.advertise<ardrone_autonomy::navdata_raw_measures>("ardrone/navdata_raw_measures", NAVDATA_QUEUE_SIZE);
00198 }
00199
00200
00201
00202 ros::param::param("~enable_navdata_phys_measures", enabled_navdata_phys_measures, false);
00203 if(enabled_navdata_phys_measures)
00204 {
00205 pub_navdata_phys_measures = node_handle.advertise<ardrone_autonomy::navdata_phys_measures>("ardrone/navdata_phys_measures", NAVDATA_QUEUE_SIZE);
00206 }
00207
00208
00209
00210 ros::param::param("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets, false);
00211 if(enabled_navdata_gyros_offsets)
00212 {
00213 pub_navdata_gyros_offsets = node_handle.advertise<ardrone_autonomy::navdata_gyros_offsets>("ardrone/navdata_gyros_offsets", NAVDATA_QUEUE_SIZE);
00214 }
00215
00216
00217
00218 ros::param::param("~enable_navdata_euler_angles", enabled_navdata_euler_angles, false);
00219 if(enabled_navdata_euler_angles)
00220 {
00221 pub_navdata_euler_angles = node_handle.advertise<ardrone_autonomy::navdata_euler_angles>("ardrone/navdata_euler_angles", NAVDATA_QUEUE_SIZE);
00222 }
00223
00224
00225
00226 ros::param::param("~enable_navdata_references", enabled_navdata_references, false);
00227 if(enabled_navdata_references)
00228 {
00229 pub_navdata_references = node_handle.advertise<ardrone_autonomy::navdata_references>("ardrone/navdata_references", NAVDATA_QUEUE_SIZE);
00230 }
00231
00232
00233
00234 ros::param::param("~enable_navdata_trims", enabled_navdata_trims, false);
00235 if(enabled_navdata_trims)
00236 {
00237 pub_navdata_trims = node_handle.advertise<ardrone_autonomy::navdata_trims>("ardrone/navdata_trims", NAVDATA_QUEUE_SIZE);
00238 }
00239
00240
00241
00242 ros::param::param("~enable_navdata_rc_references", enabled_navdata_rc_references, false);
00243 if(enabled_navdata_rc_references)
00244 {
00245 pub_navdata_rc_references = node_handle.advertise<ardrone_autonomy::navdata_rc_references>("ardrone/navdata_rc_references", NAVDATA_QUEUE_SIZE);
00246 }
00247
00248
00249
00250 ros::param::param("~enable_navdata_pwm", enabled_navdata_pwm, false);
00251 if(enabled_navdata_pwm)
00252 {
00253 pub_navdata_pwm = node_handle.advertise<ardrone_autonomy::navdata_pwm>("ardrone/navdata_pwm", NAVDATA_QUEUE_SIZE);
00254 }
00255
00256
00257
00258 ros::param::param("~enable_navdata_altitude", enabled_navdata_altitude, false);
00259 if(enabled_navdata_altitude)
00260 {
00261 pub_navdata_altitude = node_handle.advertise<ardrone_autonomy::navdata_altitude>("ardrone/navdata_altitude", NAVDATA_QUEUE_SIZE);
00262 }
00263
00264
00265
00266 ros::param::param("~enable_navdata_vision_raw", enabled_navdata_vision_raw, false);
00267 if(enabled_navdata_vision_raw)
00268 {
00269 pub_navdata_vision_raw = node_handle.advertise<ardrone_autonomy::navdata_vision_raw>("ardrone/navdata_vision_raw", NAVDATA_QUEUE_SIZE);
00270 }
00271
00272
00273
00274 ros::param::param("~enable_navdata_vision_of", enabled_navdata_vision_of, false);
00275 if(enabled_navdata_vision_of)
00276 {
00277 pub_navdata_vision_of = node_handle.advertise<ardrone_autonomy::navdata_vision_of>("ardrone/navdata_vision_of", NAVDATA_QUEUE_SIZE);
00278 }
00279
00280
00281
00282 ros::param::param("~enable_navdata_vision", enabled_navdata_vision, false);
00283 if(enabled_navdata_vision)
00284 {
00285 pub_navdata_vision = node_handle.advertise<ardrone_autonomy::navdata_vision>("ardrone/navdata_vision", NAVDATA_QUEUE_SIZE);
00286 }
00287
00288
00289
00290 ros::param::param("~enable_navdata_vision_perf", enabled_navdata_vision_perf, false);
00291 if(enabled_navdata_vision_perf)
00292 {
00293 pub_navdata_vision_perf = node_handle.advertise<ardrone_autonomy::navdata_vision_perf>("ardrone/navdata_vision_perf", NAVDATA_QUEUE_SIZE);
00294 }
00295
00296
00297
00298 ros::param::param("~enable_navdata_trackers_send", enabled_navdata_trackers_send, false);
00299 if(enabled_navdata_trackers_send)
00300 {
00301 pub_navdata_trackers_send = node_handle.advertise<ardrone_autonomy::navdata_trackers_send>("ardrone/navdata_trackers_send", NAVDATA_QUEUE_SIZE);
00302 }
00303
00304
00305
00306 ros::param::param("~enable_navdata_vision_detect", enabled_navdata_vision_detect, false);
00307 if(enabled_navdata_vision_detect)
00308 {
00309 pub_navdata_vision_detect = node_handle.advertise<ardrone_autonomy::navdata_vision_detect>("ardrone/navdata_vision_detect", NAVDATA_QUEUE_SIZE);
00310 }
00311
00312
00313
00314 ros::param::param("~enable_navdata_watchdog", enabled_navdata_watchdog, false);
00315 if(enabled_navdata_watchdog)
00316 {
00317 pub_navdata_watchdog = node_handle.advertise<ardrone_autonomy::navdata_watchdog>("ardrone/navdata_watchdog", NAVDATA_QUEUE_SIZE);
00318 }
00319
00320
00321
00322 ros::param::param("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame, false);
00323 if(enabled_navdata_adc_data_frame)
00324 {
00325 pub_navdata_adc_data_frame = node_handle.advertise<ardrone_autonomy::navdata_adc_data_frame>("ardrone/navdata_adc_data_frame", NAVDATA_QUEUE_SIZE);
00326 }
00327
00328
00329
00330 ros::param::param("~enable_navdata_video_stream", enabled_navdata_video_stream, false);
00331 if(enabled_navdata_video_stream)
00332 {
00333 pub_navdata_video_stream = node_handle.advertise<ardrone_autonomy::navdata_video_stream>("ardrone/navdata_video_stream", NAVDATA_QUEUE_SIZE);
00334 }
00335
00336
00337
00338 ros::param::param("~enable_navdata_games", enabled_navdata_games, false);
00339 if(enabled_navdata_games)
00340 {
00341 pub_navdata_games = node_handle.advertise<ardrone_autonomy::navdata_games>("ardrone/navdata_games", NAVDATA_QUEUE_SIZE);
00342 }
00343
00344
00345
00346 ros::param::param("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw, false);
00347 if(enabled_navdata_pressure_raw)
00348 {
00349 pub_navdata_pressure_raw = node_handle.advertise<ardrone_autonomy::navdata_pressure_raw>("ardrone/navdata_pressure_raw", NAVDATA_QUEUE_SIZE);
00350 }
00351
00352
00353
00354 ros::param::param("~enable_navdata_magneto", enabled_navdata_magneto, false);
00355 if(enabled_navdata_magneto)
00356 {
00357 pub_navdata_magneto = node_handle.advertise<ardrone_autonomy::navdata_magneto>("ardrone/navdata_magneto", NAVDATA_QUEUE_SIZE);
00358 }
00359
00360
00361
00362 ros::param::param("~enable_navdata_wind_speed", enabled_navdata_wind_speed, false);
00363 if(enabled_navdata_wind_speed)
00364 {
00365 pub_navdata_wind_speed = node_handle.advertise<ardrone_autonomy::navdata_wind_speed>("ardrone/navdata_wind_speed", NAVDATA_QUEUE_SIZE);
00366 }
00367
00368
00369
00370 ros::param::param("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure, false);
00371 if(enabled_navdata_kalman_pressure)
00372 {
00373 pub_navdata_kalman_pressure = node_handle.advertise<ardrone_autonomy::navdata_kalman_pressure>("ardrone/navdata_kalman_pressure", NAVDATA_QUEUE_SIZE);
00374 }
00375
00376
00377
00378 ros::param::param("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream, false);
00379 if(enabled_navdata_hdvideo_stream)
00380 {
00381 pub_navdata_hdvideo_stream = node_handle.advertise<ardrone_autonomy::navdata_hdvideo_stream>("ardrone/navdata_hdvideo_stream", NAVDATA_QUEUE_SIZE);
00382 }
00383
00384
00385
00386 ros::param::param("~enable_navdata_wifi", enabled_navdata_wifi, false);
00387 if(enabled_navdata_wifi)
00388 {
00389 pub_navdata_wifi = node_handle.advertise<ardrone_autonomy::navdata_wifi>("ardrone/navdata_wifi", NAVDATA_QUEUE_SIZE);
00390 }
00391
00392
00393
00394 ros::param::param("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000, false);
00395 if(enabled_navdata_zimmu_3000)
00396 {
00397 pub_navdata_zimmu_3000 = node_handle.advertise<ardrone_autonomy::navdata_zimmu_3000>("ardrone/navdata_zimmu_3000", NAVDATA_QUEUE_SIZE);
00398 }
00399
00400
00401
00402 }
00403 #endif
00404
00405 #ifdef NAVDATA_STRUCTS_SOURCE
00406 void ARDroneDriver::PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received)
00407 {
00408 if(initialized_navdata_publishers)
00409 {
00410 if(enabled_navdata_demo && pub_navdata_demo.getNumSubscribers()>0)
00411 {
00412 navdata_demo_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00413 navdata_demo_msg.header.stamp = received;
00414 navdata_demo_msg.header.frame_id = drone_frame_base;
00415
00416 {
00417 uint16_t c = n.navdata_demo.tag;
00418 uint16_t m;
00419 m = c;
00420
00421 navdata_demo_msg.tag = m;
00422 }
00423
00424 {
00425 uint16_t c = n.navdata_demo.size;
00426 uint16_t m;
00427 m = c;
00428
00429 navdata_demo_msg.size = m;
00430 }
00431
00432 {
00433 uint32_t c = n.navdata_demo.ctrl_state;
00434 uint32_t m;
00435 m = c;
00436
00437 navdata_demo_msg.ctrl_state = m;
00438 }
00439
00440 {
00441 uint32_t c = n.navdata_demo.vbat_flying_percentage;
00442 uint32_t m;
00443 m = c;
00444
00445 navdata_demo_msg.vbat_flying_percentage = m;
00446 }
00447
00448 {
00449 float32_t c = n.navdata_demo.theta;
00450 float32_t m;
00451 m = c;
00452
00453 navdata_demo_msg.theta = m;
00454 }
00455
00456 {
00457 float32_t c = n.navdata_demo.phi;
00458 float32_t m;
00459 m = c;
00460
00461 navdata_demo_msg.phi = m;
00462 }
00463
00464 {
00465 float32_t c = n.navdata_demo.psi;
00466 float32_t m;
00467 m = c;
00468
00469 navdata_demo_msg.psi = m;
00470 }
00471
00472 {
00473 int32_t c = n.navdata_demo.altitude;
00474 int32_t m;
00475 m = c;
00476
00477 navdata_demo_msg.altitude = m;
00478 }
00479
00480 {
00481 float32_t c = n.navdata_demo.vx;
00482 float32_t m;
00483 m = c;
00484
00485 navdata_demo_msg.vx = m;
00486 }
00487
00488 {
00489 float32_t c = n.navdata_demo.vy;
00490 float32_t m;
00491 m = c;
00492
00493 navdata_demo_msg.vy = m;
00494 }
00495
00496 {
00497 float32_t c = n.navdata_demo.vz;
00498 float32_t m;
00499 m = c;
00500
00501 navdata_demo_msg.vz = m;
00502 }
00503
00504 {
00505 uint32_t c = n.navdata_demo.num_frames;
00506 uint32_t m;
00507 m = c;
00508
00509 navdata_demo_msg.num_frames = m;
00510 }
00511
00512 {
00513 uint32_t c = n.navdata_demo.detection_camera_type;
00514 uint32_t m;
00515 m = c;
00516
00517 navdata_demo_msg.detection_camera_type = m;
00518 }
00519
00520 pub_navdata_demo.publish(navdata_demo_msg);
00521 }
00522
00523
00524
00525 if(enabled_navdata_time && pub_navdata_time.getNumSubscribers()>0)
00526 {
00527 navdata_time_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00528 navdata_time_msg.header.stamp = received;
00529 navdata_time_msg.header.frame_id = drone_frame_base;
00530
00531 {
00532 uint16_t c = n.navdata_time.tag;
00533 uint16_t m;
00534 m = c;
00535
00536 navdata_time_msg.tag = m;
00537 }
00538
00539 {
00540 uint16_t c = n.navdata_time.size;
00541 uint16_t m;
00542 m = c;
00543
00544 navdata_time_msg.size = m;
00545 }
00546
00547 {
00548 uint32_t c = n.navdata_time.time;
00549 uint32_t m;
00550 m = c;
00551
00552 navdata_time_msg.time = m;
00553 }
00554
00555 pub_navdata_time.publish(navdata_time_msg);
00556 }
00557
00558
00559
00560 if(enabled_navdata_raw_measures && pub_navdata_raw_measures.getNumSubscribers()>0)
00561 {
00562 navdata_raw_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00563 navdata_raw_measures_msg.header.stamp = received;
00564 navdata_raw_measures_msg.header.frame_id = drone_frame_base;
00565
00566 {
00567 uint16_t c = n.navdata_raw_measures.tag;
00568 uint16_t m;
00569 m = c;
00570
00571 navdata_raw_measures_msg.tag = m;
00572 }
00573
00574 {
00575 uint16_t c = n.navdata_raw_measures.size;
00576 uint16_t m;
00577 m = c;
00578
00579 navdata_raw_measures_msg.size = m;
00580 }
00581
00582 navdata_raw_measures_msg.raw_gyros.clear();
00583 for(int i=0; i<NB_GYROS; i++)
00584 {
00585 int16_t c = n.navdata_raw_measures.raw_gyros[i];
00586 int16_t m;
00587 m = c;
00588
00589 navdata_raw_measures_msg.raw_gyros.push_back(m);
00590 }
00591
00592 navdata_raw_measures_msg.raw_gyros_110.clear();
00593 for(int i=0; i<2; i++)
00594 {
00595 int16_t c = n.navdata_raw_measures.raw_gyros_110[i];
00596 int16_t m;
00597 m = c;
00598
00599 navdata_raw_measures_msg.raw_gyros_110.push_back(m);
00600 }
00601
00602 {
00603 uint32_t c = n.navdata_raw_measures.vbat_raw;
00604 uint32_t m;
00605 m = c;
00606
00607 navdata_raw_measures_msg.vbat_raw = m;
00608 }
00609
00610 {
00611 uint16_t c = n.navdata_raw_measures.us_debut_echo;
00612 uint16_t m;
00613 m = c;
00614
00615 navdata_raw_measures_msg.us_debut_echo = m;
00616 }
00617
00618 {
00619 uint16_t c = n.navdata_raw_measures.us_fin_echo;
00620 uint16_t m;
00621 m = c;
00622
00623 navdata_raw_measures_msg.us_fin_echo = m;
00624 }
00625
00626 {
00627 uint16_t c = n.navdata_raw_measures.us_association_echo;
00628 uint16_t m;
00629 m = c;
00630
00631 navdata_raw_measures_msg.us_association_echo = m;
00632 }
00633
00634 {
00635 uint16_t c = n.navdata_raw_measures.us_distance_echo;
00636 uint16_t m;
00637 m = c;
00638
00639 navdata_raw_measures_msg.us_distance_echo = m;
00640 }
00641
00642 {
00643 uint16_t c = n.navdata_raw_measures.us_courbe_temps;
00644 uint16_t m;
00645 m = c;
00646
00647 navdata_raw_measures_msg.us_courbe_temps = m;
00648 }
00649
00650 {
00651 uint16_t c = n.navdata_raw_measures.us_courbe_valeur;
00652 uint16_t m;
00653 m = c;
00654
00655 navdata_raw_measures_msg.us_courbe_valeur = m;
00656 }
00657
00658 {
00659 uint16_t c = n.navdata_raw_measures.us_courbe_ref;
00660 uint16_t m;
00661 m = c;
00662
00663 navdata_raw_measures_msg.us_courbe_ref = m;
00664 }
00665
00666 {
00667 uint16_t c = n.navdata_raw_measures.flag_echo_ini;
00668 uint16_t m;
00669 m = c;
00670
00671 navdata_raw_measures_msg.flag_echo_ini = m;
00672 }
00673
00674 {
00675 uint16_t c = n.navdata_raw_measures.nb_echo;
00676 uint16_t m;
00677 m = c;
00678
00679 navdata_raw_measures_msg.nb_echo = m;
00680 }
00681
00682 {
00683 uint32_t c = n.navdata_raw_measures.sum_echo;
00684 uint32_t m;
00685 m = c;
00686
00687 navdata_raw_measures_msg.sum_echo = m;
00688 }
00689
00690 {
00691 int32_t c = n.navdata_raw_measures.alt_temp_raw;
00692 int32_t m;
00693 m = c;
00694
00695 navdata_raw_measures_msg.alt_temp_raw = m;
00696 }
00697
00698 {
00699 int16_t c = n.navdata_raw_measures.gradient;
00700 int16_t m;
00701 m = c;
00702
00703 navdata_raw_measures_msg.gradient = m;
00704 }
00705
00706 pub_navdata_raw_measures.publish(navdata_raw_measures_msg);
00707 }
00708
00709
00710
00711 if(enabled_navdata_phys_measures && pub_navdata_phys_measures.getNumSubscribers()>0)
00712 {
00713 navdata_phys_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00714 navdata_phys_measures_msg.header.stamp = received;
00715 navdata_phys_measures_msg.header.frame_id = drone_frame_base;
00716
00717 {
00718 uint16_t c = n.navdata_phys_measures.tag;
00719 uint16_t m;
00720 m = c;
00721
00722 navdata_phys_measures_msg.tag = m;
00723 }
00724
00725 {
00726 uint16_t c = n.navdata_phys_measures.size;
00727 uint16_t m;
00728 m = c;
00729
00730 navdata_phys_measures_msg.size = m;
00731 }
00732
00733 {
00734 float32_t c = n.navdata_phys_measures.accs_temp;
00735 float32_t m;
00736 m = c;
00737
00738 navdata_phys_measures_msg.accs_temp = m;
00739 }
00740
00741 {
00742 uint16_t c = n.navdata_phys_measures.gyro_temp;
00743 uint16_t m;
00744 m = c;
00745
00746 navdata_phys_measures_msg.gyro_temp = m;
00747 }
00748
00749 navdata_phys_measures_msg.phys_accs.clear();
00750 for(int i=0; i<NB_ACCS; i++)
00751 {
00752 float32_t c = n.navdata_phys_measures.phys_accs[i];
00753 float32_t m;
00754 m = c;
00755
00756 navdata_phys_measures_msg.phys_accs.push_back(m);
00757 }
00758
00759 navdata_phys_measures_msg.phys_gyros.clear();
00760 for(int i=0; i<NB_GYROS; i++)
00761 {
00762 float32_t c = n.navdata_phys_measures.phys_gyros[i];
00763 float32_t m;
00764 m = c;
00765
00766 navdata_phys_measures_msg.phys_gyros.push_back(m);
00767 }
00768
00769 {
00770 uint32_t c = n.navdata_phys_measures.alim3V3;
00771 uint32_t m;
00772 m = c;
00773
00774 navdata_phys_measures_msg.alim3V3 = m;
00775 }
00776
00777 {
00778 uint32_t c = n.navdata_phys_measures.vrefEpson;
00779 uint32_t m;
00780 m = c;
00781
00782 navdata_phys_measures_msg.vrefEpson = m;
00783 }
00784
00785 {
00786 uint32_t c = n.navdata_phys_measures.vrefIDG;
00787 uint32_t m;
00788 m = c;
00789
00790 navdata_phys_measures_msg.vrefIDG = m;
00791 }
00792
00793 pub_navdata_phys_measures.publish(navdata_phys_measures_msg);
00794 }
00795
00796
00797
00798 if(enabled_navdata_gyros_offsets && pub_navdata_gyros_offsets.getNumSubscribers()>0)
00799 {
00800 navdata_gyros_offsets_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00801 navdata_gyros_offsets_msg.header.stamp = received;
00802 navdata_gyros_offsets_msg.header.frame_id = drone_frame_base;
00803
00804 {
00805 uint16_t c = n.navdata_gyros_offsets.tag;
00806 uint16_t m;
00807 m = c;
00808
00809 navdata_gyros_offsets_msg.tag = m;
00810 }
00811
00812 {
00813 uint16_t c = n.navdata_gyros_offsets.size;
00814 uint16_t m;
00815 m = c;
00816
00817 navdata_gyros_offsets_msg.size = m;
00818 }
00819
00820 navdata_gyros_offsets_msg.offset_g.clear();
00821 for(int i=0; i<NB_GYROS; i++)
00822 {
00823 float32_t c = n.navdata_gyros_offsets.offset_g[i];
00824 float32_t m;
00825 m = c;
00826
00827 navdata_gyros_offsets_msg.offset_g.push_back(m);
00828 }
00829
00830 pub_navdata_gyros_offsets.publish(navdata_gyros_offsets_msg);
00831 }
00832
00833
00834
00835 if(enabled_navdata_euler_angles && pub_navdata_euler_angles.getNumSubscribers()>0)
00836 {
00837 navdata_euler_angles_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00838 navdata_euler_angles_msg.header.stamp = received;
00839 navdata_euler_angles_msg.header.frame_id = drone_frame_base;
00840
00841 {
00842 uint16_t c = n.navdata_euler_angles.tag;
00843 uint16_t m;
00844 m = c;
00845
00846 navdata_euler_angles_msg.tag = m;
00847 }
00848
00849 {
00850 uint16_t c = n.navdata_euler_angles.size;
00851 uint16_t m;
00852 m = c;
00853
00854 navdata_euler_angles_msg.size = m;
00855 }
00856
00857 {
00858 float32_t c = n.navdata_euler_angles.theta_a;
00859 float32_t m;
00860 m = c;
00861
00862 navdata_euler_angles_msg.theta_a = m;
00863 }
00864
00865 {
00866 float32_t c = n.navdata_euler_angles.phi_a;
00867 float32_t m;
00868 m = c;
00869
00870 navdata_euler_angles_msg.phi_a = m;
00871 }
00872
00873 pub_navdata_euler_angles.publish(navdata_euler_angles_msg);
00874 }
00875
00876
00877
00878 if(enabled_navdata_references && pub_navdata_references.getNumSubscribers()>0)
00879 {
00880 navdata_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00881 navdata_references_msg.header.stamp = received;
00882 navdata_references_msg.header.frame_id = drone_frame_base;
00883
00884 {
00885 uint16_t c = n.navdata_references.tag;
00886 uint16_t m;
00887 m = c;
00888
00889 navdata_references_msg.tag = m;
00890 }
00891
00892 {
00893 uint16_t c = n.navdata_references.size;
00894 uint16_t m;
00895 m = c;
00896
00897 navdata_references_msg.size = m;
00898 }
00899
00900 {
00901 int32_t c = n.navdata_references.ref_theta;
00902 int32_t m;
00903 m = c;
00904
00905 navdata_references_msg.ref_theta = m;
00906 }
00907
00908 {
00909 int32_t c = n.navdata_references.ref_phi;
00910 int32_t m;
00911 m = c;
00912
00913 navdata_references_msg.ref_phi = m;
00914 }
00915
00916 {
00917 int32_t c = n.navdata_references.ref_theta_I;
00918 int32_t m;
00919 m = c;
00920
00921 navdata_references_msg.ref_theta_I = m;
00922 }
00923
00924 {
00925 int32_t c = n.navdata_references.ref_phi_I;
00926 int32_t m;
00927 m = c;
00928
00929 navdata_references_msg.ref_phi_I = m;
00930 }
00931
00932 {
00933 int32_t c = n.navdata_references.ref_pitch;
00934 int32_t m;
00935 m = c;
00936
00937 navdata_references_msg.ref_pitch = m;
00938 }
00939
00940 {
00941 int32_t c = n.navdata_references.ref_roll;
00942 int32_t m;
00943 m = c;
00944
00945 navdata_references_msg.ref_roll = m;
00946 }
00947
00948 {
00949 int32_t c = n.navdata_references.ref_yaw;
00950 int32_t m;
00951 m = c;
00952
00953 navdata_references_msg.ref_yaw = m;
00954 }
00955
00956 {
00957 int32_t c = n.navdata_references.ref_psi;
00958 int32_t m;
00959 m = c;
00960
00961 navdata_references_msg.ref_psi = m;
00962 }
00963
00964 {
00965 float32_t c = n.navdata_references.vx_ref;
00966 float32_t m;
00967 m = c;
00968
00969 navdata_references_msg.vx_ref = m;
00970 }
00971
00972 {
00973 float32_t c = n.navdata_references.vy_ref;
00974 float32_t m;
00975 m = c;
00976
00977 navdata_references_msg.vy_ref = m;
00978 }
00979
00980 {
00981 float32_t c = n.navdata_references.theta_mod;
00982 float32_t m;
00983 m = c;
00984
00985 navdata_references_msg.theta_mod = m;
00986 }
00987
00988 {
00989 float32_t c = n.navdata_references.phi_mod;
00990 float32_t m;
00991 m = c;
00992
00993 navdata_references_msg.phi_mod = m;
00994 }
00995
00996 {
00997 float32_t c = n.navdata_references.k_v_x;
00998 float32_t m;
00999 m = c;
01000
01001 navdata_references_msg.k_v_x = m;
01002 }
01003
01004 {
01005 float32_t c = n.navdata_references.k_v_y;
01006 float32_t m;
01007 m = c;
01008
01009 navdata_references_msg.k_v_y = m;
01010 }
01011
01012 {
01013 uint32_t c = n.navdata_references.k_mode;
01014 uint32_t m;
01015 m = c;
01016
01017 navdata_references_msg.k_mode = m;
01018 }
01019
01020 {
01021 float32_t c = n.navdata_references.ui_time;
01022 float32_t m;
01023 m = c;
01024
01025 navdata_references_msg.ui_time = m;
01026 }
01027
01028 {
01029 float32_t c = n.navdata_references.ui_theta;
01030 float32_t m;
01031 m = c;
01032
01033 navdata_references_msg.ui_theta = m;
01034 }
01035
01036 {
01037 float32_t c = n.navdata_references.ui_phi;
01038 float32_t m;
01039 m = c;
01040
01041 navdata_references_msg.ui_phi = m;
01042 }
01043
01044 {
01045 float32_t c = n.navdata_references.ui_psi;
01046 float32_t m;
01047 m = c;
01048
01049 navdata_references_msg.ui_psi = m;
01050 }
01051
01052 {
01053 float32_t c = n.navdata_references.ui_psi_accuracy;
01054 float32_t m;
01055 m = c;
01056
01057 navdata_references_msg.ui_psi_accuracy = m;
01058 }
01059
01060 {
01061 int32_t c = n.navdata_references.ui_seq;
01062 int32_t m;
01063 m = c;
01064
01065 navdata_references_msg.ui_seq = m;
01066 }
01067
01068 pub_navdata_references.publish(navdata_references_msg);
01069 }
01070
01071
01072
01073 if(enabled_navdata_trims && pub_navdata_trims.getNumSubscribers()>0)
01074 {
01075 navdata_trims_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01076 navdata_trims_msg.header.stamp = received;
01077 navdata_trims_msg.header.frame_id = drone_frame_base;
01078
01079 {
01080 uint16_t c = n.navdata_trims.tag;
01081 uint16_t m;
01082 m = c;
01083
01084 navdata_trims_msg.tag = m;
01085 }
01086
01087 {
01088 uint16_t c = n.navdata_trims.size;
01089 uint16_t m;
01090 m = c;
01091
01092 navdata_trims_msg.size = m;
01093 }
01094
01095 {
01096 float32_t c = n.navdata_trims.angular_rates_trim_r;
01097 float32_t m;
01098 m = c;
01099
01100 navdata_trims_msg.angular_rates_trim_r = m;
01101 }
01102
01103 {
01104 float32_t c = n.navdata_trims.euler_angles_trim_theta;
01105 float32_t m;
01106 m = c;
01107
01108 navdata_trims_msg.euler_angles_trim_theta = m;
01109 }
01110
01111 {
01112 float32_t c = n.navdata_trims.euler_angles_trim_phi;
01113 float32_t m;
01114 m = c;
01115
01116 navdata_trims_msg.euler_angles_trim_phi = m;
01117 }
01118
01119 pub_navdata_trims.publish(navdata_trims_msg);
01120 }
01121
01122
01123
01124 if(enabled_navdata_rc_references && pub_navdata_rc_references.getNumSubscribers()>0)
01125 {
01126 navdata_rc_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01127 navdata_rc_references_msg.header.stamp = received;
01128 navdata_rc_references_msg.header.frame_id = drone_frame_base;
01129
01130 {
01131 uint16_t c = n.navdata_rc_references.tag;
01132 uint16_t m;
01133 m = c;
01134
01135 navdata_rc_references_msg.tag = m;
01136 }
01137
01138 {
01139 uint16_t c = n.navdata_rc_references.size;
01140 uint16_t m;
01141 m = c;
01142
01143 navdata_rc_references_msg.size = m;
01144 }
01145
01146 {
01147 int32_t c = n.navdata_rc_references.rc_ref_pitch;
01148 int32_t m;
01149 m = c;
01150
01151 navdata_rc_references_msg.rc_ref_pitch = m;
01152 }
01153
01154 {
01155 int32_t c = n.navdata_rc_references.rc_ref_roll;
01156 int32_t m;
01157 m = c;
01158
01159 navdata_rc_references_msg.rc_ref_roll = m;
01160 }
01161
01162 {
01163 int32_t c = n.navdata_rc_references.rc_ref_yaw;
01164 int32_t m;
01165 m = c;
01166
01167 navdata_rc_references_msg.rc_ref_yaw = m;
01168 }
01169
01170 {
01171 int32_t c = n.navdata_rc_references.rc_ref_gaz;
01172 int32_t m;
01173 m = c;
01174
01175 navdata_rc_references_msg.rc_ref_gaz = m;
01176 }
01177
01178 {
01179 int32_t c = n.navdata_rc_references.rc_ref_ag;
01180 int32_t m;
01181 m = c;
01182
01183 navdata_rc_references_msg.rc_ref_ag = m;
01184 }
01185
01186 pub_navdata_rc_references.publish(navdata_rc_references_msg);
01187 }
01188
01189
01190
01191 if(enabled_navdata_pwm && pub_navdata_pwm.getNumSubscribers()>0)
01192 {
01193 navdata_pwm_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01194 navdata_pwm_msg.header.stamp = received;
01195 navdata_pwm_msg.header.frame_id = drone_frame_base;
01196
01197 {
01198 uint16_t c = n.navdata_pwm.tag;
01199 uint16_t m;
01200 m = c;
01201
01202 navdata_pwm_msg.tag = m;
01203 }
01204
01205 {
01206 uint16_t c = n.navdata_pwm.size;
01207 uint16_t m;
01208 m = c;
01209
01210 navdata_pwm_msg.size = m;
01211 }
01212
01213 {
01214 uint8_t c = n.navdata_pwm.motor1;
01215 uint8_t m;
01216 m = c;
01217
01218 navdata_pwm_msg.motor1 = m;
01219 }
01220
01221 {
01222 uint8_t c = n.navdata_pwm.motor2;
01223 uint8_t m;
01224 m = c;
01225
01226 navdata_pwm_msg.motor2 = m;
01227 }
01228
01229 {
01230 uint8_t c = n.navdata_pwm.motor3;
01231 uint8_t m;
01232 m = c;
01233
01234 navdata_pwm_msg.motor3 = m;
01235 }
01236
01237 {
01238 uint8_t c = n.navdata_pwm.motor4;
01239 uint8_t m;
01240 m = c;
01241
01242 navdata_pwm_msg.motor4 = m;
01243 }
01244
01245 {
01246 uint8_t c = n.navdata_pwm.sat_motor1;
01247 uint8_t m;
01248 m = c;
01249
01250 navdata_pwm_msg.sat_motor1 = m;
01251 }
01252
01253 {
01254 uint8_t c = n.navdata_pwm.sat_motor2;
01255 uint8_t m;
01256 m = c;
01257
01258 navdata_pwm_msg.sat_motor2 = m;
01259 }
01260
01261 {
01262 uint8_t c = n.navdata_pwm.sat_motor3;
01263 uint8_t m;
01264 m = c;
01265
01266 navdata_pwm_msg.sat_motor3 = m;
01267 }
01268
01269 {
01270 uint8_t c = n.navdata_pwm.sat_motor4;
01271 uint8_t m;
01272 m = c;
01273
01274 navdata_pwm_msg.sat_motor4 = m;
01275 }
01276
01277 {
01278 float32_t c = n.navdata_pwm.gaz_feed_forward;
01279 float32_t m;
01280 m = c;
01281
01282 navdata_pwm_msg.gaz_feed_forward = m;
01283 }
01284
01285 {
01286 float32_t c = n.navdata_pwm.gaz_altitude;
01287 float32_t m;
01288 m = c;
01289
01290 navdata_pwm_msg.gaz_altitude = m;
01291 }
01292
01293 {
01294 float32_t c = n.navdata_pwm.altitude_integral;
01295 float32_t m;
01296 m = c;
01297
01298 navdata_pwm_msg.altitude_integral = m;
01299 }
01300
01301 {
01302 float32_t c = n.navdata_pwm.vz_ref;
01303 float32_t m;
01304 m = c;
01305
01306 navdata_pwm_msg.vz_ref = m;
01307 }
01308
01309 {
01310 int32_t c = n.navdata_pwm.u_pitch;
01311 int32_t m;
01312 m = c;
01313
01314 navdata_pwm_msg.u_pitch = m;
01315 }
01316
01317 {
01318 int32_t c = n.navdata_pwm.u_roll;
01319 int32_t m;
01320 m = c;
01321
01322 navdata_pwm_msg.u_roll = m;
01323 }
01324
01325 {
01326 int32_t c = n.navdata_pwm.u_yaw;
01327 int32_t m;
01328 m = c;
01329
01330 navdata_pwm_msg.u_yaw = m;
01331 }
01332
01333 {
01334 float32_t c = n.navdata_pwm.yaw_u_I;
01335 float32_t m;
01336 m = c;
01337
01338 navdata_pwm_msg.yaw_u_I = m;
01339 }
01340
01341 {
01342 int32_t c = n.navdata_pwm.u_pitch_planif;
01343 int32_t m;
01344 m = c;
01345
01346 navdata_pwm_msg.u_pitch_planif = m;
01347 }
01348
01349 {
01350 int32_t c = n.navdata_pwm.u_roll_planif;
01351 int32_t m;
01352 m = c;
01353
01354 navdata_pwm_msg.u_roll_planif = m;
01355 }
01356
01357 {
01358 int32_t c = n.navdata_pwm.u_yaw_planif;
01359 int32_t m;
01360 m = c;
01361
01362 navdata_pwm_msg.u_yaw_planif = m;
01363 }
01364
01365 {
01366 float32_t c = n.navdata_pwm.u_gaz_planif;
01367 float32_t m;
01368 m = c;
01369
01370 navdata_pwm_msg.u_gaz_planif = m;
01371 }
01372
01373 {
01374 uint16_t c = n.navdata_pwm.current_motor1;
01375 uint16_t m;
01376 m = c;
01377
01378 navdata_pwm_msg.current_motor1 = m;
01379 }
01380
01381 {
01382 uint16_t c = n.navdata_pwm.current_motor2;
01383 uint16_t m;
01384 m = c;
01385
01386 navdata_pwm_msg.current_motor2 = m;
01387 }
01388
01389 {
01390 uint16_t c = n.navdata_pwm.current_motor3;
01391 uint16_t m;
01392 m = c;
01393
01394 navdata_pwm_msg.current_motor3 = m;
01395 }
01396
01397 {
01398 uint16_t c = n.navdata_pwm.current_motor4;
01399 uint16_t m;
01400 m = c;
01401
01402 navdata_pwm_msg.current_motor4 = m;
01403 }
01404
01405 {
01406 float32_t c = n.navdata_pwm.altitude_der;
01407 float32_t m;
01408 m = c;
01409
01410 navdata_pwm_msg.altitude_der = m;
01411 }
01412
01413 pub_navdata_pwm.publish(navdata_pwm_msg);
01414 }
01415
01416
01417
01418 if(enabled_navdata_altitude && pub_navdata_altitude.getNumSubscribers()>0)
01419 {
01420 navdata_altitude_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01421 navdata_altitude_msg.header.stamp = received;
01422 navdata_altitude_msg.header.frame_id = drone_frame_base;
01423
01424 {
01425 uint16_t c = n.navdata_altitude.tag;
01426 uint16_t m;
01427 m = c;
01428
01429 navdata_altitude_msg.tag = m;
01430 }
01431
01432 {
01433 uint16_t c = n.navdata_altitude.size;
01434 uint16_t m;
01435 m = c;
01436
01437 navdata_altitude_msg.size = m;
01438 }
01439
01440 {
01441 int32_t c = n.navdata_altitude.altitude_vision;
01442 int32_t m;
01443 m = c;
01444
01445 navdata_altitude_msg.altitude_vision = m;
01446 }
01447
01448 {
01449 float32_t c = n.navdata_altitude.altitude_vz;
01450 float32_t m;
01451 m = c;
01452
01453 navdata_altitude_msg.altitude_vz = m;
01454 }
01455
01456 {
01457 int32_t c = n.navdata_altitude.altitude_ref;
01458 int32_t m;
01459 m = c;
01460
01461 navdata_altitude_msg.altitude_ref = m;
01462 }
01463
01464 {
01465 int32_t c = n.navdata_altitude.altitude_raw;
01466 int32_t m;
01467 m = c;
01468
01469 navdata_altitude_msg.altitude_raw = m;
01470 }
01471
01472 {
01473 float32_t c = n.navdata_altitude.obs_accZ;
01474 float32_t m;
01475 m = c;
01476
01477 navdata_altitude_msg.obs_accZ = m;
01478 }
01479
01480 {
01481 float32_t c = n.navdata_altitude.obs_alt;
01482 float32_t m;
01483 m = c;
01484
01485 navdata_altitude_msg.obs_alt = m;
01486 }
01487
01488 {
01489 vector31_t c = n.navdata_altitude.obs_x;
01490 ardrone_autonomy::vector31 m;
01491 m.x = c.x;
01492 m.y = c.y;
01493 m.z = c.z;
01494
01495 navdata_altitude_msg.obs_x = m;
01496 }
01497
01498 {
01499 uint32_t c = n.navdata_altitude.obs_state;
01500 uint32_t m;
01501 m = c;
01502
01503 navdata_altitude_msg.obs_state = m;
01504 }
01505
01506 {
01507 vector21_t c = n.navdata_altitude.est_vb;
01508 ardrone_autonomy::vector21 m;
01509 m.x = c.x;
01510 m.y = c.y;
01511
01512 navdata_altitude_msg.est_vb = m;
01513 }
01514
01515 {
01516 uint32_t c = n.navdata_altitude.est_state;
01517 uint32_t m;
01518 m = c;
01519
01520 navdata_altitude_msg.est_state = m;
01521 }
01522
01523 pub_navdata_altitude.publish(navdata_altitude_msg);
01524 }
01525
01526
01527
01528 if(enabled_navdata_vision_raw && pub_navdata_vision_raw.getNumSubscribers()>0)
01529 {
01530 navdata_vision_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01531 navdata_vision_raw_msg.header.stamp = received;
01532 navdata_vision_raw_msg.header.frame_id = drone_frame_base;
01533
01534 {
01535 uint16_t c = n.navdata_vision_raw.tag;
01536 uint16_t m;
01537 m = c;
01538
01539 navdata_vision_raw_msg.tag = m;
01540 }
01541
01542 {
01543 uint16_t c = n.navdata_vision_raw.size;
01544 uint16_t m;
01545 m = c;
01546
01547 navdata_vision_raw_msg.size = m;
01548 }
01549
01550 {
01551 float32_t c = n.navdata_vision_raw.vision_tx_raw;
01552 float32_t m;
01553 m = c;
01554
01555 navdata_vision_raw_msg.vision_tx_raw = m;
01556 }
01557
01558 {
01559 float32_t c = n.navdata_vision_raw.vision_ty_raw;
01560 float32_t m;
01561 m = c;
01562
01563 navdata_vision_raw_msg.vision_ty_raw = m;
01564 }
01565
01566 {
01567 float32_t c = n.navdata_vision_raw.vision_tz_raw;
01568 float32_t m;
01569 m = c;
01570
01571 navdata_vision_raw_msg.vision_tz_raw = m;
01572 }
01573
01574 pub_navdata_vision_raw.publish(navdata_vision_raw_msg);
01575 }
01576
01577
01578
01579 if(enabled_navdata_vision_of && pub_navdata_vision_of.getNumSubscribers()>0)
01580 {
01581 navdata_vision_of_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01582 navdata_vision_of_msg.header.stamp = received;
01583 navdata_vision_of_msg.header.frame_id = drone_frame_base;
01584
01585 {
01586 uint16_t c = n.navdata_vision_of.tag;
01587 uint16_t m;
01588 m = c;
01589
01590 navdata_vision_of_msg.tag = m;
01591 }
01592
01593 {
01594 uint16_t c = n.navdata_vision_of.size;
01595 uint16_t m;
01596 m = c;
01597
01598 navdata_vision_of_msg.size = m;
01599 }
01600
01601 navdata_vision_of_msg.of_dx.clear();
01602 for(int i=0; i<5; i++)
01603 {
01604 float32_t c = n.navdata_vision_of.of_dx[i];
01605 float32_t m;
01606 m = c;
01607
01608 navdata_vision_of_msg.of_dx.push_back(m);
01609 }
01610
01611 navdata_vision_of_msg.of_dy.clear();
01612 for(int i=0; i<5; i++)
01613 {
01614 float32_t c = n.navdata_vision_of.of_dy[i];
01615 float32_t m;
01616 m = c;
01617
01618 navdata_vision_of_msg.of_dy.push_back(m);
01619 }
01620
01621 pub_navdata_vision_of.publish(navdata_vision_of_msg);
01622 }
01623
01624
01625
01626 if(enabled_navdata_vision && pub_navdata_vision.getNumSubscribers()>0)
01627 {
01628 navdata_vision_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01629 navdata_vision_msg.header.stamp = received;
01630 navdata_vision_msg.header.frame_id = drone_frame_base;
01631
01632 {
01633 uint16_t c = n.navdata_vision.tag;
01634 uint16_t m;
01635 m = c;
01636
01637 navdata_vision_msg.tag = m;
01638 }
01639
01640 {
01641 uint16_t c = n.navdata_vision.size;
01642 uint16_t m;
01643 m = c;
01644
01645 navdata_vision_msg.size = m;
01646 }
01647
01648 {
01649 uint32_t c = n.navdata_vision.vision_state;
01650 uint32_t m;
01651 m = c;
01652
01653 navdata_vision_msg.vision_state = m;
01654 }
01655
01656 {
01657 int32_t c = n.navdata_vision.vision_misc;
01658 int32_t m;
01659 m = c;
01660
01661 navdata_vision_msg.vision_misc = m;
01662 }
01663
01664 {
01665 float32_t c = n.navdata_vision.vision_phi_trim;
01666 float32_t m;
01667 m = c;
01668
01669 navdata_vision_msg.vision_phi_trim = m;
01670 }
01671
01672 {
01673 float32_t c = n.navdata_vision.vision_phi_ref_prop;
01674 float32_t m;
01675 m = c;
01676
01677 navdata_vision_msg.vision_phi_ref_prop = m;
01678 }
01679
01680 {
01681 float32_t c = n.navdata_vision.vision_theta_trim;
01682 float32_t m;
01683 m = c;
01684
01685 navdata_vision_msg.vision_theta_trim = m;
01686 }
01687
01688 {
01689 float32_t c = n.navdata_vision.vision_theta_ref_prop;
01690 float32_t m;
01691 m = c;
01692
01693 navdata_vision_msg.vision_theta_ref_prop = m;
01694 }
01695
01696 {
01697 int32_t c = n.navdata_vision.new_raw_picture;
01698 int32_t m;
01699 m = c;
01700
01701 navdata_vision_msg.new_raw_picture = m;
01702 }
01703
01704 {
01705 float32_t c = n.navdata_vision.theta_capture;
01706 float32_t m;
01707 m = c;
01708
01709 navdata_vision_msg.theta_capture = m;
01710 }
01711
01712 {
01713 float32_t c = n.navdata_vision.phi_capture;
01714 float32_t m;
01715 m = c;
01716
01717 navdata_vision_msg.phi_capture = m;
01718 }
01719
01720 {
01721 float32_t c = n.navdata_vision.psi_capture;
01722 float32_t m;
01723 m = c;
01724
01725 navdata_vision_msg.psi_capture = m;
01726 }
01727
01728 {
01729 int32_t c = n.navdata_vision.altitude_capture;
01730 int32_t m;
01731 m = c;
01732
01733 navdata_vision_msg.altitude_capture = m;
01734 }
01735
01736 {
01737 uint32_t c = n.navdata_vision.time_capture;
01738 uint32_t m;
01739 m = c;
01740
01741 navdata_vision_msg.time_capture = m;
01742 }
01743
01744 {
01745 velocities_t c = n.navdata_vision.body_v;
01746 ardrone_autonomy::vector31 m;
01747 m.x = c.x;
01748 m.y = c.y;
01749 m.z = c.z;
01750
01751 navdata_vision_msg.body_v = m;
01752 }
01753
01754 {
01755 float32_t c = n.navdata_vision.delta_phi;
01756 float32_t m;
01757 m = c;
01758
01759 navdata_vision_msg.delta_phi = m;
01760 }
01761
01762 {
01763 float32_t c = n.navdata_vision.delta_theta;
01764 float32_t m;
01765 m = c;
01766
01767 navdata_vision_msg.delta_theta = m;
01768 }
01769
01770 {
01771 float32_t c = n.navdata_vision.delta_psi;
01772 float32_t m;
01773 m = c;
01774
01775 navdata_vision_msg.delta_psi = m;
01776 }
01777
01778 {
01779 uint32_t c = n.navdata_vision.gold_defined;
01780 uint32_t m;
01781 m = c;
01782
01783 navdata_vision_msg.gold_defined = m;
01784 }
01785
01786 {
01787 uint32_t c = n.navdata_vision.gold_reset;
01788 uint32_t m;
01789 m = c;
01790
01791 navdata_vision_msg.gold_reset = m;
01792 }
01793
01794 {
01795 float32_t c = n.navdata_vision.gold_x;
01796 float32_t m;
01797 m = c;
01798
01799 navdata_vision_msg.gold_x = m;
01800 }
01801
01802 {
01803 float32_t c = n.navdata_vision.gold_y;
01804 float32_t m;
01805 m = c;
01806
01807 navdata_vision_msg.gold_y = m;
01808 }
01809
01810 pub_navdata_vision.publish(navdata_vision_msg);
01811 }
01812
01813
01814
01815 if(enabled_navdata_vision_perf && pub_navdata_vision_perf.getNumSubscribers()>0)
01816 {
01817 navdata_vision_perf_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01818 navdata_vision_perf_msg.header.stamp = received;
01819 navdata_vision_perf_msg.header.frame_id = drone_frame_base;
01820
01821 {
01822 uint16_t c = n.navdata_vision_perf.tag;
01823 uint16_t m;
01824 m = c;
01825
01826 navdata_vision_perf_msg.tag = m;
01827 }
01828
01829 {
01830 uint16_t c = n.navdata_vision_perf.size;
01831 uint16_t m;
01832 m = c;
01833
01834 navdata_vision_perf_msg.size = m;
01835 }
01836
01837 {
01838 float32_t c = n.navdata_vision_perf.time_corners;
01839 float32_t m;
01840 m = c;
01841
01842 navdata_vision_perf_msg.time_corners = m;
01843 }
01844
01845 {
01846 float32_t c = n.navdata_vision_perf.time_compute;
01847 float32_t m;
01848 m = c;
01849
01850 navdata_vision_perf_msg.time_compute = m;
01851 }
01852
01853 {
01854 float32_t c = n.navdata_vision_perf.time_tracking;
01855 float32_t m;
01856 m = c;
01857
01858 navdata_vision_perf_msg.time_tracking = m;
01859 }
01860
01861 {
01862 float32_t c = n.navdata_vision_perf.time_trans;
01863 float32_t m;
01864 m = c;
01865
01866 navdata_vision_perf_msg.time_trans = m;
01867 }
01868
01869 {
01870 float32_t c = n.navdata_vision_perf.time_update;
01871 float32_t m;
01872 m = c;
01873
01874 navdata_vision_perf_msg.time_update = m;
01875 }
01876
01877 navdata_vision_perf_msg.time_custom.clear();
01878 for(int i=0; i<NAVDATA_MAX_CUSTOM_TIME_SAVE; i++)
01879 {
01880 float32_t c = n.navdata_vision_perf.time_custom[i];
01881 float32_t m;
01882 m = c;
01883
01884 navdata_vision_perf_msg.time_custom.push_back(m);
01885 }
01886
01887 pub_navdata_vision_perf.publish(navdata_vision_perf_msg);
01888 }
01889
01890
01891
01892 if(enabled_navdata_trackers_send && pub_navdata_trackers_send.getNumSubscribers()>0)
01893 {
01894 navdata_trackers_send_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01895 navdata_trackers_send_msg.header.stamp = received;
01896 navdata_trackers_send_msg.header.frame_id = drone_frame_base;
01897
01898 {
01899 uint16_t c = n.navdata_trackers_send.tag;
01900 uint16_t m;
01901 m = c;
01902
01903 navdata_trackers_send_msg.tag = m;
01904 }
01905
01906 {
01907 uint16_t c = n.navdata_trackers_send.size;
01908 uint16_t m;
01909 m = c;
01910
01911 navdata_trackers_send_msg.size = m;
01912 }
01913
01914 navdata_trackers_send_msg.locked.clear();
01915 for(int i=0; i<DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT; i++)
01916 {
01917 int32_t c = n.navdata_trackers_send.locked[i];
01918 int32_t m;
01919 m = c;
01920
01921 navdata_trackers_send_msg.locked.push_back(m);
01922 }
01923
01924 navdata_trackers_send_msg.point.clear();
01925 for(int i=0; i<DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT; i++)
01926 {
01927 screen_point_t c = n.navdata_trackers_send.point[i];
01928 ardrone_autonomy::vector21 m;
01929 m.x = c.x;
01930 m.y = c.y;
01931
01932 navdata_trackers_send_msg.point.push_back(m);
01933 }
01934
01935 pub_navdata_trackers_send.publish(navdata_trackers_send_msg);
01936 }
01937
01938
01939
01940 if(enabled_navdata_vision_detect && pub_navdata_vision_detect.getNumSubscribers()>0)
01941 {
01942 navdata_vision_detect_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
01943 navdata_vision_detect_msg.header.stamp = received;
01944 navdata_vision_detect_msg.header.frame_id = drone_frame_base;
01945
01946 {
01947 uint16_t c = n.navdata_vision_detect.tag;
01948 uint16_t m;
01949 m = c;
01950
01951 navdata_vision_detect_msg.tag = m;
01952 }
01953
01954 {
01955 uint16_t c = n.navdata_vision_detect.size;
01956 uint16_t m;
01957 m = c;
01958
01959 navdata_vision_detect_msg.size = m;
01960 }
01961
01962 {
01963 uint32_t c = n.navdata_vision_detect.nb_detected;
01964 uint32_t m;
01965 m = c;
01966
01967 navdata_vision_detect_msg.nb_detected = m;
01968 }
01969
01970 navdata_vision_detect_msg.type.clear();
01971 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
01972 {
01973 uint32_t c = n.navdata_vision_detect.type[i];
01974 uint32_t m;
01975 m = c;
01976
01977 navdata_vision_detect_msg.type.push_back(m);
01978 }
01979
01980 navdata_vision_detect_msg.xc.clear();
01981 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
01982 {
01983 uint32_t c = n.navdata_vision_detect.xc[i];
01984 uint32_t m;
01985 m = c;
01986
01987 navdata_vision_detect_msg.xc.push_back(m);
01988 }
01989
01990 navdata_vision_detect_msg.yc.clear();
01991 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
01992 {
01993 uint32_t c = n.navdata_vision_detect.yc[i];
01994 uint32_t m;
01995 m = c;
01996
01997 navdata_vision_detect_msg.yc.push_back(m);
01998 }
01999
02000 navdata_vision_detect_msg.width.clear();
02001 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
02002 {
02003 uint32_t c = n.navdata_vision_detect.width[i];
02004 uint32_t m;
02005 m = c;
02006
02007 navdata_vision_detect_msg.width.push_back(m);
02008 }
02009
02010 navdata_vision_detect_msg.height.clear();
02011 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
02012 {
02013 uint32_t c = n.navdata_vision_detect.height[i];
02014 uint32_t m;
02015 m = c;
02016
02017 navdata_vision_detect_msg.height.push_back(m);
02018 }
02019
02020 navdata_vision_detect_msg.dist.clear();
02021 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
02022 {
02023 uint32_t c = n.navdata_vision_detect.dist[i];
02024 uint32_t m;
02025 m = c;
02026
02027 navdata_vision_detect_msg.dist.push_back(m);
02028 }
02029
02030 navdata_vision_detect_msg.orientation_angle.clear();
02031 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
02032 {
02033 float32_t c = n.navdata_vision_detect.orientation_angle[i];
02034 float32_t m;
02035 m = c;
02036
02037 navdata_vision_detect_msg.orientation_angle.push_back(m);
02038 }
02039
02040 navdata_vision_detect_msg.rotation.clear();
02041 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
02042 {
02043 matrix33_t c = n.navdata_vision_detect.rotation[i];
02044 ardrone_autonomy::matrix33 m;
02045 m.m11 = c.m11;
02046 m.m12 = c.m12;
02047 m.m13 = c.m13;
02048 m.m21 = c.m21;
02049 m.m22 = c.m22;
02050 m.m23 = c.m23;
02051 m.m31 = c.m31;
02052 m.m32 = c.m32;
02053 m.m33 = c.m33;
02054
02055 navdata_vision_detect_msg.rotation.push_back(m);
02056 }
02057
02058 navdata_vision_detect_msg.translation.clear();
02059 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
02060 {
02061 vector31_t c = n.navdata_vision_detect.translation[i];
02062 ardrone_autonomy::vector31 m;
02063 m.x = c.x;
02064 m.y = c.y;
02065 m.z = c.z;
02066
02067 navdata_vision_detect_msg.translation.push_back(m);
02068 }
02069
02070 navdata_vision_detect_msg.camera_source.clear();
02071 for(int i=0; i<NB_NAVDATA_DETECTION_RESULTS; i++)
02072 {
02073 uint32_t c = n.navdata_vision_detect.camera_source[i];
02074 uint32_t m;
02075 m = c;
02076
02077 navdata_vision_detect_msg.camera_source.push_back(m);
02078 }
02079
02080 pub_navdata_vision_detect.publish(navdata_vision_detect_msg);
02081 }
02082
02083
02084
02085 if(enabled_navdata_watchdog && pub_navdata_watchdog.getNumSubscribers()>0)
02086 {
02087 navdata_watchdog_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02088 navdata_watchdog_msg.header.stamp = received;
02089 navdata_watchdog_msg.header.frame_id = drone_frame_base;
02090
02091 {
02092 uint16_t c = n.navdata_watchdog.tag;
02093 uint16_t m;
02094 m = c;
02095
02096 navdata_watchdog_msg.tag = m;
02097 }
02098
02099 {
02100 uint16_t c = n.navdata_watchdog.size;
02101 uint16_t m;
02102 m = c;
02103
02104 navdata_watchdog_msg.size = m;
02105 }
02106
02107 pub_navdata_watchdog.publish(navdata_watchdog_msg);
02108 }
02109
02110
02111
02112 if(enabled_navdata_adc_data_frame && pub_navdata_adc_data_frame.getNumSubscribers()>0)
02113 {
02114 navdata_adc_data_frame_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02115 navdata_adc_data_frame_msg.header.stamp = received;
02116 navdata_adc_data_frame_msg.header.frame_id = drone_frame_base;
02117
02118 {
02119 uint16_t c = n.navdata_adc_data_frame.tag;
02120 uint16_t m;
02121 m = c;
02122
02123 navdata_adc_data_frame_msg.tag = m;
02124 }
02125
02126 {
02127 uint16_t c = n.navdata_adc_data_frame.size;
02128 uint16_t m;
02129 m = c;
02130
02131 navdata_adc_data_frame_msg.size = m;
02132 }
02133
02134 {
02135 uint32_t c = n.navdata_adc_data_frame.version;
02136 uint32_t m;
02137 m = c;
02138
02139 navdata_adc_data_frame_msg.version = m;
02140 }
02141
02142 navdata_adc_data_frame_msg.data_frame.clear();
02143 for(int i=0; i<32; i++)
02144 {
02145 uint8_t c = n.navdata_adc_data_frame.data_frame[i];
02146 uint8_t m;
02147 m = c;
02148
02149 navdata_adc_data_frame_msg.data_frame.push_back(m);
02150 }
02151
02152 pub_navdata_adc_data_frame.publish(navdata_adc_data_frame_msg);
02153 }
02154
02155
02156
02157 if(enabled_navdata_video_stream && pub_navdata_video_stream.getNumSubscribers()>0)
02158 {
02159 navdata_video_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02160 navdata_video_stream_msg.header.stamp = received;
02161 navdata_video_stream_msg.header.frame_id = drone_frame_base;
02162
02163 {
02164 uint16_t c = n.navdata_video_stream.tag;
02165 uint16_t m;
02166 m = c;
02167
02168 navdata_video_stream_msg.tag = m;
02169 }
02170
02171 {
02172 uint16_t c = n.navdata_video_stream.size;
02173 uint16_t m;
02174 m = c;
02175
02176 navdata_video_stream_msg.size = m;
02177 }
02178
02179 {
02180 uint8_t c = n.navdata_video_stream.quant;
02181 uint8_t m;
02182 m = c;
02183
02184 navdata_video_stream_msg.quant = m;
02185 }
02186
02187 {
02188 uint32_t c = n.navdata_video_stream.frame_size;
02189 uint32_t m;
02190 m = c;
02191
02192 navdata_video_stream_msg.frame_size = m;
02193 }
02194
02195 {
02196 uint32_t c = n.navdata_video_stream.frame_number;
02197 uint32_t m;
02198 m = c;
02199
02200 navdata_video_stream_msg.frame_number = m;
02201 }
02202
02203 {
02204 uint32_t c = n.navdata_video_stream.atcmd_ref_seq;
02205 uint32_t m;
02206 m = c;
02207
02208 navdata_video_stream_msg.atcmd_ref_seq = m;
02209 }
02210
02211 {
02212 uint32_t c = n.navdata_video_stream.atcmd_mean_ref_gap;
02213 uint32_t m;
02214 m = c;
02215
02216 navdata_video_stream_msg.atcmd_mean_ref_gap = m;
02217 }
02218
02219 {
02220 float32_t c = n.navdata_video_stream.atcmd_var_ref_gap;
02221 float32_t m;
02222 m = c;
02223
02224 navdata_video_stream_msg.atcmd_var_ref_gap = m;
02225 }
02226
02227 {
02228 uint32_t c = n.navdata_video_stream.atcmd_ref_quality;
02229 uint32_t m;
02230 m = c;
02231
02232 navdata_video_stream_msg.atcmd_ref_quality = m;
02233 }
02234
02235 {
02236 uint32_t c = n.navdata_video_stream.desired_bitrate;
02237 uint32_t m;
02238 m = c;
02239
02240 navdata_video_stream_msg.desired_bitrate = m;
02241 }
02242
02243 {
02244 int32_t c = n.navdata_video_stream.data2;
02245 int32_t m;
02246 m = c;
02247
02248 navdata_video_stream_msg.data2 = m;
02249 }
02250
02251 {
02252 int32_t c = n.navdata_video_stream.data3;
02253 int32_t m;
02254 m = c;
02255
02256 navdata_video_stream_msg.data3 = m;
02257 }
02258
02259 {
02260 int32_t c = n.navdata_video_stream.data4;
02261 int32_t m;
02262 m = c;
02263
02264 navdata_video_stream_msg.data4 = m;
02265 }
02266
02267 {
02268 int32_t c = n.navdata_video_stream.data5;
02269 int32_t m;
02270 m = c;
02271
02272 navdata_video_stream_msg.data5 = m;
02273 }
02274
02275 {
02276 uint32_t c = n.navdata_video_stream.fifo_queue_level;
02277 uint32_t m;
02278 m = c;
02279
02280 navdata_video_stream_msg.fifo_queue_level = m;
02281 }
02282
02283 pub_navdata_video_stream.publish(navdata_video_stream_msg);
02284 }
02285
02286
02287
02288 if(enabled_navdata_games && pub_navdata_games.getNumSubscribers()>0)
02289 {
02290 navdata_games_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02291 navdata_games_msg.header.stamp = received;
02292 navdata_games_msg.header.frame_id = drone_frame_base;
02293
02294 {
02295 uint16_t c = n.navdata_games.tag;
02296 uint16_t m;
02297 m = c;
02298
02299 navdata_games_msg.tag = m;
02300 }
02301
02302 {
02303 uint16_t c = n.navdata_games.size;
02304 uint16_t m;
02305 m = c;
02306
02307 navdata_games_msg.size = m;
02308 }
02309
02310 {
02311 uint32_t c = n.navdata_games.double_tap_counter;
02312 uint32_t m;
02313 m = c;
02314
02315 navdata_games_msg.double_tap_counter = m;
02316 }
02317
02318 {
02319 uint32_t c = n.navdata_games.finish_line_counter;
02320 uint32_t m;
02321 m = c;
02322
02323 navdata_games_msg.finish_line_counter = m;
02324 }
02325
02326 pub_navdata_games.publish(navdata_games_msg);
02327 }
02328
02329
02330
02331 if(enabled_navdata_pressure_raw && pub_navdata_pressure_raw.getNumSubscribers()>0)
02332 {
02333 navdata_pressure_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02334 navdata_pressure_raw_msg.header.stamp = received;
02335 navdata_pressure_raw_msg.header.frame_id = drone_frame_base;
02336
02337 {
02338 uint16_t c = n.navdata_pressure_raw.tag;
02339 uint16_t m;
02340 m = c;
02341
02342 navdata_pressure_raw_msg.tag = m;
02343 }
02344
02345 {
02346 uint16_t c = n.navdata_pressure_raw.size;
02347 uint16_t m;
02348 m = c;
02349
02350 navdata_pressure_raw_msg.size = m;
02351 }
02352
02353 {
02354 int32_t c = n.navdata_pressure_raw.up;
02355 int32_t m;
02356 m = c;
02357
02358 navdata_pressure_raw_msg.up = m;
02359 }
02360
02361 {
02362 int16_t c = n.navdata_pressure_raw.ut;
02363 int16_t m;
02364 m = c;
02365
02366 navdata_pressure_raw_msg.ut = m;
02367 }
02368
02369 {
02370 int32_t c = n.navdata_pressure_raw.Temperature_meas;
02371 int32_t m;
02372 m = c;
02373
02374 navdata_pressure_raw_msg.Temperature_meas = m;
02375 }
02376
02377 {
02378 int32_t c = n.navdata_pressure_raw.Pression_meas;
02379 int32_t m;
02380 m = c;
02381
02382 navdata_pressure_raw_msg.Pression_meas = m;
02383 }
02384
02385 pub_navdata_pressure_raw.publish(navdata_pressure_raw_msg);
02386 }
02387
02388
02389
02390 if(enabled_navdata_magneto && pub_navdata_magneto.getNumSubscribers()>0)
02391 {
02392 navdata_magneto_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02393 navdata_magneto_msg.header.stamp = received;
02394 navdata_magneto_msg.header.frame_id = drone_frame_base;
02395
02396 {
02397 uint16_t c = n.navdata_magneto.tag;
02398 uint16_t m;
02399 m = c;
02400
02401 navdata_magneto_msg.tag = m;
02402 }
02403
02404 {
02405 uint16_t c = n.navdata_magneto.size;
02406 uint16_t m;
02407 m = c;
02408
02409 navdata_magneto_msg.size = m;
02410 }
02411
02412 {
02413 int16_t c = n.navdata_magneto.mx;
02414 int16_t m;
02415 m = c;
02416
02417 navdata_magneto_msg.mx = m;
02418 }
02419
02420 {
02421 int16_t c = n.navdata_magneto.my;
02422 int16_t m;
02423 m = c;
02424
02425 navdata_magneto_msg.my = m;
02426 }
02427
02428 {
02429 int16_t c = n.navdata_magneto.mz;
02430 int16_t m;
02431 m = c;
02432
02433 navdata_magneto_msg.mz = m;
02434 }
02435
02436 {
02437 vector31_t c = n.navdata_magneto.magneto_raw;
02438 ardrone_autonomy::vector31 m;
02439 m.x = c.x;
02440 m.y = c.y;
02441 m.z = c.z;
02442
02443 navdata_magneto_msg.magneto_raw = m;
02444 }
02445
02446 {
02447 vector31_t c = n.navdata_magneto.magneto_rectified;
02448 ardrone_autonomy::vector31 m;
02449 m.x = c.x;
02450 m.y = c.y;
02451 m.z = c.z;
02452
02453 navdata_magneto_msg.magneto_rectified = m;
02454 }
02455
02456 {
02457 vector31_t c = n.navdata_magneto.magneto_offset;
02458 ardrone_autonomy::vector31 m;
02459 m.x = c.x;
02460 m.y = c.y;
02461 m.z = c.z;
02462
02463 navdata_magneto_msg.magneto_offset = m;
02464 }
02465
02466 {
02467 float32_t c = n.navdata_magneto.heading_unwrapped;
02468 float32_t m;
02469 m = c;
02470
02471 navdata_magneto_msg.heading_unwrapped = m;
02472 }
02473
02474 {
02475 float32_t c = n.navdata_magneto.heading_gyro_unwrapped;
02476 float32_t m;
02477 m = c;
02478
02479 navdata_magneto_msg.heading_gyro_unwrapped = m;
02480 }
02481
02482 {
02483 float32_t c = n.navdata_magneto.heading_fusion_unwrapped;
02484 float32_t m;
02485 m = c;
02486
02487 navdata_magneto_msg.heading_fusion_unwrapped = m;
02488 }
02489
02490 {
02491 char c = n.navdata_magneto.magneto_calibration_ok;
02492 char m;
02493 m = c;
02494
02495 navdata_magneto_msg.magneto_calibration_ok = m;
02496 }
02497
02498 {
02499 uint32_t c = n.navdata_magneto.magneto_state;
02500 uint32_t m;
02501 m = c;
02502
02503 navdata_magneto_msg.magneto_state = m;
02504 }
02505
02506 {
02507 float32_t c = n.navdata_magneto.magneto_radius;
02508 float32_t m;
02509 m = c;
02510
02511 navdata_magneto_msg.magneto_radius = m;
02512 }
02513
02514 {
02515 float32_t c = n.navdata_magneto.error_mean;
02516 float32_t m;
02517 m = c;
02518
02519 navdata_magneto_msg.error_mean = m;
02520 }
02521
02522 {
02523 float32_t c = n.navdata_magneto.error_var;
02524 float32_t m;
02525 m = c;
02526
02527 navdata_magneto_msg.error_var = m;
02528 }
02529
02530 pub_navdata_magneto.publish(navdata_magneto_msg);
02531 }
02532
02533
02534
02535 if(enabled_navdata_wind_speed && pub_navdata_wind_speed.getNumSubscribers()>0)
02536 {
02537 navdata_wind_speed_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02538 navdata_wind_speed_msg.header.stamp = received;
02539 navdata_wind_speed_msg.header.frame_id = drone_frame_base;
02540
02541 {
02542 uint16_t c = n.navdata_wind_speed.tag;
02543 uint16_t m;
02544 m = c;
02545
02546 navdata_wind_speed_msg.tag = m;
02547 }
02548
02549 {
02550 uint16_t c = n.navdata_wind_speed.size;
02551 uint16_t m;
02552 m = c;
02553
02554 navdata_wind_speed_msg.size = m;
02555 }
02556
02557 {
02558 float32_t c = n.navdata_wind_speed.wind_speed;
02559 float32_t m;
02560 m = c;
02561
02562 navdata_wind_speed_msg.wind_speed = m;
02563 }
02564
02565 {
02566 float32_t c = n.navdata_wind_speed.wind_angle;
02567 float32_t m;
02568 m = c;
02569
02570 navdata_wind_speed_msg.wind_angle = m;
02571 }
02572
02573 {
02574 float32_t c = n.navdata_wind_speed.wind_compensation_theta;
02575 float32_t m;
02576 m = c;
02577
02578 navdata_wind_speed_msg.wind_compensation_theta = m;
02579 }
02580
02581 {
02582 float32_t c = n.navdata_wind_speed.wind_compensation_phi;
02583 float32_t m;
02584 m = c;
02585
02586 navdata_wind_speed_msg.wind_compensation_phi = m;
02587 }
02588
02589 {
02590 float32_t c = n.navdata_wind_speed.state_x1;
02591 float32_t m;
02592 m = c;
02593
02594 navdata_wind_speed_msg.state_x1 = m;
02595 }
02596
02597 {
02598 float32_t c = n.navdata_wind_speed.state_x2;
02599 float32_t m;
02600 m = c;
02601
02602 navdata_wind_speed_msg.state_x2 = m;
02603 }
02604
02605 {
02606 float32_t c = n.navdata_wind_speed.state_x3;
02607 float32_t m;
02608 m = c;
02609
02610 navdata_wind_speed_msg.state_x3 = m;
02611 }
02612
02613 {
02614 float32_t c = n.navdata_wind_speed.state_x4;
02615 float32_t m;
02616 m = c;
02617
02618 navdata_wind_speed_msg.state_x4 = m;
02619 }
02620
02621 {
02622 float32_t c = n.navdata_wind_speed.state_x5;
02623 float32_t m;
02624 m = c;
02625
02626 navdata_wind_speed_msg.state_x5 = m;
02627 }
02628
02629 {
02630 float32_t c = n.navdata_wind_speed.state_x6;
02631 float32_t m;
02632 m = c;
02633
02634 navdata_wind_speed_msg.state_x6 = m;
02635 }
02636
02637 {
02638 float32_t c = n.navdata_wind_speed.magneto_debug1;
02639 float32_t m;
02640 m = c;
02641
02642 navdata_wind_speed_msg.magneto_debug1 = m;
02643 }
02644
02645 {
02646 float32_t c = n.navdata_wind_speed.magneto_debug2;
02647 float32_t m;
02648 m = c;
02649
02650 navdata_wind_speed_msg.magneto_debug2 = m;
02651 }
02652
02653 {
02654 float32_t c = n.navdata_wind_speed.magneto_debug3;
02655 float32_t m;
02656 m = c;
02657
02658 navdata_wind_speed_msg.magneto_debug3 = m;
02659 }
02660
02661 pub_navdata_wind_speed.publish(navdata_wind_speed_msg);
02662 }
02663
02664
02665
02666 if(enabled_navdata_kalman_pressure && pub_navdata_kalman_pressure.getNumSubscribers()>0)
02667 {
02668 navdata_kalman_pressure_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02669 navdata_kalman_pressure_msg.header.stamp = received;
02670 navdata_kalman_pressure_msg.header.frame_id = drone_frame_base;
02671
02672 {
02673 uint16_t c = n.navdata_kalman_pressure.tag;
02674 uint16_t m;
02675 m = c;
02676
02677 navdata_kalman_pressure_msg.tag = m;
02678 }
02679
02680 {
02681 uint16_t c = n.navdata_kalman_pressure.size;
02682 uint16_t m;
02683 m = c;
02684
02685 navdata_kalman_pressure_msg.size = m;
02686 }
02687
02688 {
02689 float32_t c = n.navdata_kalman_pressure.offset_pressure;
02690 float32_t m;
02691 m = c;
02692
02693 navdata_kalman_pressure_msg.offset_pressure = m;
02694 }
02695
02696 {
02697 float32_t c = n.navdata_kalman_pressure.est_z;
02698 float32_t m;
02699 m = c;
02700
02701 navdata_kalman_pressure_msg.est_z = m;
02702 }
02703
02704 {
02705 float32_t c = n.navdata_kalman_pressure.est_zdot;
02706 float32_t m;
02707 m = c;
02708
02709 navdata_kalman_pressure_msg.est_zdot = m;
02710 }
02711
02712 {
02713 float32_t c = n.navdata_kalman_pressure.est_bias_PWM;
02714 float32_t m;
02715 m = c;
02716
02717 navdata_kalman_pressure_msg.est_bias_PWM = m;
02718 }
02719
02720 {
02721 float32_t c = n.navdata_kalman_pressure.est_biais_pression;
02722 float32_t m;
02723 m = c;
02724
02725 navdata_kalman_pressure_msg.est_biais_pression = m;
02726 }
02727
02728 {
02729 float32_t c = n.navdata_kalman_pressure.offset_US;
02730 float32_t m;
02731 m = c;
02732
02733 navdata_kalman_pressure_msg.offset_US = m;
02734 }
02735
02736 {
02737 float32_t c = n.navdata_kalman_pressure.prediction_US;
02738 float32_t m;
02739 m = c;
02740
02741 navdata_kalman_pressure_msg.prediction_US = m;
02742 }
02743
02744 {
02745 float32_t c = n.navdata_kalman_pressure.cov_alt;
02746 float32_t m;
02747 m = c;
02748
02749 navdata_kalman_pressure_msg.cov_alt = m;
02750 }
02751
02752 {
02753 float32_t c = n.navdata_kalman_pressure.cov_PWM;
02754 float32_t m;
02755 m = c;
02756
02757 navdata_kalman_pressure_msg.cov_PWM = m;
02758 }
02759
02760 {
02761 float32_t c = n.navdata_kalman_pressure.cov_vitesse;
02762 float32_t m;
02763 m = c;
02764
02765 navdata_kalman_pressure_msg.cov_vitesse = m;
02766 }
02767
02768 {
02769 bool_t c = n.navdata_kalman_pressure.bool_effet_sol;
02770 bool_t m;
02771 m = c;
02772
02773 navdata_kalman_pressure_msg.bool_effet_sol = m;
02774 }
02775
02776 {
02777 float32_t c = n.navdata_kalman_pressure.somme_inno;
02778 float32_t m;
02779 m = c;
02780
02781 navdata_kalman_pressure_msg.somme_inno = m;
02782 }
02783
02784 {
02785 bool_t c = n.navdata_kalman_pressure.flag_rejet_US;
02786 bool_t m;
02787 m = c;
02788
02789 navdata_kalman_pressure_msg.flag_rejet_US = m;
02790 }
02791
02792 {
02793 float32_t c = n.navdata_kalman_pressure.u_multisinus;
02794 float32_t m;
02795 m = c;
02796
02797 navdata_kalman_pressure_msg.u_multisinus = m;
02798 }
02799
02800 {
02801 float32_t c = n.navdata_kalman_pressure.gaz_altitude;
02802 float32_t m;
02803 m = c;
02804
02805 navdata_kalman_pressure_msg.gaz_altitude = m;
02806 }
02807
02808 {
02809 bool_t c = n.navdata_kalman_pressure.Flag_multisinus;
02810 bool_t m;
02811 m = c;
02812
02813 navdata_kalman_pressure_msg.Flag_multisinus = m;
02814 }
02815
02816 {
02817 bool_t c = n.navdata_kalman_pressure.Flag_multisinus_debut;
02818 bool_t m;
02819 m = c;
02820
02821 navdata_kalman_pressure_msg.Flag_multisinus_debut = m;
02822 }
02823
02824 pub_navdata_kalman_pressure.publish(navdata_kalman_pressure_msg);
02825 }
02826
02827
02828
02829 if(enabled_navdata_hdvideo_stream && pub_navdata_hdvideo_stream.getNumSubscribers()>0)
02830 {
02831 navdata_hdvideo_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02832 navdata_hdvideo_stream_msg.header.stamp = received;
02833 navdata_hdvideo_stream_msg.header.frame_id = drone_frame_base;
02834
02835 {
02836 uint16_t c = n.navdata_hdvideo_stream.tag;
02837 uint16_t m;
02838 m = c;
02839
02840 navdata_hdvideo_stream_msg.tag = m;
02841 }
02842
02843 {
02844 uint16_t c = n.navdata_hdvideo_stream.size;
02845 uint16_t m;
02846 m = c;
02847
02848 navdata_hdvideo_stream_msg.size = m;
02849 }
02850
02851 {
02852 uint32_t c = n.navdata_hdvideo_stream.hdvideo_state;
02853 uint32_t m;
02854 m = c;
02855
02856 navdata_hdvideo_stream_msg.hdvideo_state = m;
02857 }
02858
02859 {
02860 uint32_t c = n.navdata_hdvideo_stream.storage_fifo_nb_packets;
02861 uint32_t m;
02862 m = c;
02863
02864 navdata_hdvideo_stream_msg.storage_fifo_nb_packets = m;
02865 }
02866
02867 {
02868 uint32_t c = n.navdata_hdvideo_stream.storage_fifo_size;
02869 uint32_t m;
02870 m = c;
02871
02872 navdata_hdvideo_stream_msg.storage_fifo_size = m;
02873 }
02874
02875 {
02876 uint32_t c = n.navdata_hdvideo_stream.usbkey_size;
02877 uint32_t m;
02878 m = c;
02879
02880 navdata_hdvideo_stream_msg.usbkey_size = m;
02881 }
02882
02883 {
02884 uint32_t c = n.navdata_hdvideo_stream.usbkey_freespace;
02885 uint32_t m;
02886 m = c;
02887
02888 navdata_hdvideo_stream_msg.usbkey_freespace = m;
02889 }
02890
02891 {
02892 uint32_t c = n.navdata_hdvideo_stream.frame_number;
02893 uint32_t m;
02894 m = c;
02895
02896 navdata_hdvideo_stream_msg.frame_number = m;
02897 }
02898
02899 {
02900 uint32_t c = n.navdata_hdvideo_stream.usbkey_remaining_time;
02901 uint32_t m;
02902 m = c;
02903
02904 navdata_hdvideo_stream_msg.usbkey_remaining_time = m;
02905 }
02906
02907 pub_navdata_hdvideo_stream.publish(navdata_hdvideo_stream_msg);
02908 }
02909
02910
02911
02912 if(enabled_navdata_wifi && pub_navdata_wifi.getNumSubscribers()>0)
02913 {
02914 navdata_wifi_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02915 navdata_wifi_msg.header.stamp = received;
02916 navdata_wifi_msg.header.frame_id = drone_frame_base;
02917
02918 {
02919 uint16_t c = n.navdata_wifi.tag;
02920 uint16_t m;
02921 m = c;
02922
02923 navdata_wifi_msg.tag = m;
02924 }
02925
02926 {
02927 uint16_t c = n.navdata_wifi.size;
02928 uint16_t m;
02929 m = c;
02930
02931 navdata_wifi_msg.size = m;
02932 }
02933
02934 {
02935 uint32_t c = n.navdata_wifi.link_quality;
02936 uint32_t m;
02937 m = c;
02938
02939 navdata_wifi_msg.link_quality = m;
02940 }
02941
02942 pub_navdata_wifi.publish(navdata_wifi_msg);
02943 }
02944
02945
02946
02947 if(enabled_navdata_zimmu_3000 && pub_navdata_zimmu_3000.getNumSubscribers()>0)
02948 {
02949 navdata_zimmu_3000_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
02950 navdata_zimmu_3000_msg.header.stamp = received;
02951 navdata_zimmu_3000_msg.header.frame_id = drone_frame_base;
02952
02953 {
02954 uint16_t c = n.navdata_zimmu_3000.tag;
02955 uint16_t m;
02956 m = c;
02957
02958 navdata_zimmu_3000_msg.tag = m;
02959 }
02960
02961 {
02962 uint16_t c = n.navdata_zimmu_3000.size;
02963 uint16_t m;
02964 m = c;
02965
02966 navdata_zimmu_3000_msg.size = m;
02967 }
02968
02969 {
02970 int32_t c = n.navdata_zimmu_3000.vzimmuLSB;
02971 int32_t m;
02972 m = c;
02973
02974 navdata_zimmu_3000_msg.vzimmuLSB = m;
02975 }
02976
02977 {
02978 float32_t c = n.navdata_zimmu_3000.vzfind;
02979 float32_t m;
02980 m = c;
02981
02982 navdata_zimmu_3000_msg.vzfind = m;
02983 }
02984
02985 pub_navdata_zimmu_3000.publish(navdata_zimmu_3000_msg);
02986 }
02987
02988
02989
02990 }
02991 }
02992 #endif