Merge pull request #38 from mikehamer/master

navdata_packet.header now filled, also using cmd_vel.angular.x/y != 0 for hover

launch/ardrone_aggressive.launch

@@ -24,9 +24,6 @@
 	    <param name="do_imu_caliberation" value="false" />
 	    <param name="tf_prefix" value="mydrone" />
 
-	    <param name="command_disable_hover" value="true" />
-	    <param name="command_always_send" value="true" />
-
 	    <param name="enable_navdata_demo" value="true" />
 		<param name="enable_navdata_time" value="true" />
 		<param name="enable_navdata_raw_measures" value="true" />

scripts/NavdataMessageDefinitionsTemplate.c

@@ -19,6 +19,7 @@
 % for item in structs:
 	ros::Publisher pub_${item['struct_name']};
 	bool enabled_${item['struct_name']};
+	ardrone_autonomy::${item['struct_name']} ${item['struct_name']}_msg;
 % endfor
 
 	bool enabled_legacy_navdata;
@@ -30,6 +31,8 @@
 #ifdef NAVDATA_STRUCTS_SOURCE
 void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 {
+	const ros::Time now = ros::Time::now();
+
 	if(!initialized_navdata_publishers)
 	{
 		initialized_navdata_publishers = true;
@@ -63,26 +66,27 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 % 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;
+		${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)}
-			msg.${member['name']} = m;
+			${item['struct_name']}_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);
+			${item['struct_name']}_msg.${member['name']}.push_back(m);
 		}
 		
 % endif
 % endfor
-		pub_${item['struct_name']}.publish(msg);
+		pub_${item['struct_name']}.publish(${item['struct_name']}_msg);
 	}
 
 	//-------------------------

src/ardrone_sdk.cpp

@@ -12,9 +12,6 @@ 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;
 vp_os_mutex_t twist_lock;
@@ -66,14 +63,6 @@ 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;

src/ardrone_sdk.h

@@ -54,9 +54,6 @@ 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;
 extern vp_os_mutex_t twist_lock;

src/teleop_twist.cpp

@@ -155,8 +155,7 @@ C_RESULT update_teleop(void)
                 (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
+        if(cmd_vel.angular.x > _EPS || cmd_vel.angular.y > _EPS) hover=1; //set angular.x or angular.y to a non-zero value to disable entering hover
 
         control_flag |= (hover << 0);
         control_flag |= (combined_yaw << 1);