NavdataMessageDefinitions.h
Go to the documentation of this file.
00001 // Autogenerated from source-files using the CreateNavdataFormat.py script
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 


ardrone_autonomy
Author(s): Mani Monajjemi
autogenerated on Mon Jan 6 2014 11:03:00