Merge branch 'more-navdata'

launch/ardrone.launch

@@ -18,6 +18,36 @@
 	    <param name="enemy_without_shell" value="0" />
 	    <param name="do_imu_caliberation" value="false" />
 	    <param name="tf_prefix" value="mydrone" />
+
+		<param name="enable_navdata_demo" value="true" />
+		<param name="enable_navdata_time" value="true" />
+		<param name="enable_navdata_raw_measures" value="true" />
+		<param name="enable_navdata_phys_measures" value="true" />
+		<param name="enable_navdata_gyros_offsets" value="true" />
+		<param name="enable_navdata_euler_angles" value="true" />
+		<param name="enable_navdata_references" value="true" />
+		<param name="enable_navdata_trims" value="true" />
+		<param name="enable_navdata_rc_references" value="true" />
+		<param name="enable_navdata_pwm" value="true" />
+		<param name="enable_navdata_altitude" value="true" />
+		<param name="enable_navdata_vision_raw" value="true" />
+		<param name="enable_navdata_vision_of" value="true" />
+		<param name="enable_navdata_vision" value="true" />
+		<param name="enable_navdata_vision_perf" value="true" />
+		<param name="enable_navdata_trackers_send" value="true" />
+		<param name="enable_navdata_vision_detect" value="true" />
+		<param name="enable_navdata_watchdog" value="true" />
+		<param name="enable_navdata_adc_data_frame" value="true" />
+		<param name="enable_navdata_video_stream" value="true" />
+		<param name="enable_navdata_games" value="true" />
+		<param name="enable_navdata_pressure_raw" value="true" />
+		<param name="enable_navdata_magneto" value="true" />
+		<param name="enable_navdata_wind_speed" value="true" />
+		<param name="enable_navdata_kalman_pressure" value="true" />
+		<param name="enable_navdata_hdvideo_stream" value="true" />
+		<param name="enable_navdata_wifi" value="true" />
+		<param name="enable_navdata_zimmu_3000" value="true" />
+
         <!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
         <rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
         <rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>

msg/matrix33.msg

@@ -0,0 +1,9 @@
+float32 m11
+float32 m12
+float32 m13
+float32 m21
+float32 m22
+float32 m23
+float32 m31
+float32 m32
+float32 m33

msg/navdata_adc_data_frame.msg

@@ -0,0 +1,6 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 version
+uint8[] data_frame

msg/navdata_altitude.msg

@@ -0,0 +1,14 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int32 altitude_vision
+float32 altitude_vz
+int32 altitude_ref
+int32 altitude_raw
+float32 obs_accZ
+float32 obs_alt
+vector31 obs_x
+uint32 obs_state
+vector21 est_vb
+uint32 est_state

msg/navdata_demo.msg

@@ -0,0 +1,15 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 ctrl_state
+uint32 vbat_flying_percentage
+float32 theta
+float32 phi
+float32 psi
+int32 altitude
+float32 vx
+float32 vy
+float32 vz
+uint32 num_frames
+uint32 detection_camera_type

msg/navdata_euler_angles.msg

@@ -0,0 +1,6 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32 theta_a
+float32 phi_a

msg/navdata_games.msg

@@ -0,0 +1,6 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 double_tap_counter
+uint32 finish_line_counter

msg/navdata_gyros_offsets.msg

@@ -0,0 +1,5 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32[] offset_g

msg/navdata_hdvideo_stream.msg

@@ -0,0 +1,11 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 hdvideo_state
+uint32 storage_fifo_nb_packets
+uint32 storage_fifo_size
+uint32 usbkey_size
+uint32 usbkey_freespace
+uint32 frame_number
+uint32 usbkey_remaining_time

msg/navdata_kalman_pressure.msg

@@ -0,0 +1,21 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32 offset_pressure
+float32 est_z
+float32 est_zdot
+float32 est_bias_PWM
+float32 est_biais_pression
+float32 offset_US
+float32 prediction_US
+float32 cov_alt
+float32 cov_PWM
+float32 cov_vitesse
+int32 bool_effet_sol
+float32 somme_inno
+int32 flag_rejet_US
+float32 u_multisinus
+float32 gaz_altitude
+int32 Flag_multisinus
+int32 Flag_multisinus_debut

msg/navdata_magneto.msg

@@ -0,0 +1,18 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int16 mx
+int16 my
+int16 mz
+vector31 magneto_raw
+vector31 magneto_rectified
+vector31 magneto_offset
+float32 heading_unwrapped
+float32 heading_gyro_unwrapped
+float32 heading_fusion_unwrapped
+uint8 magneto_calibration_ok
+uint32 magneto_state
+float32 magneto_radius
+float32 error_mean
+float32 error_var

msg/navdata_phys_measures.msg

@@ -0,0 +1,11 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32 accs_temp
+uint16 gyro_temp
+float32[] phys_accs
+float32[] phys_gyros
+uint32 alim3V3
+uint32 vrefEpson
+uint32 vrefIDG

msg/navdata_pressure_raw.msg

@@ -0,0 +1,8 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int32 up
+int16 ut
+int32 Temperature_meas
+int32 Pression_meas

msg/navdata_pwm.msg

@@ -0,0 +1,29 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint8 motor1
+uint8 motor2
+uint8 motor3
+uint8 motor4
+uint8 sat_motor1
+uint8 sat_motor2
+uint8 sat_motor3
+uint8 sat_motor4
+float32 gaz_feed_forward
+float32 gaz_altitude
+float32 altitude_integral
+float32 vz_ref
+int32 u_pitch
+int32 u_roll
+int32 u_yaw
+float32 yaw_u_I
+int32 u_pitch_planif
+int32 u_roll_planif
+int32 u_yaw_planif
+float32 u_gaz_planif
+uint16 current_motor1
+uint16 current_motor2
+uint16 current_motor3
+uint16 current_motor4
+float32 altitude_der

msg/navdata_raw_measures.msg

@@ -0,0 +1,19 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int16[] raw_gyros
+int16[] raw_gyros_110
+uint32 vbat_raw
+uint16 us_debut_echo
+uint16 us_fin_echo
+uint16 us_association_echo
+uint16 us_distance_echo
+uint16 us_courbe_temps
+uint16 us_courbe_valeur
+uint16 us_courbe_ref
+uint16 flag_echo_ini
+uint16 nb_echo
+uint32 sum_echo
+int32 alt_temp_raw
+int16 gradient

msg/navdata_rc_references.msg

@@ -0,0 +1,9 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int32 rc_ref_pitch
+int32 rc_ref_roll
+int32 rc_ref_yaw
+int32 rc_ref_gaz
+int32 rc_ref_ag

msg/navdata_references.msg

@@ -0,0 +1,25 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int32 ref_theta
+int32 ref_phi
+int32 ref_theta_I
+int32 ref_phi_I
+int32 ref_pitch
+int32 ref_roll
+int32 ref_yaw
+int32 ref_psi
+float32 vx_ref
+float32 vy_ref
+float32 theta_mod
+float32 phi_mod
+float32 k_v_x
+float32 k_v_y
+uint32 k_mode
+float32 ui_time
+float32 ui_theta
+float32 ui_phi
+float32 ui_psi
+float32 ui_psi_accuracy
+int32 ui_seq

msg/navdata_time.msg

@@ -0,0 +1,5 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 time

msg/navdata_trackers_send.msg

@@ -0,0 +1,6 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int32[] locked
+vector21[] point

msg/navdata_trims.msg

@@ -0,0 +1,7 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32 angular_rates_trim_r
+float32 euler_angles_trim_theta
+float32 euler_angles_trim_phi

msg/navdata_video_stream.msg

@@ -0,0 +1,17 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint8 quant
+uint32 frame_size
+uint32 frame_number
+uint32 atcmd_ref_seq
+uint32 atcmd_mean_ref_gap
+float32 atcmd_var_ref_gap
+uint32 atcmd_ref_quality
+uint32 desired_bitrate
+int32 data2
+int32 data3
+int32 data4
+int32 data5
+uint32 fifo_queue_level

msg/navdata_vision.msg

@@ -0,0 +1,24 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 vision_state
+int32 vision_misc
+float32 vision_phi_trim
+float32 vision_phi_ref_prop
+float32 vision_theta_trim
+float32 vision_theta_ref_prop
+int32 new_raw_picture
+float32 theta_capture
+float32 phi_capture
+float32 psi_capture
+int32 altitude_capture
+uint32 time_capture
+vector31 body_v
+float32 delta_phi
+float32 delta_theta
+float32 delta_psi
+uint32 gold_defined
+uint32 gold_reset
+float32 gold_x
+float32 gold_y

msg/navdata_vision_detect.msg

@@ -0,0 +1,15 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 nb_detected
+uint32[] type
+uint32[] xc
+uint32[] yc
+uint32[] width
+uint32[] height
+uint32[] dist
+float32[] orientation_angle
+matrix33[] rotation
+vector31[] translation
+uint32[] camera_source

msg/navdata_vision_of.msg

@@ -0,0 +1,6 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32[] of_dx
+float32[] of_dy

msg/navdata_vision_perf.msg

@@ -0,0 +1,10 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32 time_corners
+float32 time_compute
+float32 time_tracking
+float32 time_trans
+float32 time_update
+float32[] time_custom

msg/navdata_vision_raw.msg

@@ -0,0 +1,7 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32 vision_tx_raw
+float32 vision_ty_raw
+float32 vision_tz_raw

msg/navdata_watchdog.msg

@@ -0,0 +1,4 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size

msg/navdata_wifi.msg

@@ -0,0 +1,5 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+uint32 link_quality

msg/navdata_wind_speed.msg

@@ -0,0 +1,17 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+float32 wind_speed
+float32 wind_angle
+float32 wind_compensation_theta
+float32 wind_compensation_phi
+float32 state_x1
+float32 state_x2
+float32 state_x3
+float32 state_x4
+float32 state_x5
+float32 state_x6
+float32 magneto_debug1
+float32 magneto_debug2
+float32 magneto_debug3

msg/navdata_zimmu_3000.msg

@@ -0,0 +1,6 @@
+Header header
+float64 drone_time
+uint16 tag
+uint16 size
+int32 vzimmuLSB
+float32 vzfind

msg/vector21.msg

@@ -0,0 +1,2 @@
+float32 x
+float32 y

msg/vector31.msg

@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 z

scripts/CreateNavdataFormat.py

@@ -0,0 +1,112 @@
+import re
+from mako.template import Template
+
+PATH_NAVDATA_KEYS = '../ARDroneLib/Soft/Common/navdata_keys.h'
+PATH_NAVDATA_COMMON = '../ARDroneLib/Soft/Common/navdata_common.h'
+
+keys = []
+
+r_keys = re.compile(r'''
+			^\s*[^#]?\s*?
+			(NAVDATA_OPTION(?:_DEMO)?)
+			\s*\(\s*
+			((?P<structure>\w+))
+			\s*,\s*
+			((?P<name>\w+))
+			\s*,\s*
+			((?P<tag>\w+))
+			\s*\)\s*$
+			''', re.X)
+
+print 'Parsing Navdata Struct Names'
+with open(PATH_NAVDATA_KEYS,'r') as navdata_keys:
+	for line in navdata_keys:
+		matches = re.search(r_keys,line)
+		if matches:
+			print '-- '+matches.group('name')
+			keys.append(matches.group('structure','name','tag'))
+
+contents = ''
+with open(PATH_NAVDATA_COMMON,'r') as navdata_common:
+	contents = navdata_common.read()
+
+ros_datatypes = {'uint32_t'		:('std_msgs::UInt32','uint32_t'),
+				 'uint16_t'		:('std_msgs::UInt16','uint16_t'),
+				 'uint8_t' 		:('std_msgs::UInt8','uint8_t'),
+				 'int32_t'		:('std_msgs::Int32','int32_t'),
+				 'int16_t'		:('std_msgs::Int16','int16_t'),
+				 'int8_t' 		:('std_msgs::Int8','int8_t'),
+				 'float64_t'	:('std_msgs::Float64','float64_t'),
+				 'float32_t'	:('std_msgs::Float32','float32_t'),
+				 'matrix33_t'	:('ardrone_autonomy::matrix33','ardrone_autonomy::matrix33'),
+				 'vector31_t'	:('ardrone_autonomy::vector31','ardrone_autonomy::vector31'),
+				 'vector21_t'	:('ardrone_autonomy::vector21','ardrone_autonomy::vector21'),
+				 'screen_point_t'	:('ardrone_autonomy::vector21','ardrone_autonomy::vector21'),
+				 'velocities_t'	:('ardrone_autonomy::vector31','ardrone_autonomy::vector31'),
+				 'char'			:('std_msgs::UInt8','char'),
+				 'bool_t'		:('std_msgs::Int32','bool_t')}
+
+
+
+
+structs = []
+print 'Parsing Navdata Struct Contents'
+for (struct,name,_) in keys:
+	members = []
+	rg = re.compile(r'''
+		.*?
+		typedef\s*struct\s*_?'''+struct+r'''\s*\{
+		(?P<inside>.*)
+		\}\s*\w*?\s*'''+struct+r'''\s*;
+		.*?
+		''',re.X|re.DOTALL|re.MULTILINE)
+	structcontents = re.search(rg,contents)
+	if structcontents:
+		print '-- '+name
+		allmembers = re.findall(r'(.*?)\s*(\w+(?:\s*?\*)?)\s*(\w+)\s*(?:\[([\w\s*/+-]+)\])?\s*;(.*)',structcontents.groups('inside')[0])
+		members = [(t,n,s) for (b,t,n,s,c) in allmembers if 'Deprecated' not in c and '//' not in b]
+	structs.append((struct,name,members))
+
+
+
+
+
+print 'Saving Custom ROS Message Definitions'
+for (struct,name,members) in structs:
+	with open('../msg/'+name+'.msg','w') as f:
+		f.write('Header header\n')
+		f.write('float64 drone_time\n')
+		for (t,n,s) in members:
+			dt = ros_datatypes[t][0]
+			dt = dt.rpartition('::')[2].lower()
+			if s!='':
+				f.write('{0}[] {1}\n'.format(dt,n))
+			else:
+				f.write('{0} {1}\n'.format(dt,n))
+
+
+
+
+print 'Generating C Source'
+
+items = []
+for (struct_type, struct_name, members) in structs:
+	item = {}
+	item['struct_name'] = struct_name
+	item['members'] = []
+	for (c_type,name,size) in members:
+		member = {}
+		member['name'] = name
+		member['c_type'] = c_type
+		member['include'] = ros_datatypes[c_type][0]
+		member['ros_type'] = ros_datatypes[c_type][1]
+		member['array_size'] = size if size!='' else None
+		item['members'].append(member)
+	items.append(item)
+
+template = Template(filename='NavdataMessageDefinitionsTemplate.c');
+
+with open('../src/NavdataMessageDefinitions.h','w') as f:
+	f.write(template.render(structs=items))
+
+print 'You should now run `rosmake ardrone_autonomy` to build the custom messages.'

scripts/NavdataMessageDefinitionsTemplate.c

@@ -0,0 +1,121 @@
+// Autogenerated from source-files using the CreateNavdataFormat.py script
+
+#ifdef NAVDATA_STRUCTS_INCLUDES
+% for item in structs:
+#include "ardrone_autonomy/${item['struct_name']}.h"
+% endfor
+<% included = [] %>\
+% for item in structs:
+% for member in item['members']:
+% if member['include'] not in included:
+<% included.append(member['include']) %>\
+#include <${'/'.join(member['include'].split('::'))}.h>
+% endif
+% endfor
+% endfor	
+#endif
+
+#ifdef NAVDATA_STRUCTS_HEADER
+% for item in structs:
+	ros::Publisher pub_${item['struct_name']};
+	bool enabled_${item['struct_name']};
+% endfor
+
+	bool enabled_legacy_navdata;
+
+	bool initialized_navdata_publishers;
+	void PublishNavdataTypes(navdata_unpacked_t n);
+#endif
+
+#ifdef NAVDATA_STRUCTS_SOURCE
+void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
+{
+	if(!initialized_navdata_publishers)
+	{
+		initialized_navdata_publishers = true;
+
+		enabled_legacy_navdata = true;
+		ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
+
+		if(enabled_legacy_navdata)
+		{
+			navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
+			imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
+			mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
+		}
+
+		//-------------------------
+
+% for item in structs:
+		enabled_${item['struct_name']} = false;
+		ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
+
+		if(enabled_${item['struct_name']})
+		{
+			pub_${item['struct_name']} = node_handle.advertise<ardrone_autonomy::${item['struct_name']}>("ardrone/${item['struct_name']}", NAVDATA_QUEUE_SIZE);
+		}
+
+		//-------------------------
+
+% endfor
+	}
+
+% for item in structs:
+	if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
+	{
+		ardrone_autonomy::${item['struct_name']} msg;
+		msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
+
+% for member in item['members']:
+% if member['array_size'] is None:
+		{\
+			${format_member(item, member, None)}
+			msg.${member['name']} = m;
+		}
+
+% else:
+		for(int i=0; i<${member['array_size']}; i++)
+		{\
+			${format_member(item, member, 'i')}
+			msg.${member['name']}.push_back(m);
+		}
+		
+% endif
+% endfor
+		pub_${item['struct_name']}.publish(msg);
+	}
+
+	//-------------------------
+
+% endfor
+
+}
+#endif
+
+
+
+
+<%def name="format_member(item, member, i)">
+			${member['c_type']} c = n.${item['struct_name']}.${member['name']}${'[{}]'.format(i) if i is not None else ''};
+			${member['ros_type']} m;
+% if 'matrix33' in member['ros_type']:
+			m.m11 = c.m11;
+			m.m12 = c.m12;
+			m.m13 = c.m13;
+			m.m21 = c.m21;
+			m.m22 = c.m22;
+			m.m23 = c.m23;
+			m.m31 = c.m31;
+			m.m32 = c.m32;
+			m.m33 = c.m33;
+% elif 'vector21' in member['ros_type']:
+			m.x = c.x;
+			m.y = c.y;
+% elif 'vector31' in member['ros_type']:
+			m.x = c.x;
+			m.y = c.y;
+			m.z = c.z;
+% else:
+			m = c;
+% endif
+</%def>

src/ardrone_driver.cpp

@@ -20,9 +20,6 @@ ARDroneDriver::ARDroneDriver()
     image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
     hori_pub = image_transport.advertiseCamera("ardrone/front/image_raw", 10);
     vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
-    navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
-    imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
-    mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
     toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
     setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
     setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
@@ -562,6 +559,7 @@ void ARDroneDriver::publish_navdata()
 {
     // Thread safe copy of interesting Navdata data
     vp_os_mutex_lock(&navdata_lock);
+    navdata_raw = shared_raw_navdata;
     navdata_detect = shared_navdata_detect;
     navdata_phys = shared_navdata_phys;
     navdata = shared_navdata;
@@ -614,7 +612,10 @@ void ARDroneDriver::publish_navdata()
         navdata.vz -= vel_bias[2];
 
     }
-    if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
+
+    PublishNavdataTypes(navdata_raw); // This is defined in the template NavdataMessageDefinitions.h template file
+
+    if (!enabled_legacy_navdata || (navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
         return; // why bother, no one is listening.
     const ros::Time _now = ros::Time::now();
 
@@ -729,6 +730,10 @@ void ARDroneDriver::publish_navdata()
     imu_pub.publish(imu_msg);
 }
 
+#define NAVDATA_STRUCTS_SOURCE
+#include "NavdataMessageDefinitions.h"
+#undef NAVDATA_STRUCTS_SOURCE
+
 void ARDroneDriver::publish_tf()
 {
     tf_base_front.stamp_ = ros::Time::now();

src/ardrone_driver.h

@@ -16,12 +16,18 @@
 #include <ardrone_tool/ardrone_version.h>
 #include <ardrone_tool/ardrone_tool.h>
 
+#define NAVDATA_STRUCTS_INCLUDES
+#include "NavdataMessageDefinitions.h"
+#undef NAVDATA_STRUCTS_INCLUDES
+
 
 #define _DEG2RAD 0.01745331111
 #define _RAD2DEG 57.2957184819
 
 #define DRIVER_USERNAME "ardrone_driver"
 #define DRIVER_APPNAME "ardrone_driver"
+#define CAMERA_QUEUE_SIZE (10)
+#define NAVDATA_QUEUE_SIZE (25)
 
 enum ROOT_FRAME
 {
@@ -104,6 +110,11 @@ private:
     navdata_magneto_t navdata_magneto;
     navdata_wind_speed_t navdata_wind;
     navdata_time_t arnavtime;
+    navdata_unpacked_t navdata_raw;
+
+    #define NAVDATA_STRUCTS_HEADER
+    #include "NavdataMessageDefinitions.h"
+    #undef NAVDATA_STRUCTS_HEADER
 
     /*
      * TF Frames

src/ardrone_sdk.cpp

@@ -10,6 +10,10 @@ navdata_pressure_raw_t shared_navdata_pressure;
 navdata_magneto_t shared_navdata_magneto;
 navdata_wind_speed_t shared_navdata_wind;
 navdata_time_t shared_arnavtime;
+navdata_unpacked_t shared_raw_navdata;
+
+bool command_disable_hover;
+bool command_always_send;
 
 vp_os_mutex_t navdata_lock;
 vp_os_mutex_t video_lock;
@@ -62,6 +66,14 @@ extern "C" {
         }
 
 
+        command_disable_hover = false; //disables the drone from entering the hover state - constant dynamics rather than onboard state changes
+        command_always_send = false;   //constantly sends navdata messages to the drone, even if the messages haven't changed
+        ros::param::param("~command_disable_hover", command_disable_hover, false);
+        ros::param::param("~command_always_send"  , command_always_send,   false);
+        ROS_INFO("Hovering is %s",(command_disable_hover?"DISABLED!":"Enabled."));
+        ROS_INFO("Will %s send duplicate commands.",(command_always_send?"ALWAYS":"not"));
+
+
         // SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
         // THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)
         ardrone_application_default_config.bitrate_ctrl_mode = VBC_MODE_DISABLED;
@@ -92,7 +104,7 @@ extern "C" {
         #define LOAD_PARAM_NUM(NAME,C_TYPE,DEFAULT)                                                                             \
             {                                                                                                                   \
               double param;                                                                                                     \
-              ROS_DEBUG("CHECK: "#NAME);                                                                                        \
+              ROS_DEBUG("CHECK: "#NAME" (Default = "#DEFAULT" = %f)",(float)DEFAULT);                                           \
               if(ros::param::get("~"#NAME,param))                                                                               \
               {                                                                                                                 \
                 ardrone_application_default_config.NAME = (C_TYPE)param;                                                        \
@@ -103,7 +115,7 @@ extern "C" {
         #define LOAD_PARAM_STR(NAME,DEFAULT)                                                                                    \
             {                                                                                                                   \
               std::string param;                                                                                                \
-              ROS_DEBUG("CHECK: "#NAME);                                                                                        \
+              ROS_DEBUG("CHECK: "#NAME" (Default = "#DEFAULT" = %s)",DEFAULT);                                                  \
               if(ros::param::get("~"#NAME,param))                                                                               \
               {                                                                                                                 \
                 param = param.substr(0,STRING_T_SIZE-1);                                                                        \
@@ -225,6 +237,7 @@ extern "C" {
 
     C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
         vp_os_mutex_lock(&navdata_lock);
+        shared_raw_navdata = *pnd;
         shared_navdata_detect = pnd->navdata_vision_detect;
         shared_navdata_phys = pnd->navdata_phys_measures;
         shared_navdata = pnd->navdata_demo;

src/ardrone_sdk.h

@@ -52,6 +52,10 @@ extern navdata_time_t shared_arnavtime;
 extern navdata_pressure_raw_t shared_navdata_pressure;
 extern navdata_magneto_t shared_navdata_magneto;
 extern navdata_wind_speed_t shared_navdata_wind;
+extern navdata_unpacked_t shared_raw_navdata;
+
+extern bool command_disable_hover;
+extern bool command_always_send;
 
 extern vp_os_mutex_t navdata_lock;
 extern vp_os_mutex_t video_lock;

src/teleop_twist.cpp

@@ -154,6 +154,10 @@ C_RESULT update_teleop(void)
                 (fabs(up_down) < _EPS) && 
                 (fabs(turn) < _EPS)
                 );
+
+        if(command_disable_hover) hover = 1; //force the hover flag to 1 (0 == enter hover) if we want to disable the hover state
+        if(command_always_send)   is_changed = true; //force the packet to send if so desired
+
         control_flag |= (hover << 0);
         control_flag |= (combined_yaw << 1);
         //ROS_INFO (">>> Control Flag: %d", control_flag);