Added flattrim service (fixes #18). Updated documentation. Some minor

files/functions cleanup.

README.md

@@ -148,7 +148,10 @@ The `frame_id` field in header of all published topics (navdata, imu, cameras) w
 
 ### Toggle AR-Drone's Camera
 
-Calling `ardrone/togglecam` service with no parameters will change the active video camera stream. (e.g `rosservice call /ardrone/togglecam`)
+Calling `ardrone/togglecam` service with no parameters will change the active video camera stream. (e.g `rosservice call /ardrone/togglecam`).
+
+`ardrone/setcamchannel` service directly sets the current active camera channel. One parameter (`uint8 channel
+`) should be sent to this service. For AR-Drone 1.0 the valid values are [0..3] and for AR-Drone 2.0 the valid values are [0..1]. The order is similar to the order described in "Cameras" section.
 
 ### LED Animations
 
@@ -168,7 +171,11 @@ You can test these animations in command line using commands like `rosservice ca
 
 ### IMU Calibration
 
-If `do_imu_caliberation` parameter is set to true, calling this service would make the driver recalculate the biases in IMU data based on data from a short sampling period.
+If `do_imu_caliberation` parameter is set to true, calling `ardrone/imu_recalib` service will make the driver recalculate the biases in IMU data based on data from a short sampling period.
+
+### Flat Trim
+
+Calling `ardrone/flattrim` service without any parameter will send a "Flat Trim" request to AR-Drone to re-caliberate its rotation estimates assuming that it is on a flat surface. Do not call this service while Drone is flying or while the drone is not actually on a flat surface.
 
 ## Parameters
 
@@ -202,6 +209,8 @@ The Parrot's license, copyright and disclaimer for `ARDroneLib` are included wit
 ## Contributors
 
 - [Rachel Brindle](https://github.com/younata) - [Enhanced Navdata for AR-Drone 2.0](https://github.com/AutonomyLab/ardrone_autonomy/pull/2)
+- [Devmax](https://github.com/devmax) - [Flat Trim](https://github.com/AutonomyLab/ardrone_autonomy/issues/18) + Various
+comments for enhancements
 
 ## FAQ
 
@@ -290,5 +299,5 @@ After successful calibration, press the `commit` button in the UI. The driver wi
 * Make the `tf` publish optional.
 * Add separate topic for drone's debug stream (`navdata_demo`)
 * Add the currently selected camera name to `Navdata`
-* Make the `togglecam` service accept parameters
+* [DONE] Make the `togglecam` service accept parameters
 * [DONE] Enrich `Navdata` with magneto meter and baro meter information

src/ardrone_driver.cpp

@@ -1,7 +1,6 @@
 #include "ardrone_driver.h"
 #include "teleop_twist.h"
 #include "video.h"
-#include "ardrone_autonomy/LedAnim.h"
 #include <signal.h>
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -24,9 +23,9 @@ ARDroneDriver::ARDroneDriver()
     navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
     imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
 	toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
-	toggleNavdataDemo_service = node_handle.advertiseService("ardrone/togglenavdatademo", toggleNavdataDemoCallback);
-	setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
+    setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
 	setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
+    flatTrim_service = node_handle.advertiseService("ardrone/flattrim", flatTrimCallback);
 
     /*
         To be honest, I am not sure why advertising a service using class members should be this complicated!

src/ardrone_driver.h

@@ -61,10 +61,10 @@ private:
 
 	//ros::Subscriber toggleCam_sub;
 	ros::ServiceServer toggleCam_service;
-	ros::ServiceServer toggleNavdataDemo_service;
 	ros::ServiceServer setCamChannel_service;
 	ros::ServiceServer setLedAnimation_service;
     ros::ServiceServer imuReCalib_service;
+    ros::ServiceServer flatTrim_service;
 	
 	/*
 	 * Orange Green : 1

src/teleop_twist.cpp

@@ -28,19 +28,11 @@ const LED_ANIMATION_IDS ledAnimMap[14] = {
 	SNAKE_GREEN_RED, FIRE, STANDARD, RED, GREEN, RED_SNAKE,BLANK,
 	LEFT_GREEN_RIGHT_RED, LEFT_RED_RIGHT_GREEN, BLINK_STANDARD};
 
-bool toggleNavdataDemoCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
-{
-    set_navdata_demo_value ^= 1;
-    ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &set_navdata_demo_value, NULL);
-    fprintf(stderr, "\nToggling navdata_demo, set to %d.\n", set_navdata_demo_value);
-    return true;
-}
-
 //ros service callback to set the camera channel
-//TODO: add input check
 bool setCamChannelCallback(ardrone_autonomy::CamSelect::Request& request, ardrone_autonomy::CamSelect::Response& response)
 {
-    cam_state = request.channel;
+    const int _modes = (IS_ARDRONE1) ? 4 : 2;
+    cam_state = request.channel % _modes;
     ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
     fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
     response.result = true;
@@ -49,7 +41,7 @@ bool setCamChannelCallback(ardrone_autonomy::CamSelect::Request& request, ardron
 // ros service callback function for toggling Cam
 bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
 {
-    int _modes = (IS_ARDRONE1) ? 4 : 2;
+    const int _modes = (IS_ARDRONE1) ? 4 : 2;
     cam_state = (cam_state + 1) % _modes;
     ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
     fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
@@ -66,24 +58,13 @@ bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardron
     return true;
 }
 
-/*
-// Older rostopic callback function for toggling Cam
-void toggleCamCallback(const std_msgs::Empty &msg)
+bool flatTrimCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
 {
-  if (cam_state == 0) // toggle to 1, the vertical camera
-    {
-      cam_state = 1;
-      ardrone_at_set_toy_configuration("video:video_channel","1");
-      fprintf(stderr, "\nToggling from frontal camera to vertical camera.\n");
-    }
-  else if (cam_state == 1) // toggle to the forward camera
-    {
-      cam_state = 0;
-      ardrone_at_set_toy_configuration("video:video_channel","0");
-      fprintf(stderr, "\nToggling from vertical camera to frontal camera.\n");      
-    }
+    vp_os_mutex_lock(&twist_lock);
+    ardrone_at_set_flat_trim();
+    vp_os_mutex_unlock(&twist_lock);
+    fprintf(stderr, "\nFlat Trim Set.\n");
 }
-*/
 
 void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg)
 {

src/teleop_twist.h

@@ -20,8 +20,8 @@ void takeoffCallback(const std_msgs::Empty &msg);
 //void toggleCamCallback(const std_msgs::Empty &msg);
 bool setCamChannelCallback(ardrone_autonomy::CamSelect::Request& request, ardrone_autonomy::CamSelect::Response& response);
 bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
-bool toggleNavdataDemoCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response);
+bool flatTrimCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 
 //All global drone configs that should be sent on init
 

srv/HullSelect.srv

@@ -1,3 +0,0 @@
-uint8 type
----
-bool result