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


ardrone_autonomy
Author(s): Mani Monajjemi, Mani Monajjemi
autogenerated on Fri Dec 9 2016 03:36:59