Hack the heading to 0

Author: aftersomemath

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/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

@@ -36,6 +36,8 @@ roll_accumulator_max: 5.0
 roll_accumulator_min: -5.0
 roll_accumulator_enable_threshold: 10.0
 
+yaw_p: 0.0
+
 min_side_thrust: 0.1
 max_side_thrust: 10.0
 

param/low_level_motion_sim.yaml

@@ -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/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)