From d3249e151e71b1ab8da1499aa4b7ef3cbd51cbf6 Mon Sep 17 00:00:00 2001 From: Mike Hamer Date: Tue, 27 Nov 2012 12:57:29 +0100 Subject: [PATCH 1/3] Fixed a bug whereby all navdata messages with array-typed members would grow in size. This bug was introduced when moving local scope message variables to class-level. This move meant that messages wouldn't be reset after being sent. This was not a problem for non-array type message members, which were reassigned before being sent. However for array-type message members, which are filled using .push_back, no reset occurred. This fix appropriately clears these members before re-filling them on every send-loop. --- scripts/NavdataMessageDefinitionsTemplate.c | 1 + src/NavdataMessageDefinitions.h | 21 +++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/scripts/NavdataMessageDefinitionsTemplate.c b/scripts/NavdataMessageDefinitionsTemplate.c index 88a911f..47c1814 100644 --- a/scripts/NavdataMessageDefinitionsTemplate.c +++ b/scripts/NavdataMessageDefinitionsTemplate.c @@ -77,6 +77,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) } % else: + ${item['struct_name']}_msg.${member['name']}.clear(); for(int i=0; i<${member['array_size']}; i++) {\ ${format_member(item, member, 'i')} diff --git a/src/NavdataMessageDefinitions.h b/src/NavdataMessageDefinitions.h index 269893a..c5eec3d 100644 --- a/src/NavdataMessageDefinitions.h +++ b/src/NavdataMessageDefinitions.h @@ -552,6 +552,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) navdata_raw_measures_msg.size = m; } + navdata_raw_measures_msg.raw_gyros.clear(); for(int i=0; i Date: Tue, 27 Nov 2012 13:51:32 +0100 Subject: [PATCH 2/3] Moved navdata initialization to configureDrone() rather than happening the first time that navdata is sent --- scripts/NavdataMessageDefinitionsTemplate.c | 54 +- src/NavdataMessageDefinitions.h | 2232 ++++++++++--------- src/ardrone_driver.cpp | 7 +- 3 files changed, 1153 insertions(+), 1140 deletions(-) diff --git a/scripts/NavdataMessageDefinitionsTemplate.c b/scripts/NavdataMessageDefinitionsTemplate.c index 47c1814..58b6ee2 100644 --- a/scripts/NavdataMessageDefinitionsTemplate.c +++ b/scripts/NavdataMessageDefinitionsTemplate.c @@ -31,11 +31,7 @@ bool initialized_navdata_publishers; #endif -#ifdef NAVDATA_STRUCTS_SOURCE -void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) -{ - const ros::Time now = ros::Time::now(); - +#ifdef NAVDATA_STRUCTS_INITIALIZE if(!initialized_navdata_publishers) { initialized_navdata_publishers = true; @@ -61,38 +57,46 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) % endfor } +#endif -% for item in structs: - if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0) +#ifdef NAVDATA_STRUCTS_SOURCE +void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) +{ + const ros::Time now = ros::Time::now(); + + if(initialized_navdata_publishers) { - ${item['struct_name']}_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - ${item['struct_name']}_msg.header.stamp = now; - ${item['struct_name']}_msg.header.frame_id = droneFrameBase; +% for item in structs: + if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0) + { + ${item['struct_name']}_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + ${item['struct_name']}_msg.header.stamp = now; + ${item['struct_name']}_msg.header.frame_id = droneFrameBase; % for member in item['members']: % if member['array_size'] is None: - {\ - ${format_member(item, member, None)} - ${item['struct_name']}_msg.${member['name']} = m; - } + {\ + ${format_member(item, member, None)} + ${item['struct_name']}_msg.${member['name']} = m; + } % else: - ${item['struct_name']}_msg.${member['name']}.clear(); - for(int i=0; i<${member['array_size']}; i++) - {\ - ${format_member(item, member, 'i')} - ${item['struct_name']}_msg.${member['name']}.push_back(m); - } - + ${item['struct_name']}_msg.${member['name']}.clear(); + for(int i=0; i<${member['array_size']}; i++) + {\ + ${format_member(item, member, 'i')} + ${item['struct_name']}_msg.${member['name']}.push_back(m); + } + % endif % endfor - pub_${item['struct_name']}.publish(${item['struct_name']}_msg); - } + pub_${item['struct_name']}.publish(${item['struct_name']}_msg); + } - //------------------------- + //------------------------- % endfor - + } } #endif diff --git a/src/NavdataMessageDefinitions.h b/src/NavdataMessageDefinitions.h index c5eec3d..8ef4703 100644 --- a/src/NavdataMessageDefinitions.h +++ b/src/NavdataMessageDefinitions.h @@ -135,11 +135,7 @@ bool initialized_navdata_publishers; #endif -#ifdef NAVDATA_STRUCTS_SOURCE -void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) -{ - const ros::Time now = ros::Time::now(); - +#ifdef NAVDATA_STRUCTS_INITIALIZE if(!initialized_navdata_publishers) { initialized_navdata_publishers = true; @@ -379,1640 +375,1648 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) //------------------------- } +#endif - if(enabled_navdata_demo && pub_navdata_demo.getNumSubscribers()>0) +#ifdef NAVDATA_STRUCTS_SOURCE +void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n) +{ + const ros::Time now = ros::Time::now(); + + if(initialized_navdata_publishers) { - navdata_demo_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_demo_msg.header.stamp = now; - navdata_demo_msg.header.frame_id = droneFrameBase; + if(enabled_navdata_demo && pub_navdata_demo.getNumSubscribers()>0) + { + navdata_demo_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_demo_msg.header.stamp = now; + navdata_demo_msg.header.frame_id = droneFrameBase; - { + { uint16_t c = n.navdata_demo.tag; uint16_t m; m = c; - navdata_demo_msg.tag = m; - } + navdata_demo_msg.tag = m; + } - { + { uint16_t c = n.navdata_demo.size; uint16_t m; m = c; - navdata_demo_msg.size = m; - } + navdata_demo_msg.size = m; + } - { + { uint32_t c = n.navdata_demo.ctrl_state; uint32_t m; m = c; - navdata_demo_msg.ctrl_state = m; - } + navdata_demo_msg.ctrl_state = m; + } - { + { uint32_t c = n.navdata_demo.vbat_flying_percentage; uint32_t m; m = c; - navdata_demo_msg.vbat_flying_percentage = m; - } + navdata_demo_msg.vbat_flying_percentage = m; + } - { + { float32_t c = n.navdata_demo.theta; float32_t m; m = c; - navdata_demo_msg.theta = m; - } + navdata_demo_msg.theta = m; + } - { + { float32_t c = n.navdata_demo.phi; float32_t m; m = c; - navdata_demo_msg.phi = m; - } + navdata_demo_msg.phi = m; + } - { + { float32_t c = n.navdata_demo.psi; float32_t m; m = c; - navdata_demo_msg.psi = m; - } + navdata_demo_msg.psi = m; + } - { + { int32_t c = n.navdata_demo.altitude; int32_t m; m = c; - navdata_demo_msg.altitude = m; - } + navdata_demo_msg.altitude = m; + } - { + { float32_t c = n.navdata_demo.vx; float32_t m; m = c; - navdata_demo_msg.vx = m; - } + navdata_demo_msg.vx = m; + } - { + { float32_t c = n.navdata_demo.vy; float32_t m; m = c; - navdata_demo_msg.vy = m; - } + navdata_demo_msg.vy = m; + } - { + { float32_t c = n.navdata_demo.vz; float32_t m; m = c; - navdata_demo_msg.vz = m; - } + navdata_demo_msg.vz = m; + } - { + { uint32_t c = n.navdata_demo.num_frames; uint32_t m; m = c; - navdata_demo_msg.num_frames = m; - } + navdata_demo_msg.num_frames = m; + } - { + { uint32_t c = n.navdata_demo.detection_camera_type; uint32_t m; m = c; - navdata_demo_msg.detection_camera_type = m; + navdata_demo_msg.detection_camera_type = m; + } + + pub_navdata_demo.publish(navdata_demo_msg); } - pub_navdata_demo.publish(navdata_demo_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_time && pub_navdata_time.getNumSubscribers()>0) + { + navdata_time_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_time_msg.header.stamp = now; + navdata_time_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_time && pub_navdata_time.getNumSubscribers()>0) - { - navdata_time_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_time_msg.header.stamp = now; - navdata_time_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_time.tag; uint16_t m; m = c; - navdata_time_msg.tag = m; - } + navdata_time_msg.tag = m; + } - { + { uint16_t c = n.navdata_time.size; uint16_t m; m = c; - navdata_time_msg.size = m; - } + navdata_time_msg.size = m; + } - { + { uint32_t c = n.navdata_time.time; uint32_t m; m = c; - navdata_time_msg.time = m; + navdata_time_msg.time = m; + } + + pub_navdata_time.publish(navdata_time_msg); } - pub_navdata_time.publish(navdata_time_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_raw_measures && pub_navdata_raw_measures.getNumSubscribers()>0) + { + navdata_raw_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_raw_measures_msg.header.stamp = now; + navdata_raw_measures_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_raw_measures && pub_navdata_raw_measures.getNumSubscribers()>0) - { - navdata_raw_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_raw_measures_msg.header.stamp = now; - navdata_raw_measures_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_raw_measures.tag; uint16_t m; m = c; - navdata_raw_measures_msg.tag = m; - } + navdata_raw_measures_msg.tag = m; + } - { + { uint16_t c = n.navdata_raw_measures.size; uint16_t m; m = c; - navdata_raw_measures_msg.size = m; - } + navdata_raw_measures_msg.size = m; + } - navdata_raw_measures_msg.raw_gyros.clear(); - for(int i=0; i0) + { + navdata_phys_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_phys_measures_msg.header.stamp = now; + navdata_phys_measures_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_phys_measures && pub_navdata_phys_measures.getNumSubscribers()>0) - { - navdata_phys_measures_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_phys_measures_msg.header.stamp = now; - navdata_phys_measures_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_phys_measures.tag; uint16_t m; m = c; - navdata_phys_measures_msg.tag = m; - } + navdata_phys_measures_msg.tag = m; + } - { + { uint16_t c = n.navdata_phys_measures.size; uint16_t m; m = c; - navdata_phys_measures_msg.size = m; - } + navdata_phys_measures_msg.size = m; + } - { + { float32_t c = n.navdata_phys_measures.accs_temp; float32_t m; m = c; - navdata_phys_measures_msg.accs_temp = m; - } + navdata_phys_measures_msg.accs_temp = m; + } - { + { uint16_t c = n.navdata_phys_measures.gyro_temp; uint16_t m; m = c; - navdata_phys_measures_msg.gyro_temp = m; - } + navdata_phys_measures_msg.gyro_temp = m; + } - navdata_phys_measures_msg.phys_accs.clear(); - for(int i=0; i0) + { + navdata_gyros_offsets_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_gyros_offsets_msg.header.stamp = now; + navdata_gyros_offsets_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_gyros_offsets && pub_navdata_gyros_offsets.getNumSubscribers()>0) - { - navdata_gyros_offsets_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_gyros_offsets_msg.header.stamp = now; - navdata_gyros_offsets_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_gyros_offsets.tag; uint16_t m; m = c; - navdata_gyros_offsets_msg.tag = m; - } + navdata_gyros_offsets_msg.tag = m; + } - { + { uint16_t c = n.navdata_gyros_offsets.size; uint16_t m; m = c; - navdata_gyros_offsets_msg.size = m; - } + navdata_gyros_offsets_msg.size = m; + } - navdata_gyros_offsets_msg.offset_g.clear(); - for(int i=0; i0) - { - navdata_euler_angles_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_euler_angles_msg.header.stamp = now; - navdata_euler_angles_msg.header.frame_id = droneFrameBase; + if(enabled_navdata_euler_angles && pub_navdata_euler_angles.getNumSubscribers()>0) + { + navdata_euler_angles_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_euler_angles_msg.header.stamp = now; + navdata_euler_angles_msg.header.frame_id = droneFrameBase; - { + { uint16_t c = n.navdata_euler_angles.tag; uint16_t m; m = c; - navdata_euler_angles_msg.tag = m; - } + navdata_euler_angles_msg.tag = m; + } - { + { uint16_t c = n.navdata_euler_angles.size; uint16_t m; m = c; - navdata_euler_angles_msg.size = m; - } + navdata_euler_angles_msg.size = m; + } - { + { float32_t c = n.navdata_euler_angles.theta_a; float32_t m; m = c; - navdata_euler_angles_msg.theta_a = m; - } + navdata_euler_angles_msg.theta_a = m; + } - { + { float32_t c = n.navdata_euler_angles.phi_a; float32_t m; m = c; - navdata_euler_angles_msg.phi_a = m; + navdata_euler_angles_msg.phi_a = m; + } + + pub_navdata_euler_angles.publish(navdata_euler_angles_msg); } - pub_navdata_euler_angles.publish(navdata_euler_angles_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_references && pub_navdata_references.getNumSubscribers()>0) + { + navdata_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_references_msg.header.stamp = now; + navdata_references_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_references && pub_navdata_references.getNumSubscribers()>0) - { - navdata_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_references_msg.header.stamp = now; - navdata_references_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_references.tag; uint16_t m; m = c; - navdata_references_msg.tag = m; - } + navdata_references_msg.tag = m; + } - { + { uint16_t c = n.navdata_references.size; uint16_t m; m = c; - navdata_references_msg.size = m; - } + navdata_references_msg.size = m; + } - { + { int32_t c = n.navdata_references.ref_theta; int32_t m; m = c; - navdata_references_msg.ref_theta = m; - } + navdata_references_msg.ref_theta = m; + } - { + { int32_t c = n.navdata_references.ref_phi; int32_t m; m = c; - navdata_references_msg.ref_phi = m; - } + navdata_references_msg.ref_phi = m; + } - { + { int32_t c = n.navdata_references.ref_theta_I; int32_t m; m = c; - navdata_references_msg.ref_theta_I = m; - } + navdata_references_msg.ref_theta_I = m; + } - { + { int32_t c = n.navdata_references.ref_phi_I; int32_t m; m = c; - navdata_references_msg.ref_phi_I = m; - } + navdata_references_msg.ref_phi_I = m; + } - { + { int32_t c = n.navdata_references.ref_pitch; int32_t m; m = c; - navdata_references_msg.ref_pitch = m; - } + navdata_references_msg.ref_pitch = m; + } - { + { int32_t c = n.navdata_references.ref_roll; int32_t m; m = c; - navdata_references_msg.ref_roll = m; - } + navdata_references_msg.ref_roll = m; + } - { + { int32_t c = n.navdata_references.ref_yaw; int32_t m; m = c; - navdata_references_msg.ref_yaw = m; - } + navdata_references_msg.ref_yaw = m; + } - { + { int32_t c = n.navdata_references.ref_psi; int32_t m; m = c; - navdata_references_msg.ref_psi = m; - } + navdata_references_msg.ref_psi = m; + } - { + { float32_t c = n.navdata_references.vx_ref; float32_t m; m = c; - navdata_references_msg.vx_ref = m; - } + navdata_references_msg.vx_ref = m; + } - { + { float32_t c = n.navdata_references.vy_ref; float32_t m; m = c; - navdata_references_msg.vy_ref = m; - } + navdata_references_msg.vy_ref = m; + } - { + { float32_t c = n.navdata_references.theta_mod; float32_t m; m = c; - navdata_references_msg.theta_mod = m; - } + navdata_references_msg.theta_mod = m; + } - { + { float32_t c = n.navdata_references.phi_mod; float32_t m; m = c; - navdata_references_msg.phi_mod = m; - } + navdata_references_msg.phi_mod = m; + } - { + { float32_t c = n.navdata_references.k_v_x; float32_t m; m = c; - navdata_references_msg.k_v_x = m; - } + navdata_references_msg.k_v_x = m; + } - { + { float32_t c = n.navdata_references.k_v_y; float32_t m; m = c; - navdata_references_msg.k_v_y = m; - } + navdata_references_msg.k_v_y = m; + } - { + { uint32_t c = n.navdata_references.k_mode; uint32_t m; m = c; - navdata_references_msg.k_mode = m; - } + navdata_references_msg.k_mode = m; + } - { + { float32_t c = n.navdata_references.ui_time; float32_t m; m = c; - navdata_references_msg.ui_time = m; - } + navdata_references_msg.ui_time = m; + } - { + { float32_t c = n.navdata_references.ui_theta; float32_t m; m = c; - navdata_references_msg.ui_theta = m; - } + navdata_references_msg.ui_theta = m; + } - { + { float32_t c = n.navdata_references.ui_phi; float32_t m; m = c; - navdata_references_msg.ui_phi = m; - } + navdata_references_msg.ui_phi = m; + } - { + { float32_t c = n.navdata_references.ui_psi; float32_t m; m = c; - navdata_references_msg.ui_psi = m; - } + navdata_references_msg.ui_psi = m; + } - { + { float32_t c = n.navdata_references.ui_psi_accuracy; float32_t m; m = c; - navdata_references_msg.ui_psi_accuracy = m; - } + navdata_references_msg.ui_psi_accuracy = m; + } - { + { int32_t c = n.navdata_references.ui_seq; int32_t m; m = c; - navdata_references_msg.ui_seq = m; + navdata_references_msg.ui_seq = m; + } + + pub_navdata_references.publish(navdata_references_msg); } - pub_navdata_references.publish(navdata_references_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_trims && pub_navdata_trims.getNumSubscribers()>0) + { + navdata_trims_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_trims_msg.header.stamp = now; + navdata_trims_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_trims && pub_navdata_trims.getNumSubscribers()>0) - { - navdata_trims_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_trims_msg.header.stamp = now; - navdata_trims_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_trims.tag; uint16_t m; m = c; - navdata_trims_msg.tag = m; - } + navdata_trims_msg.tag = m; + } - { + { uint16_t c = n.navdata_trims.size; uint16_t m; m = c; - navdata_trims_msg.size = m; - } + navdata_trims_msg.size = m; + } - { + { float32_t c = n.navdata_trims.angular_rates_trim_r; float32_t m; m = c; - navdata_trims_msg.angular_rates_trim_r = m; - } + navdata_trims_msg.angular_rates_trim_r = m; + } - { + { float32_t c = n.navdata_trims.euler_angles_trim_theta; float32_t m; m = c; - navdata_trims_msg.euler_angles_trim_theta = m; - } + navdata_trims_msg.euler_angles_trim_theta = m; + } - { + { float32_t c = n.navdata_trims.euler_angles_trim_phi; float32_t m; m = c; - navdata_trims_msg.euler_angles_trim_phi = m; + navdata_trims_msg.euler_angles_trim_phi = m; + } + + pub_navdata_trims.publish(navdata_trims_msg); } - pub_navdata_trims.publish(navdata_trims_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_rc_references && pub_navdata_rc_references.getNumSubscribers()>0) + { + navdata_rc_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_rc_references_msg.header.stamp = now; + navdata_rc_references_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_rc_references && pub_navdata_rc_references.getNumSubscribers()>0) - { - navdata_rc_references_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_rc_references_msg.header.stamp = now; - navdata_rc_references_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_rc_references.tag; uint16_t m; m = c; - navdata_rc_references_msg.tag = m; - } + navdata_rc_references_msg.tag = m; + } - { + { uint16_t c = n.navdata_rc_references.size; uint16_t m; m = c; - navdata_rc_references_msg.size = m; - } + navdata_rc_references_msg.size = m; + } - { + { int32_t c = n.navdata_rc_references.rc_ref_pitch; int32_t m; m = c; - navdata_rc_references_msg.rc_ref_pitch = m; - } + navdata_rc_references_msg.rc_ref_pitch = m; + } - { + { int32_t c = n.navdata_rc_references.rc_ref_roll; int32_t m; m = c; - navdata_rc_references_msg.rc_ref_roll = m; - } + navdata_rc_references_msg.rc_ref_roll = m; + } - { + { int32_t c = n.navdata_rc_references.rc_ref_yaw; int32_t m; m = c; - navdata_rc_references_msg.rc_ref_yaw = m; - } + navdata_rc_references_msg.rc_ref_yaw = m; + } - { + { int32_t c = n.navdata_rc_references.rc_ref_gaz; int32_t m; m = c; - navdata_rc_references_msg.rc_ref_gaz = m; - } + navdata_rc_references_msg.rc_ref_gaz = m; + } - { + { int32_t c = n.navdata_rc_references.rc_ref_ag; int32_t m; m = c; - navdata_rc_references_msg.rc_ref_ag = m; + navdata_rc_references_msg.rc_ref_ag = m; + } + + pub_navdata_rc_references.publish(navdata_rc_references_msg); } - pub_navdata_rc_references.publish(navdata_rc_references_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_pwm && pub_navdata_pwm.getNumSubscribers()>0) + { + navdata_pwm_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_pwm_msg.header.stamp = now; + navdata_pwm_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_pwm && pub_navdata_pwm.getNumSubscribers()>0) - { - navdata_pwm_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_pwm_msg.header.stamp = now; - navdata_pwm_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_pwm.tag; uint16_t m; m = c; - navdata_pwm_msg.tag = m; - } + navdata_pwm_msg.tag = m; + } - { + { uint16_t c = n.navdata_pwm.size; uint16_t m; m = c; - navdata_pwm_msg.size = m; - } + navdata_pwm_msg.size = m; + } - { + { uint8_t c = n.navdata_pwm.motor1; uint8_t m; m = c; - navdata_pwm_msg.motor1 = m; - } + navdata_pwm_msg.motor1 = m; + } - { + { uint8_t c = n.navdata_pwm.motor2; uint8_t m; m = c; - navdata_pwm_msg.motor2 = m; - } + navdata_pwm_msg.motor2 = m; + } - { + { uint8_t c = n.navdata_pwm.motor3; uint8_t m; m = c; - navdata_pwm_msg.motor3 = m; - } + navdata_pwm_msg.motor3 = m; + } - { + { uint8_t c = n.navdata_pwm.motor4; uint8_t m; m = c; - navdata_pwm_msg.motor4 = m; - } + navdata_pwm_msg.motor4 = m; + } - { + { uint8_t c = n.navdata_pwm.sat_motor1; uint8_t m; m = c; - navdata_pwm_msg.sat_motor1 = m; - } + navdata_pwm_msg.sat_motor1 = m; + } - { + { uint8_t c = n.navdata_pwm.sat_motor2; uint8_t m; m = c; - navdata_pwm_msg.sat_motor2 = m; - } + navdata_pwm_msg.sat_motor2 = m; + } - { + { uint8_t c = n.navdata_pwm.sat_motor3; uint8_t m; m = c; - navdata_pwm_msg.sat_motor3 = m; - } + navdata_pwm_msg.sat_motor3 = m; + } - { + { uint8_t c = n.navdata_pwm.sat_motor4; uint8_t m; m = c; - navdata_pwm_msg.sat_motor4 = m; - } + navdata_pwm_msg.sat_motor4 = m; + } - { + { float32_t c = n.navdata_pwm.gaz_feed_forward; float32_t m; m = c; - navdata_pwm_msg.gaz_feed_forward = m; - } + navdata_pwm_msg.gaz_feed_forward = m; + } - { + { float32_t c = n.navdata_pwm.gaz_altitude; float32_t m; m = c; - navdata_pwm_msg.gaz_altitude = m; - } + navdata_pwm_msg.gaz_altitude = m; + } - { + { float32_t c = n.navdata_pwm.altitude_integral; float32_t m; m = c; - navdata_pwm_msg.altitude_integral = m; - } + navdata_pwm_msg.altitude_integral = m; + } - { + { float32_t c = n.navdata_pwm.vz_ref; float32_t m; m = c; - navdata_pwm_msg.vz_ref = m; - } + navdata_pwm_msg.vz_ref = m; + } - { + { int32_t c = n.navdata_pwm.u_pitch; int32_t m; m = c; - navdata_pwm_msg.u_pitch = m; - } + navdata_pwm_msg.u_pitch = m; + } - { + { int32_t c = n.navdata_pwm.u_roll; int32_t m; m = c; - navdata_pwm_msg.u_roll = m; - } + navdata_pwm_msg.u_roll = m; + } - { + { int32_t c = n.navdata_pwm.u_yaw; int32_t m; m = c; - navdata_pwm_msg.u_yaw = m; - } + navdata_pwm_msg.u_yaw = m; + } - { + { float32_t c = n.navdata_pwm.yaw_u_I; float32_t m; m = c; - navdata_pwm_msg.yaw_u_I = m; - } + navdata_pwm_msg.yaw_u_I = m; + } - { + { int32_t c = n.navdata_pwm.u_pitch_planif; int32_t m; m = c; - navdata_pwm_msg.u_pitch_planif = m; - } + navdata_pwm_msg.u_pitch_planif = m; + } - { + { int32_t c = n.navdata_pwm.u_roll_planif; int32_t m; m = c; - navdata_pwm_msg.u_roll_planif = m; - } + navdata_pwm_msg.u_roll_planif = m; + } - { + { int32_t c = n.navdata_pwm.u_yaw_planif; int32_t m; m = c; - navdata_pwm_msg.u_yaw_planif = m; - } + navdata_pwm_msg.u_yaw_planif = m; + } - { + { float32_t c = n.navdata_pwm.u_gaz_planif; float32_t m; m = c; - navdata_pwm_msg.u_gaz_planif = m; - } + navdata_pwm_msg.u_gaz_planif = m; + } - { + { uint16_t c = n.navdata_pwm.current_motor1; uint16_t m; m = c; - navdata_pwm_msg.current_motor1 = m; - } + navdata_pwm_msg.current_motor1 = m; + } - { + { uint16_t c = n.navdata_pwm.current_motor2; uint16_t m; m = c; - navdata_pwm_msg.current_motor2 = m; - } + navdata_pwm_msg.current_motor2 = m; + } - { + { uint16_t c = n.navdata_pwm.current_motor3; uint16_t m; m = c; - navdata_pwm_msg.current_motor3 = m; - } + navdata_pwm_msg.current_motor3 = m; + } - { + { uint16_t c = n.navdata_pwm.current_motor4; uint16_t m; m = c; - navdata_pwm_msg.current_motor4 = m; - } + navdata_pwm_msg.current_motor4 = m; + } - { + { float32_t c = n.navdata_pwm.altitude_der; float32_t m; m = c; - navdata_pwm_msg.altitude_der = m; + navdata_pwm_msg.altitude_der = m; + } + + pub_navdata_pwm.publish(navdata_pwm_msg); } - pub_navdata_pwm.publish(navdata_pwm_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_altitude && pub_navdata_altitude.getNumSubscribers()>0) + { + navdata_altitude_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_altitude_msg.header.stamp = now; + navdata_altitude_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_altitude && pub_navdata_altitude.getNumSubscribers()>0) - { - navdata_altitude_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_altitude_msg.header.stamp = now; - navdata_altitude_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_altitude.tag; uint16_t m; m = c; - navdata_altitude_msg.tag = m; - } + navdata_altitude_msg.tag = m; + } - { + { uint16_t c = n.navdata_altitude.size; uint16_t m; m = c; - navdata_altitude_msg.size = m; - } + navdata_altitude_msg.size = m; + } - { + { int32_t c = n.navdata_altitude.altitude_vision; int32_t m; m = c; - navdata_altitude_msg.altitude_vision = m; - } + navdata_altitude_msg.altitude_vision = m; + } - { + { float32_t c = n.navdata_altitude.altitude_vz; float32_t m; m = c; - navdata_altitude_msg.altitude_vz = m; - } + navdata_altitude_msg.altitude_vz = m; + } - { + { int32_t c = n.navdata_altitude.altitude_ref; int32_t m; m = c; - navdata_altitude_msg.altitude_ref = m; - } + navdata_altitude_msg.altitude_ref = m; + } - { + { int32_t c = n.navdata_altitude.altitude_raw; int32_t m; m = c; - navdata_altitude_msg.altitude_raw = m; - } + navdata_altitude_msg.altitude_raw = m; + } - { + { float32_t c = n.navdata_altitude.obs_accZ; float32_t m; m = c; - navdata_altitude_msg.obs_accZ = m; - } + navdata_altitude_msg.obs_accZ = m; + } - { + { float32_t c = n.navdata_altitude.obs_alt; float32_t m; m = c; - navdata_altitude_msg.obs_alt = m; - } + navdata_altitude_msg.obs_alt = m; + } - { + { vector31_t c = n.navdata_altitude.obs_x; ardrone_autonomy::vector31 m; m.x = c.x; m.y = c.y; m.z = c.z; - navdata_altitude_msg.obs_x = m; - } + navdata_altitude_msg.obs_x = m; + } - { + { uint32_t c = n.navdata_altitude.obs_state; uint32_t m; m = c; - navdata_altitude_msg.obs_state = m; - } + navdata_altitude_msg.obs_state = m; + } - { + { vector21_t c = n.navdata_altitude.est_vb; ardrone_autonomy::vector21 m; m.x = c.x; m.y = c.y; - navdata_altitude_msg.est_vb = m; - } + navdata_altitude_msg.est_vb = m; + } - { + { uint32_t c = n.navdata_altitude.est_state; uint32_t m; m = c; - navdata_altitude_msg.est_state = m; + navdata_altitude_msg.est_state = m; + } + + pub_navdata_altitude.publish(navdata_altitude_msg); } - pub_navdata_altitude.publish(navdata_altitude_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_vision_raw && pub_navdata_vision_raw.getNumSubscribers()>0) + { + navdata_vision_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_vision_raw_msg.header.stamp = now; + navdata_vision_raw_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_vision_raw && pub_navdata_vision_raw.getNumSubscribers()>0) - { - navdata_vision_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_vision_raw_msg.header.stamp = now; - navdata_vision_raw_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_vision_raw.tag; uint16_t m; m = c; - navdata_vision_raw_msg.tag = m; - } + navdata_vision_raw_msg.tag = m; + } - { + { uint16_t c = n.navdata_vision_raw.size; uint16_t m; m = c; - navdata_vision_raw_msg.size = m; - } + navdata_vision_raw_msg.size = m; + } - { + { float32_t c = n.navdata_vision_raw.vision_tx_raw; float32_t m; m = c; - navdata_vision_raw_msg.vision_tx_raw = m; - } + navdata_vision_raw_msg.vision_tx_raw = m; + } - { + { float32_t c = n.navdata_vision_raw.vision_ty_raw; float32_t m; m = c; - navdata_vision_raw_msg.vision_ty_raw = m; - } + navdata_vision_raw_msg.vision_ty_raw = m; + } - { + { float32_t c = n.navdata_vision_raw.vision_tz_raw; float32_t m; m = c; - navdata_vision_raw_msg.vision_tz_raw = m; + navdata_vision_raw_msg.vision_tz_raw = m; + } + + pub_navdata_vision_raw.publish(navdata_vision_raw_msg); } - pub_navdata_vision_raw.publish(navdata_vision_raw_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_vision_of && pub_navdata_vision_of.getNumSubscribers()>0) + { + navdata_vision_of_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_vision_of_msg.header.stamp = now; + navdata_vision_of_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_vision_of && pub_navdata_vision_of.getNumSubscribers()>0) - { - navdata_vision_of_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_vision_of_msg.header.stamp = now; - navdata_vision_of_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_vision_of.tag; uint16_t m; m = c; - navdata_vision_of_msg.tag = m; - } + navdata_vision_of_msg.tag = m; + } - { + { uint16_t c = n.navdata_vision_of.size; uint16_t m; m = c; - navdata_vision_of_msg.size = m; - } + navdata_vision_of_msg.size = m; + } - navdata_vision_of_msg.of_dx.clear(); - for(int i=0; i<5; i++) - { + navdata_vision_of_msg.of_dx.clear(); + for(int i=0; i<5; i++) + { float32_t c = n.navdata_vision_of.of_dx[i]; float32_t m; m = c; - navdata_vision_of_msg.of_dx.push_back(m); - } - - navdata_vision_of_msg.of_dy.clear(); - for(int i=0; i<5; i++) - { + navdata_vision_of_msg.of_dx.push_back(m); + } + + navdata_vision_of_msg.of_dy.clear(); + for(int i=0; i<5; i++) + { float32_t c = n.navdata_vision_of.of_dy[i]; float32_t m; m = c; - navdata_vision_of_msg.of_dy.push_back(m); + navdata_vision_of_msg.of_dy.push_back(m); + } + + pub_navdata_vision_of.publish(navdata_vision_of_msg); } - - pub_navdata_vision_of.publish(navdata_vision_of_msg); - } - //------------------------- + //------------------------- - if(enabled_navdata_vision && pub_navdata_vision.getNumSubscribers()>0) - { - navdata_vision_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_vision_msg.header.stamp = now; - navdata_vision_msg.header.frame_id = droneFrameBase; + if(enabled_navdata_vision && pub_navdata_vision.getNumSubscribers()>0) + { + navdata_vision_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_vision_msg.header.stamp = now; + navdata_vision_msg.header.frame_id = droneFrameBase; - { + { uint16_t c = n.navdata_vision.tag; uint16_t m; m = c; - navdata_vision_msg.tag = m; - } + navdata_vision_msg.tag = m; + } - { + { uint16_t c = n.navdata_vision.size; uint16_t m; m = c; - navdata_vision_msg.size = m; - } + navdata_vision_msg.size = m; + } - { + { uint32_t c = n.navdata_vision.vision_state; uint32_t m; m = c; - navdata_vision_msg.vision_state = m; - } + navdata_vision_msg.vision_state = m; + } - { + { int32_t c = n.navdata_vision.vision_misc; int32_t m; m = c; - navdata_vision_msg.vision_misc = m; - } + navdata_vision_msg.vision_misc = m; + } - { + { float32_t c = n.navdata_vision.vision_phi_trim; float32_t m; m = c; - navdata_vision_msg.vision_phi_trim = m; - } + navdata_vision_msg.vision_phi_trim = m; + } - { + { float32_t c = n.navdata_vision.vision_phi_ref_prop; float32_t m; m = c; - navdata_vision_msg.vision_phi_ref_prop = m; - } + navdata_vision_msg.vision_phi_ref_prop = m; + } - { + { float32_t c = n.navdata_vision.vision_theta_trim; float32_t m; m = c; - navdata_vision_msg.vision_theta_trim = m; - } + navdata_vision_msg.vision_theta_trim = m; + } - { + { float32_t c = n.navdata_vision.vision_theta_ref_prop; float32_t m; m = c; - navdata_vision_msg.vision_theta_ref_prop = m; - } + navdata_vision_msg.vision_theta_ref_prop = m; + } - { + { int32_t c = n.navdata_vision.new_raw_picture; int32_t m; m = c; - navdata_vision_msg.new_raw_picture = m; - } + navdata_vision_msg.new_raw_picture = m; + } - { + { float32_t c = n.navdata_vision.theta_capture; float32_t m; m = c; - navdata_vision_msg.theta_capture = m; - } + navdata_vision_msg.theta_capture = m; + } - { + { float32_t c = n.navdata_vision.phi_capture; float32_t m; m = c; - navdata_vision_msg.phi_capture = m; - } + navdata_vision_msg.phi_capture = m; + } - { + { float32_t c = n.navdata_vision.psi_capture; float32_t m; m = c; - navdata_vision_msg.psi_capture = m; - } + navdata_vision_msg.psi_capture = m; + } - { + { int32_t c = n.navdata_vision.altitude_capture; int32_t m; m = c; - navdata_vision_msg.altitude_capture = m; - } + navdata_vision_msg.altitude_capture = m; + } - { + { uint32_t c = n.navdata_vision.time_capture; uint32_t m; m = c; - navdata_vision_msg.time_capture = m; - } + navdata_vision_msg.time_capture = m; + } - { + { velocities_t c = n.navdata_vision.body_v; ardrone_autonomy::vector31 m; m.x = c.x; m.y = c.y; m.z = c.z; - navdata_vision_msg.body_v = m; - } + navdata_vision_msg.body_v = m; + } - { + { float32_t c = n.navdata_vision.delta_phi; float32_t m; m = c; - navdata_vision_msg.delta_phi = m; - } + navdata_vision_msg.delta_phi = m; + } - { + { float32_t c = n.navdata_vision.delta_theta; float32_t m; m = c; - navdata_vision_msg.delta_theta = m; - } + navdata_vision_msg.delta_theta = m; + } - { + { float32_t c = n.navdata_vision.delta_psi; float32_t m; m = c; - navdata_vision_msg.delta_psi = m; - } + navdata_vision_msg.delta_psi = m; + } - { + { uint32_t c = n.navdata_vision.gold_defined; uint32_t m; m = c; - navdata_vision_msg.gold_defined = m; - } + navdata_vision_msg.gold_defined = m; + } - { + { uint32_t c = n.navdata_vision.gold_reset; uint32_t m; m = c; - navdata_vision_msg.gold_reset = m; - } + navdata_vision_msg.gold_reset = m; + } - { + { float32_t c = n.navdata_vision.gold_x; float32_t m; m = c; - navdata_vision_msg.gold_x = m; - } + navdata_vision_msg.gold_x = m; + } - { + { float32_t c = n.navdata_vision.gold_y; float32_t m; m = c; - navdata_vision_msg.gold_y = m; + navdata_vision_msg.gold_y = m; + } + + pub_navdata_vision.publish(navdata_vision_msg); } - pub_navdata_vision.publish(navdata_vision_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_vision_perf && pub_navdata_vision_perf.getNumSubscribers()>0) + { + navdata_vision_perf_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_vision_perf_msg.header.stamp = now; + navdata_vision_perf_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_vision_perf && pub_navdata_vision_perf.getNumSubscribers()>0) - { - navdata_vision_perf_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_vision_perf_msg.header.stamp = now; - navdata_vision_perf_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_vision_perf.tag; uint16_t m; m = c; - navdata_vision_perf_msg.tag = m; - } + navdata_vision_perf_msg.tag = m; + } - { + { uint16_t c = n.navdata_vision_perf.size; uint16_t m; m = c; - navdata_vision_perf_msg.size = m; - } + navdata_vision_perf_msg.size = m; + } - { + { float32_t c = n.navdata_vision_perf.time_corners; float32_t m; m = c; - navdata_vision_perf_msg.time_corners = m; - } + navdata_vision_perf_msg.time_corners = m; + } - { + { float32_t c = n.navdata_vision_perf.time_compute; float32_t m; m = c; - navdata_vision_perf_msg.time_compute = m; - } + navdata_vision_perf_msg.time_compute = m; + } - { + { float32_t c = n.navdata_vision_perf.time_tracking; float32_t m; m = c; - navdata_vision_perf_msg.time_tracking = m; - } + navdata_vision_perf_msg.time_tracking = m; + } - { + { float32_t c = n.navdata_vision_perf.time_trans; float32_t m; m = c; - navdata_vision_perf_msg.time_trans = m; - } + navdata_vision_perf_msg.time_trans = m; + } - { + { float32_t c = n.navdata_vision_perf.time_update; float32_t m; m = c; - navdata_vision_perf_msg.time_update = m; - } + navdata_vision_perf_msg.time_update = m; + } - navdata_vision_perf_msg.time_custom.clear(); - for(int i=0; i0) - { - navdata_trackers_send_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_trackers_send_msg.header.stamp = now; - navdata_trackers_send_msg.header.frame_id = droneFrameBase; + if(enabled_navdata_trackers_send && pub_navdata_trackers_send.getNumSubscribers()>0) + { + navdata_trackers_send_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_trackers_send_msg.header.stamp = now; + navdata_trackers_send_msg.header.frame_id = droneFrameBase; - { + { uint16_t c = n.navdata_trackers_send.tag; uint16_t m; m = c; - navdata_trackers_send_msg.tag = m; - } + navdata_trackers_send_msg.tag = m; + } - { + { uint16_t c = n.navdata_trackers_send.size; uint16_t m; m = c; - navdata_trackers_send_msg.size = m; - } + navdata_trackers_send_msg.size = m; + } - navdata_trackers_send_msg.locked.clear(); - for(int i=0; i0) - { - navdata_vision_detect_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_vision_detect_msg.header.stamp = now; - navdata_vision_detect_msg.header.frame_id = droneFrameBase; + if(enabled_navdata_vision_detect && pub_navdata_vision_detect.getNumSubscribers()>0) + { + navdata_vision_detect_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_vision_detect_msg.header.stamp = now; + navdata_vision_detect_msg.header.frame_id = droneFrameBase; - { + { uint16_t c = n.navdata_vision_detect.tag; uint16_t m; m = c; - navdata_vision_detect_msg.tag = m; - } + navdata_vision_detect_msg.tag = m; + } - { + { uint16_t c = n.navdata_vision_detect.size; uint16_t m; m = c; - navdata_vision_detect_msg.size = m; - } + navdata_vision_detect_msg.size = m; + } - { + { uint32_t c = n.navdata_vision_detect.nb_detected; uint32_t m; m = c; - navdata_vision_detect_msg.nb_detected = m; - } + navdata_vision_detect_msg.nb_detected = m; + } - navdata_vision_detect_msg.type.clear(); - for(int i=0; i0) - { - navdata_watchdog_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_watchdog_msg.header.stamp = now; - navdata_watchdog_msg.header.frame_id = droneFrameBase; + if(enabled_navdata_watchdog && pub_navdata_watchdog.getNumSubscribers()>0) + { + navdata_watchdog_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_watchdog_msg.header.stamp = now; + navdata_watchdog_msg.header.frame_id = droneFrameBase; - { + { uint16_t c = n.navdata_watchdog.tag; uint16_t m; m = c; - navdata_watchdog_msg.tag = m; - } + navdata_watchdog_msg.tag = m; + } - { + { uint16_t c = n.navdata_watchdog.size; uint16_t m; m = c; - navdata_watchdog_msg.size = m; + navdata_watchdog_msg.size = m; + } + + pub_navdata_watchdog.publish(navdata_watchdog_msg); } - pub_navdata_watchdog.publish(navdata_watchdog_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_adc_data_frame && pub_navdata_adc_data_frame.getNumSubscribers()>0) + { + navdata_adc_data_frame_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_adc_data_frame_msg.header.stamp = now; + navdata_adc_data_frame_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_adc_data_frame && pub_navdata_adc_data_frame.getNumSubscribers()>0) - { - navdata_adc_data_frame_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_adc_data_frame_msg.header.stamp = now; - navdata_adc_data_frame_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_adc_data_frame.tag; uint16_t m; m = c; - navdata_adc_data_frame_msg.tag = m; - } + navdata_adc_data_frame_msg.tag = m; + } - { + { uint16_t c = n.navdata_adc_data_frame.size; uint16_t m; m = c; - navdata_adc_data_frame_msg.size = m; - } + navdata_adc_data_frame_msg.size = m; + } - { + { uint32_t c = n.navdata_adc_data_frame.version; uint32_t m; m = c; - navdata_adc_data_frame_msg.version = m; - } + navdata_adc_data_frame_msg.version = m; + } - navdata_adc_data_frame_msg.data_frame.clear(); - for(int i=0; i<32; i++) - { + navdata_adc_data_frame_msg.data_frame.clear(); + for(int i=0; i<32; i++) + { uint8_t c = n.navdata_adc_data_frame.data_frame[i]; uint8_t m; m = c; - navdata_adc_data_frame_msg.data_frame.push_back(m); + navdata_adc_data_frame_msg.data_frame.push_back(m); + } + + pub_navdata_adc_data_frame.publish(navdata_adc_data_frame_msg); } - - pub_navdata_adc_data_frame.publish(navdata_adc_data_frame_msg); - } - //------------------------- + //------------------------- - if(enabled_navdata_video_stream && pub_navdata_video_stream.getNumSubscribers()>0) - { - navdata_video_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_video_stream_msg.header.stamp = now; - navdata_video_stream_msg.header.frame_id = droneFrameBase; + if(enabled_navdata_video_stream && pub_navdata_video_stream.getNumSubscribers()>0) + { + navdata_video_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_video_stream_msg.header.stamp = now; + navdata_video_stream_msg.header.frame_id = droneFrameBase; - { + { uint16_t c = n.navdata_video_stream.tag; uint16_t m; m = c; - navdata_video_stream_msg.tag = m; - } + navdata_video_stream_msg.tag = m; + } - { + { uint16_t c = n.navdata_video_stream.size; uint16_t m; m = c; - navdata_video_stream_msg.size = m; - } + navdata_video_stream_msg.size = m; + } - { + { uint8_t c = n.navdata_video_stream.quant; uint8_t m; m = c; - navdata_video_stream_msg.quant = m; - } + navdata_video_stream_msg.quant = m; + } - { + { uint32_t c = n.navdata_video_stream.frame_size; uint32_t m; m = c; - navdata_video_stream_msg.frame_size = m; - } + navdata_video_stream_msg.frame_size = m; + } - { + { uint32_t c = n.navdata_video_stream.frame_number; uint32_t m; m = c; - navdata_video_stream_msg.frame_number = m; - } + navdata_video_stream_msg.frame_number = m; + } - { + { uint32_t c = n.navdata_video_stream.atcmd_ref_seq; uint32_t m; m = c; - navdata_video_stream_msg.atcmd_ref_seq = m; - } + navdata_video_stream_msg.atcmd_ref_seq = m; + } - { + { uint32_t c = n.navdata_video_stream.atcmd_mean_ref_gap; uint32_t m; m = c; - navdata_video_stream_msg.atcmd_mean_ref_gap = m; - } + navdata_video_stream_msg.atcmd_mean_ref_gap = m; + } - { + { float32_t c = n.navdata_video_stream.atcmd_var_ref_gap; float32_t m; m = c; - navdata_video_stream_msg.atcmd_var_ref_gap = m; - } + navdata_video_stream_msg.atcmd_var_ref_gap = m; + } - { + { uint32_t c = n.navdata_video_stream.atcmd_ref_quality; uint32_t m; m = c; - navdata_video_stream_msg.atcmd_ref_quality = m; - } + navdata_video_stream_msg.atcmd_ref_quality = m; + } - { + { uint32_t c = n.navdata_video_stream.desired_bitrate; uint32_t m; m = c; - navdata_video_stream_msg.desired_bitrate = m; - } + navdata_video_stream_msg.desired_bitrate = m; + } - { + { int32_t c = n.navdata_video_stream.data2; int32_t m; m = c; - navdata_video_stream_msg.data2 = m; - } + navdata_video_stream_msg.data2 = m; + } - { + { int32_t c = n.navdata_video_stream.data3; int32_t m; m = c; - navdata_video_stream_msg.data3 = m; - } + navdata_video_stream_msg.data3 = m; + } - { + { int32_t c = n.navdata_video_stream.data4; int32_t m; m = c; - navdata_video_stream_msg.data4 = m; - } + navdata_video_stream_msg.data4 = m; + } - { + { int32_t c = n.navdata_video_stream.data5; int32_t m; m = c; - navdata_video_stream_msg.data5 = m; - } + navdata_video_stream_msg.data5 = m; + } - { + { uint32_t c = n.navdata_video_stream.fifo_queue_level; uint32_t m; m = c; - navdata_video_stream_msg.fifo_queue_level = m; + navdata_video_stream_msg.fifo_queue_level = m; + } + + pub_navdata_video_stream.publish(navdata_video_stream_msg); } - pub_navdata_video_stream.publish(navdata_video_stream_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_games && pub_navdata_games.getNumSubscribers()>0) + { + navdata_games_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_games_msg.header.stamp = now; + navdata_games_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_games && pub_navdata_games.getNumSubscribers()>0) - { - navdata_games_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_games_msg.header.stamp = now; - navdata_games_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_games.tag; uint16_t m; m = c; - navdata_games_msg.tag = m; - } + navdata_games_msg.tag = m; + } - { + { uint16_t c = n.navdata_games.size; uint16_t m; m = c; - navdata_games_msg.size = m; - } + navdata_games_msg.size = m; + } - { + { uint32_t c = n.navdata_games.double_tap_counter; uint32_t m; m = c; - navdata_games_msg.double_tap_counter = m; - } + navdata_games_msg.double_tap_counter = m; + } - { + { uint32_t c = n.navdata_games.finish_line_counter; uint32_t m; m = c; - navdata_games_msg.finish_line_counter = m; + navdata_games_msg.finish_line_counter = m; + } + + pub_navdata_games.publish(navdata_games_msg); } - pub_navdata_games.publish(navdata_games_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_pressure_raw && pub_navdata_pressure_raw.getNumSubscribers()>0) + { + navdata_pressure_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_pressure_raw_msg.header.stamp = now; + navdata_pressure_raw_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_pressure_raw && pub_navdata_pressure_raw.getNumSubscribers()>0) - { - navdata_pressure_raw_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_pressure_raw_msg.header.stamp = now; - navdata_pressure_raw_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_pressure_raw.tag; uint16_t m; m = c; - navdata_pressure_raw_msg.tag = m; - } + navdata_pressure_raw_msg.tag = m; + } - { + { uint16_t c = n.navdata_pressure_raw.size; uint16_t m; m = c; - navdata_pressure_raw_msg.size = m; - } + navdata_pressure_raw_msg.size = m; + } - { + { int32_t c = n.navdata_pressure_raw.up; int32_t m; m = c; - navdata_pressure_raw_msg.up = m; - } + navdata_pressure_raw_msg.up = m; + } - { + { int16_t c = n.navdata_pressure_raw.ut; int16_t m; m = c; - navdata_pressure_raw_msg.ut = m; - } + navdata_pressure_raw_msg.ut = m; + } - { + { int32_t c = n.navdata_pressure_raw.Temperature_meas; int32_t m; m = c; - navdata_pressure_raw_msg.Temperature_meas = m; - } + navdata_pressure_raw_msg.Temperature_meas = m; + } - { + { int32_t c = n.navdata_pressure_raw.Pression_meas; int32_t m; m = c; - navdata_pressure_raw_msg.Pression_meas = m; + navdata_pressure_raw_msg.Pression_meas = m; + } + + pub_navdata_pressure_raw.publish(navdata_pressure_raw_msg); } - pub_navdata_pressure_raw.publish(navdata_pressure_raw_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_magneto && pub_navdata_magneto.getNumSubscribers()>0) + { + navdata_magneto_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_magneto_msg.header.stamp = now; + navdata_magneto_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_magneto && pub_navdata_magneto.getNumSubscribers()>0) - { - navdata_magneto_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_magneto_msg.header.stamp = now; - navdata_magneto_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_magneto.tag; uint16_t m; m = c; - navdata_magneto_msg.tag = m; - } + navdata_magneto_msg.tag = m; + } - { + { uint16_t c = n.navdata_magneto.size; uint16_t m; m = c; - navdata_magneto_msg.size = m; - } + navdata_magneto_msg.size = m; + } - { + { int16_t c = n.navdata_magneto.mx; int16_t m; m = c; - navdata_magneto_msg.mx = m; - } + navdata_magneto_msg.mx = m; + } - { + { int16_t c = n.navdata_magneto.my; int16_t m; m = c; - navdata_magneto_msg.my = m; - } + navdata_magneto_msg.my = m; + } - { + { int16_t c = n.navdata_magneto.mz; int16_t m; m = c; - navdata_magneto_msg.mz = m; - } + navdata_magneto_msg.mz = m; + } - { + { vector31_t c = n.navdata_magneto.magneto_raw; ardrone_autonomy::vector31 m; m.x = c.x; m.y = c.y; m.z = c.z; - navdata_magneto_msg.magneto_raw = m; - } + navdata_magneto_msg.magneto_raw = m; + } - { + { vector31_t c = n.navdata_magneto.magneto_rectified; ardrone_autonomy::vector31 m; m.x = c.x; m.y = c.y; m.z = c.z; - navdata_magneto_msg.magneto_rectified = m; - } + navdata_magneto_msg.magneto_rectified = m; + } - { + { vector31_t c = n.navdata_magneto.magneto_offset; ardrone_autonomy::vector31 m; m.x = c.x; m.y = c.y; m.z = c.z; - navdata_magneto_msg.magneto_offset = m; - } + navdata_magneto_msg.magneto_offset = m; + } - { + { float32_t c = n.navdata_magneto.heading_unwrapped; float32_t m; m = c; - navdata_magneto_msg.heading_unwrapped = m; - } + navdata_magneto_msg.heading_unwrapped = m; + } - { + { float32_t c = n.navdata_magneto.heading_gyro_unwrapped; float32_t m; m = c; - navdata_magneto_msg.heading_gyro_unwrapped = m; - } + navdata_magneto_msg.heading_gyro_unwrapped = m; + } - { + { float32_t c = n.navdata_magneto.heading_fusion_unwrapped; float32_t m; m = c; - navdata_magneto_msg.heading_fusion_unwrapped = m; - } + navdata_magneto_msg.heading_fusion_unwrapped = m; + } - { + { char c = n.navdata_magneto.magneto_calibration_ok; char m; m = c; - navdata_magneto_msg.magneto_calibration_ok = m; - } + navdata_magneto_msg.magneto_calibration_ok = m; + } - { + { uint32_t c = n.navdata_magneto.magneto_state; uint32_t m; m = c; - navdata_magneto_msg.magneto_state = m; - } + navdata_magneto_msg.magneto_state = m; + } - { + { float32_t c = n.navdata_magneto.magneto_radius; float32_t m; m = c; - navdata_magneto_msg.magneto_radius = m; - } + navdata_magneto_msg.magneto_radius = m; + } - { + { float32_t c = n.navdata_magneto.error_mean; float32_t m; m = c; - navdata_magneto_msg.error_mean = m; - } + navdata_magneto_msg.error_mean = m; + } - { + { float32_t c = n.navdata_magneto.error_var; float32_t m; m = c; - navdata_magneto_msg.error_var = m; + navdata_magneto_msg.error_var = m; + } + + pub_navdata_magneto.publish(navdata_magneto_msg); } - pub_navdata_magneto.publish(navdata_magneto_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_wind_speed && pub_navdata_wind_speed.getNumSubscribers()>0) + { + navdata_wind_speed_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_wind_speed_msg.header.stamp = now; + navdata_wind_speed_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_wind_speed && pub_navdata_wind_speed.getNumSubscribers()>0) - { - navdata_wind_speed_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_wind_speed_msg.header.stamp = now; - navdata_wind_speed_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_wind_speed.tag; uint16_t m; m = c; - navdata_wind_speed_msg.tag = m; - } + navdata_wind_speed_msg.tag = m; + } - { + { uint16_t c = n.navdata_wind_speed.size; uint16_t m; m = c; - navdata_wind_speed_msg.size = m; - } + navdata_wind_speed_msg.size = m; + } - { + { float32_t c = n.navdata_wind_speed.wind_speed; float32_t m; m = c; - navdata_wind_speed_msg.wind_speed = m; - } + navdata_wind_speed_msg.wind_speed = m; + } - { + { float32_t c = n.navdata_wind_speed.wind_angle; float32_t m; m = c; - navdata_wind_speed_msg.wind_angle = m; - } + navdata_wind_speed_msg.wind_angle = m; + } - { + { float32_t c = n.navdata_wind_speed.wind_compensation_theta; float32_t m; m = c; - navdata_wind_speed_msg.wind_compensation_theta = m; - } + navdata_wind_speed_msg.wind_compensation_theta = m; + } - { + { float32_t c = n.navdata_wind_speed.wind_compensation_phi; float32_t m; m = c; - navdata_wind_speed_msg.wind_compensation_phi = m; - } + navdata_wind_speed_msg.wind_compensation_phi = m; + } - { + { float32_t c = n.navdata_wind_speed.state_x1; float32_t m; m = c; - navdata_wind_speed_msg.state_x1 = m; - } + navdata_wind_speed_msg.state_x1 = m; + } - { + { float32_t c = n.navdata_wind_speed.state_x2; float32_t m; m = c; - navdata_wind_speed_msg.state_x2 = m; - } + navdata_wind_speed_msg.state_x2 = m; + } - { + { float32_t c = n.navdata_wind_speed.state_x3; float32_t m; m = c; - navdata_wind_speed_msg.state_x3 = m; - } + navdata_wind_speed_msg.state_x3 = m; + } - { + { float32_t c = n.navdata_wind_speed.state_x4; float32_t m; m = c; - navdata_wind_speed_msg.state_x4 = m; - } + navdata_wind_speed_msg.state_x4 = m; + } - { + { float32_t c = n.navdata_wind_speed.state_x5; float32_t m; m = c; - navdata_wind_speed_msg.state_x5 = m; - } + navdata_wind_speed_msg.state_x5 = m; + } - { + { float32_t c = n.navdata_wind_speed.state_x6; float32_t m; m = c; - navdata_wind_speed_msg.state_x6 = m; - } + navdata_wind_speed_msg.state_x6 = m; + } - { + { float32_t c = n.navdata_wind_speed.magneto_debug1; float32_t m; m = c; - navdata_wind_speed_msg.magneto_debug1 = m; - } + navdata_wind_speed_msg.magneto_debug1 = m; + } - { + { float32_t c = n.navdata_wind_speed.magneto_debug2; float32_t m; m = c; - navdata_wind_speed_msg.magneto_debug2 = m; - } + navdata_wind_speed_msg.magneto_debug2 = m; + } - { + { float32_t c = n.navdata_wind_speed.magneto_debug3; float32_t m; m = c; - navdata_wind_speed_msg.magneto_debug3 = m; + navdata_wind_speed_msg.magneto_debug3 = m; + } + + pub_navdata_wind_speed.publish(navdata_wind_speed_msg); } - pub_navdata_wind_speed.publish(navdata_wind_speed_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_kalman_pressure && pub_navdata_kalman_pressure.getNumSubscribers()>0) + { + navdata_kalman_pressure_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_kalman_pressure_msg.header.stamp = now; + navdata_kalman_pressure_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_kalman_pressure && pub_navdata_kalman_pressure.getNumSubscribers()>0) - { - navdata_kalman_pressure_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_kalman_pressure_msg.header.stamp = now; - navdata_kalman_pressure_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_kalman_pressure.tag; uint16_t m; m = c; - navdata_kalman_pressure_msg.tag = m; - } + navdata_kalman_pressure_msg.tag = m; + } - { + { uint16_t c = n.navdata_kalman_pressure.size; uint16_t m; m = c; - navdata_kalman_pressure_msg.size = m; - } + navdata_kalman_pressure_msg.size = m; + } - { + { float32_t c = n.navdata_kalman_pressure.offset_pressure; float32_t m; m = c; - navdata_kalman_pressure_msg.offset_pressure = m; - } + navdata_kalman_pressure_msg.offset_pressure = m; + } - { + { float32_t c = n.navdata_kalman_pressure.est_z; float32_t m; m = c; - navdata_kalman_pressure_msg.est_z = m; - } + navdata_kalman_pressure_msg.est_z = m; + } - { + { float32_t c = n.navdata_kalman_pressure.est_zdot; float32_t m; m = c; - navdata_kalman_pressure_msg.est_zdot = m; - } + navdata_kalman_pressure_msg.est_zdot = m; + } - { + { float32_t c = n.navdata_kalman_pressure.est_bias_PWM; float32_t m; m = c; - navdata_kalman_pressure_msg.est_bias_PWM = m; - } + navdata_kalman_pressure_msg.est_bias_PWM = m; + } - { + { float32_t c = n.navdata_kalman_pressure.est_biais_pression; float32_t m; m = c; - navdata_kalman_pressure_msg.est_biais_pression = m; - } + navdata_kalman_pressure_msg.est_biais_pression = m; + } - { + { float32_t c = n.navdata_kalman_pressure.offset_US; float32_t m; m = c; - navdata_kalman_pressure_msg.offset_US = m; - } + navdata_kalman_pressure_msg.offset_US = m; + } - { + { float32_t c = n.navdata_kalman_pressure.prediction_US; float32_t m; m = c; - navdata_kalman_pressure_msg.prediction_US = m; - } + navdata_kalman_pressure_msg.prediction_US = m; + } - { + { float32_t c = n.navdata_kalman_pressure.cov_alt; float32_t m; m = c; - navdata_kalman_pressure_msg.cov_alt = m; - } + navdata_kalman_pressure_msg.cov_alt = m; + } - { + { float32_t c = n.navdata_kalman_pressure.cov_PWM; float32_t m; m = c; - navdata_kalman_pressure_msg.cov_PWM = m; - } + navdata_kalman_pressure_msg.cov_PWM = m; + } - { + { float32_t c = n.navdata_kalman_pressure.cov_vitesse; float32_t m; m = c; - navdata_kalman_pressure_msg.cov_vitesse = m; - } + navdata_kalman_pressure_msg.cov_vitesse = m; + } - { + { bool_t c = n.navdata_kalman_pressure.bool_effet_sol; bool_t m; m = c; - navdata_kalman_pressure_msg.bool_effet_sol = m; - } + navdata_kalman_pressure_msg.bool_effet_sol = m; + } - { + { float32_t c = n.navdata_kalman_pressure.somme_inno; float32_t m; m = c; - navdata_kalman_pressure_msg.somme_inno = m; - } + navdata_kalman_pressure_msg.somme_inno = m; + } - { + { bool_t c = n.navdata_kalman_pressure.flag_rejet_US; bool_t m; m = c; - navdata_kalman_pressure_msg.flag_rejet_US = m; - } + navdata_kalman_pressure_msg.flag_rejet_US = m; + } - { + { float32_t c = n.navdata_kalman_pressure.u_multisinus; float32_t m; m = c; - navdata_kalman_pressure_msg.u_multisinus = m; - } + navdata_kalman_pressure_msg.u_multisinus = m; + } - { + { float32_t c = n.navdata_kalman_pressure.gaz_altitude; float32_t m; m = c; - navdata_kalman_pressure_msg.gaz_altitude = m; - } + navdata_kalman_pressure_msg.gaz_altitude = m; + } - { + { bool_t c = n.navdata_kalman_pressure.Flag_multisinus; bool_t m; m = c; - navdata_kalman_pressure_msg.Flag_multisinus = m; - } + navdata_kalman_pressure_msg.Flag_multisinus = m; + } - { + { bool_t c = n.navdata_kalman_pressure.Flag_multisinus_debut; bool_t m; m = c; - navdata_kalman_pressure_msg.Flag_multisinus_debut = m; + navdata_kalman_pressure_msg.Flag_multisinus_debut = m; + } + + pub_navdata_kalman_pressure.publish(navdata_kalman_pressure_msg); } - pub_navdata_kalman_pressure.publish(navdata_kalman_pressure_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_hdvideo_stream && pub_navdata_hdvideo_stream.getNumSubscribers()>0) + { + navdata_hdvideo_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_hdvideo_stream_msg.header.stamp = now; + navdata_hdvideo_stream_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_hdvideo_stream && pub_navdata_hdvideo_stream.getNumSubscribers()>0) - { - navdata_hdvideo_stream_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_hdvideo_stream_msg.header.stamp = now; - navdata_hdvideo_stream_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_hdvideo_stream.tag; uint16_t m; m = c; - navdata_hdvideo_stream_msg.tag = m; - } + navdata_hdvideo_stream_msg.tag = m; + } - { + { uint16_t c = n.navdata_hdvideo_stream.size; uint16_t m; m = c; - navdata_hdvideo_stream_msg.size = m; - } + navdata_hdvideo_stream_msg.size = m; + } - { + { uint32_t c = n.navdata_hdvideo_stream.hdvideo_state; uint32_t m; m = c; - navdata_hdvideo_stream_msg.hdvideo_state = m; - } + navdata_hdvideo_stream_msg.hdvideo_state = m; + } - { + { uint32_t c = n.navdata_hdvideo_stream.storage_fifo_nb_packets; uint32_t m; m = c; - navdata_hdvideo_stream_msg.storage_fifo_nb_packets = m; - } + navdata_hdvideo_stream_msg.storage_fifo_nb_packets = m; + } - { + { uint32_t c = n.navdata_hdvideo_stream.storage_fifo_size; uint32_t m; m = c; - navdata_hdvideo_stream_msg.storage_fifo_size = m; - } + navdata_hdvideo_stream_msg.storage_fifo_size = m; + } - { + { uint32_t c = n.navdata_hdvideo_stream.usbkey_size; uint32_t m; m = c; - navdata_hdvideo_stream_msg.usbkey_size = m; - } + navdata_hdvideo_stream_msg.usbkey_size = m; + } - { + { uint32_t c = n.navdata_hdvideo_stream.usbkey_freespace; uint32_t m; m = c; - navdata_hdvideo_stream_msg.usbkey_freespace = m; - } + navdata_hdvideo_stream_msg.usbkey_freespace = m; + } - { + { uint32_t c = n.navdata_hdvideo_stream.frame_number; uint32_t m; m = c; - navdata_hdvideo_stream_msg.frame_number = m; - } + navdata_hdvideo_stream_msg.frame_number = m; + } - { + { uint32_t c = n.navdata_hdvideo_stream.usbkey_remaining_time; uint32_t m; m = c; - navdata_hdvideo_stream_msg.usbkey_remaining_time = m; + navdata_hdvideo_stream_msg.usbkey_remaining_time = m; + } + + pub_navdata_hdvideo_stream.publish(navdata_hdvideo_stream_msg); } - pub_navdata_hdvideo_stream.publish(navdata_hdvideo_stream_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_wifi && pub_navdata_wifi.getNumSubscribers()>0) + { + navdata_wifi_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_wifi_msg.header.stamp = now; + navdata_wifi_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_wifi && pub_navdata_wifi.getNumSubscribers()>0) - { - navdata_wifi_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_wifi_msg.header.stamp = now; - navdata_wifi_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_wifi.tag; uint16_t m; m = c; - navdata_wifi_msg.tag = m; - } + navdata_wifi_msg.tag = m; + } - { + { uint16_t c = n.navdata_wifi.size; uint16_t m; m = c; - navdata_wifi_msg.size = m; - } + navdata_wifi_msg.size = m; + } - { + { uint32_t c = n.navdata_wifi.link_quality; uint32_t m; m = c; - navdata_wifi_msg.link_quality = m; + navdata_wifi_msg.link_quality = m; + } + + pub_navdata_wifi.publish(navdata_wifi_msg); } - pub_navdata_wifi.publish(navdata_wifi_msg); - } + //------------------------- - //------------------------- + if(enabled_navdata_zimmu_3000 && pub_navdata_zimmu_3000.getNumSubscribers()>0) + { + navdata_zimmu_3000_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + navdata_zimmu_3000_msg.header.stamp = now; + navdata_zimmu_3000_msg.header.frame_id = droneFrameBase; - if(enabled_navdata_zimmu_3000 && pub_navdata_zimmu_3000.getNumSubscribers()>0) - { - navdata_zimmu_3000_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; - navdata_zimmu_3000_msg.header.stamp = now; - navdata_zimmu_3000_msg.header.frame_id = droneFrameBase; - - { + { uint16_t c = n.navdata_zimmu_3000.tag; uint16_t m; m = c; - navdata_zimmu_3000_msg.tag = m; - } + navdata_zimmu_3000_msg.tag = m; + } - { + { uint16_t c = n.navdata_zimmu_3000.size; uint16_t m; m = c; - navdata_zimmu_3000_msg.size = m; - } + navdata_zimmu_3000_msg.size = m; + } - { + { int32_t c = n.navdata_zimmu_3000.vzimmuLSB; int32_t m; m = c; - navdata_zimmu_3000_msg.vzimmuLSB = m; - } + navdata_zimmu_3000_msg.vzimmuLSB = m; + } - { + { float32_t c = n.navdata_zimmu_3000.vzfind; float32_t m; m = c; - navdata_zimmu_3000_msg.vzfind = m; + navdata_zimmu_3000_msg.vzfind = m; + } + + pub_navdata_zimmu_3000.publish(navdata_zimmu_3000_msg); } - pub_navdata_zimmu_3000.publish(navdata_zimmu_3000_msg); + //------------------------- + } - - //------------------------- - - } #endif diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 37bcc71..2f23dc8 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -137,7 +137,7 @@ void ARDroneDriver::run() ardrone_control_config.num_version_soft, shared_raw_navdata.navdata_demo.vbat_flying_percentage); ROS_INFO("Navdata Publish Settings:"); - //ROS_INFO(" Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off"); //Bug: This is being inited after in the NavdataMessage*.h + ROS_INFO(" Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off"); //Bug: This is being inited after in the NavdataMessage*.h ROS_INFO(" ROS Loop Rate: %d", looprate); ROS_INFO(" Instant New Navdata Publish: %s", fullspeed_navdata ? "On" : "Off"); // TODO: Enabled Navdata Demo @@ -225,6 +225,11 @@ void ARDroneDriver::configureDrone() #undef ARDRONE_CONFIG_KEY_IMM_a10 #undef ARDRONE_CONFIG_KEY_REF_a10 #undef ARDRONE_CONFIG_KEY_STR_a10 + + + #define NAVDATA_STRUCTS_INITIALIZE + #include "NavdataMessageDefinitions.h" + #undef NAVDATA_STRUCTS_INITIALIZE } void ARDroneDriver::resetCaliberation() From 140f55e32322ebde8e401e534af16e06bd81b144 Mon Sep 17 00:00:00 2001 From: Mike Hamer Date: Tue, 27 Nov 2012 14:18:45 +0100 Subject: [PATCH 3/3] Added navdata_demo enabled/disabled ros_info output --- src/ardrone_driver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 2f23dc8..1dfbb10 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -140,6 +140,7 @@ void ARDroneDriver::run() ROS_INFO(" Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off"); //Bug: This is being inited after in the NavdataMessage*.h ROS_INFO(" ROS Loop Rate: %d", looprate); ROS_INFO(" Instant New Navdata Publish: %s", fullspeed_navdata ? "On" : "Off"); + ROS_INFO(" Drone Send Speed: %s", ardrone_application_default_config.navdata_demo==0 ? "200Hz (navdata_demo=0)" : "15Hz (navdata_demo=1)"); // TODO: Enabled Navdata Demo vp_os_mutex_unlock(&navdata_lock); if (ardrone_control_config.num_version_soft[0] == '0')