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