Merge pull request #145 from Pitt-RAS/yaw-hold

Hack the heading to straight ahead

Author: Riteshlogin

cfg/LowLevelMotion.cfg

@@ -30,4 +30,6 @@ gen.add('roll_accumulator_max',  double_t, 0, '', 0.0, 0.0, 100.0)
 gen.add('roll_accumulator_min',  double_t, 0, '', 0.0, 0.0, 100.0)
 gen.add('roll_accumulator_enable_threshold',  double_t, 0, '', 0.0, 0.0, 100.0)
 
+gen.add('yaw_p',  double_t, 0, '', 0.0, 0.0, 100.0)
+
 exit(gen.generate(PACKAGE, "iarc7_motion", "LowLevelMotion"))

include/iarc7_motion/LandPlanner.hpp

@@ -68,6 +68,8 @@ class LandPlanner
 
     LandState state_;
 
+    double requested_x_;
+    double requested_y_;
     double requested_height_;
     double cushion_height_;
 

include/iarc7_motion/QuadVelocityController.hpp

@@ -51,6 +51,7 @@ class QuadVelocityController
                            double pitch_pid_settings[6],
                            double roll_pid_settings[6],
                            double (&position_p)[3],
+                           double yaw_p,
                            const ThrustModel& thrust_model,
                            const ThrustModel& thrust_model_side,
                            const ros::Duration& battery_timeout,
@@ -121,6 +122,9 @@ class QuadVelocityController
     // P terms for the position control
     double (&position_p_)[3];
 
+    // P term for yaw hold
+    double yaw_p_;
+
     // Last time an update was successful
     ros::Time last_update_time_;
 

param/low_level_motion_2.0.yaml

@@ -1,6 +1,6 @@
 # The PID constants are applied to positional errors in the map frame
-position_p_x: 0.0
-position_p_y: 0.0
+position_p_x: 1.0
+position_p_y: 1.0
 position_p_z: 1.5
 
 # PID values are positive for forward acting PID loops and negative for
@@ -36,6 +36,8 @@ roll_accumulator_max: 5.0
 roll_accumulator_min: -5.0
 roll_accumulator_enable_threshold: 10.0
 
+yaw_p: 0.4
+
 min_side_thrust: 0.1
 max_side_thrust: 10.0
 
@@ -45,11 +47,11 @@ throttle_max_rate : 200.0
 
 pitch_max         : 0.175
 pitch_min         : -0.175
-pitch_max_rate    : 1.0
+pitch_max_rate    : 2.0
 
 roll_max          : 0.175
 roll_min          : -0.175
-roll_max_rate     : 1.0
+roll_max_rate     : 2.0
 
 yaw_max           : 0.5
 yaw_min           : -0.5

param/low_level_motion_sim.yaml

@@ -1,6 +1,6 @@
 # The PID constants are applied to positional errors in the map frame
-position_p_x: 0.0
-position_p_y: 0.0
+position_p_x: 1.0
+position_p_y: 1.0
 position_p_z: 5.0
 
 # PID values are positive for forward acting PID loops and negative for
@@ -36,6 +36,8 @@ roll_accumulator_max: 5.0
 roll_accumulator_min: -5.0
 roll_accumulator_enable_threshold: 10.0
 
+yaw_p: 0.4
+
 min_side_thrust: 0.0
 max_side_thrust: 0.0
 

src/LandPlanner.cpp

@@ -23,6 +23,8 @@ LandPlanner::LandPlanner(
       landing_detected_message_received_(false),
       transform_wrapper_(),
       state_(LandState::DONE),
+      requested_x_(0.0),
+      requested_y_(0.0),
       requested_height_(0.0),
       cushion_height_(0.0),
       actual_descend_rate_(0.0),
@@ -44,7 +46,7 @@ LandPlanner::LandPlanner(
               "startup_timeout")),
       update_timeout_(ros_utils::ParamUtils::getParam<double>(
               private_nh,
-              "update_timeout")),                    
+              "update_timeout")),
       uav_arm_client_(nh.serviceClient<iarc7_msgs::Arm>("uav_arm"))
 {
     landing_detected_subscriber_ = nh.subscribe("landing_detected",
@@ -83,10 +85,12 @@ bool LandPlanner::prepareForTakeover(const ros::Time& time)
         return false;
     }
 
-    requested_height_ = transform.transform.translation.z; 
+    requested_x_ = transform.transform.translation.x;
+    requested_y_ = transform.transform.translation.y;
+    requested_height_ = transform.transform.translation.z;
 
     // height determined by the ratio of landing accelerations
-    cushion_height_ = std::min(0.5 * (std::pow(descend_rate_,2)/cushion_acceleration_), 
+    cushion_height_ = std::min(0.5 * (std::pow(descend_rate_,2)/cushion_acceleration_),
                                requested_height_ * ( 1 - ( 1 / ( 1 - descend_acceleration_/cushion_acceleration_))));
     actual_descend_rate_ = 0.0;
 
@@ -133,7 +137,7 @@ bool LandPlanner::getTargetMotionPoint(const ros::Time& time,
                                             + (cushion_acceleration_
                                             * (time - last_update_time_).toSec()));
         }
-        
+
         requested_height_ = std::max(0.0, requested_height_
                                           + (actual_descend_rate_
                                           * (time - last_update_time_).toSec()));
@@ -169,6 +173,8 @@ bool LandPlanner::getTargetMotionPoint(const ros::Time& time,
     // Fill in the uav_command's information
     motion_point.header.stamp = time;
 
+    motion_point.motion_point.pose.position.x = requested_x_;
+    motion_point.motion_point.pose.position.y = requested_y_;
     motion_point.motion_point.pose.position.z = requested_height_;
     motion_point.motion_point.twist.linear.z = actual_descend_rate_;
 

src/LowLevelMotionController.cpp

@@ -94,6 +94,7 @@ int main(int argc, char **argv)
     double pitch_pid[pid_param_array_size];
     double roll_pid[pid_param_array_size];
     double position_p[3];
+    double yaw_p;
 
     ThrustModel thrust_model(private_nh, "thrust_model");
     ThrustModel thrust_model_side;
@@ -140,6 +141,8 @@ int main(int argc, char **argv)
             position_p[1] = config.position_p_y;
             position_p[2] = config.position_p_z;
 
+            yaw_p = config.yaw_p;
+
             dynamic_reconfigure_called = true;
         };
     dynamic_reconfigure_server.setCallback(dynamic_reconfigure_settings_callback);
@@ -193,6 +196,7 @@ int main(int argc, char **argv)
                                           pitch_pid,
                                           roll_pid,
                                           position_p,
+                                          yaw_p,
                                           thrust_model,
                                           thrust_model_side,
                                           ros::Duration(battery_timeout),

src/QuadVelocityController.cpp

@@ -35,6 +35,7 @@ QuadVelocityController::QuadVelocityController(
         double vx_pid_settings[6],
         double vy_pid_settings[6],
         double (&position_p)[3],
+        double yaw_p,
         const ThrustModel& thrust_model,
         const ThrustModel& thrust_model_side,
         const ros::Duration& battery_timeout,
@@ -60,6 +61,7 @@ QuadVelocityController::QuadVelocityController(
               private_nh,
               "xy_mixer")),
       position_p_(position_p),
+      yaw_p_(yaw_p),
       startup_timeout_(ros_utils::ParamUtils::getParam<double>(
               private_nh,
               "startup_timeout")),
@@ -346,9 +348,8 @@ bool QuadVelocityController::update(const ros::Time& time,
             col_height)
             / voltage;
 
-    // Yaw rate needs no correction because the input and output are both
-    // velocities
-    uav_command.data.yaw = -setpoint_.motion_point.twist.angular.z;
+    // Hack heading to straight ahead
+    uav_command.data.yaw = -yaw_p_ * (0.0 - current_yaw);
 
     // Check that the PID loops did not return invalid values before returning
     if (!std::isfinite(uav_command.throttle)