[Spinal] Add an option for body rate cmd

aerial_robot_nerve/spinal/CMakeLists.txt

@@ -52,6 +52,7 @@ add_service_files(
   SetAttitudeGains.srv
   ImuCalib.srv
   MagDeclination.srv
+  SetControlMode.srv
 )
 
 generate_messages(

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp

@@ -31,6 +31,8 @@ void AttitudeController::init(ros::NodeHandle* nh, StateEstimate* estimator)
   att_control_srv_ = nh_->advertiseService("set_attitude_control", &AttitudeController::setAttitudeControlCallback, this);
   torque_allocation_matrix_inv_sub_ = nh_->subscribe("torque_allocation_matrix_inv", 1, &AttitudeController::torqueAllocationMatrixInvCallback, this);
   sim_vol_sub_ = nh_->subscribe("set_sim_voltage", 1, &AttitudeController::setSimVolCallback, this);
+  control_mode_srv_ = nh_->advertiseService("set_control_mode", &AttitudeController::setControlModeCallback, this);
+
   baseInit();
 }
 
@@ -46,7 +48,8 @@ AttitudeController::AttitudeController():
   p_matrix_pseudo_inverse_inertia_sub_("p_matrix_pseudo_inverse_inertia", &AttitudeController::pMatrixInertiaCallback, this),
   pwm_test_sub_("pwm_test", &AttitudeController::pwmTestCallback, this ),
   att_control_srv_("set_attitude_control", &AttitudeController::setAttitudeControlCallback, this),
-  torque_allocation_matrix_inv_sub_("torque_allocation_matrix_inv", &AttitudeController::torqueAllocationMatrixInvCallback, this)
+  torque_allocation_matrix_inv_sub_("torque_allocation_matrix_inv", &AttitudeController::torqueAllocationMatrixInvCallback, this),
+  control_mode_srv_("set_control_mode", &AttitudeController::setControlModeCallback, this)
 {
 }
 
@@ -82,6 +85,7 @@ void AttitudeController::init(TIM_HandleTypeDef* htim1, TIM_HandleTypeDef* htim2
   nh_->subscribe(torque_allocation_matrix_inv_sub_);
 
   nh_->advertiseService(att_control_srv_);
+  nh_->advertiseService(control_mode_srv_);
 
   baseInit();
 }
@@ -116,6 +120,10 @@ void AttitudeController::baseInit()
   control_term_pub_last_time_ = 0;
   control_feedback_state_pub_last_time_ = 0;
 
+  // control mode: attitude or body rate
+  is_attitude_ctrl_ = true;
+  is_body_rate_ctrl_ = false;
+
   reset();
 }
 
@@ -225,20 +233,20 @@ void AttitudeController::update(void)
         }
 
       ap::Vector3f angles;
-      ap::Vector3f vel;
+      ap::Vector3f ang_vel;
 #ifdef SIMULATION
       if(use_ground_truth_)
         {
           angles = true_angles_;
-          vel = true_vel_;
+          ang_vel = true_ang_vel_;
         }
       else
         {
           angles = estimator_->getAttEstimator()->getAttitude(Frame::VIRTUAL);
-          vel = estimator_->getAttEstimator()->getAngular(Frame::VIRTUAL);
+          ang_vel = estimator_->getAttEstimator()->getAngular(Frame::VIRTUAL);
         }
 
-      ROS_DEBUG_THROTTLE(0.01, "true vs spinal: r [%f vs %f], p [%f vs %f], y [%f vs %f], ws [%f vs %f], wy [%f vs %f], wz [%f vs %f]", true_angles_.x, angles.x, true_angles_.y, angles.y, true_angles_.z, angles.z, true_vel_.x, vel.x, true_vel_.y, vel.y, true_vel_.z, vel.z);
+      ROS_DEBUG_THROTTLE(0.01, "true vs spinal: r [%f vs %f], p [%f vs %f], y [%f vs %f], ws [%f vs %f], wy [%f vs %f], wz [%f vs %f]", true_angles_.x, angles.x, true_angles_.y, angles.y, true_angles_.z, angles.z, true_ang_vel_.x, ang_vel.x, true_ang_vel_.y, ang_vel.y, true_ang_vel_.z, ang_vel.z);
 
       if(prev_time_ < 0) DELTA_T = 0;
       else DELTA_T = ros::Time::now().toSec() - prev_time_;
@@ -275,66 +283,87 @@ void AttitudeController::update(void)
       // linear control method
       {
         /* gyro moment */
-        ap::Vector3f gyro_moment = vel % (inertia_ * vel);
+        ap::Vector3f gyro_moment = ang_vel % (inertia_ * ang_vel);
 #ifdef SIMULATION
         std_msgs::Float32MultiArray anti_gyro_msg;
 #endif
 
-        float error_angle[3];
-        for(int axis = 0; axis < 3; axis++)
+      float error_angle[3];
+      float error_ang_vel[3];
+      for (int axis = 0; axis < 3; axis++)
+      {
+        error_angle[axis] = target_angle_[axis] - angles[axis];
+
+        if (is_body_rate_ctrl_)
+          error_ang_vel[axis] = target_ang_vel_[axis] - ang_vel[axis];
+        else
+          error_ang_vel[axis] = 0 - ang_vel[axis];
+
+        if (integrate_flag_)
+          error_angle_i_[axis] += error_angle[axis] * DELTA_T;
+
+        if (axis == X)
+        {
+          control_feedback_state_msg_.roll_p = error_angle[axis] * 1000;
+          control_feedback_state_msg_.roll_i = error_angle_i_[axis] * 1000;
+          control_feedback_state_msg_.roll_d = error_ang_vel[axis] * 1000;
+        }
+        if (axis == Y)
+        {
+          control_feedback_state_msg_.pitch_p = error_angle[axis] * 1000;
+          control_feedback_state_msg_.pitch_i = error_angle_i_[axis] * 1000;
+          control_feedback_state_msg_.pitch_d = error_ang_vel[axis] * 1000;
+        }
+        if (axis == Z)
+        {
+          control_feedback_state_msg_.yaw_d = error_ang_vel[axis] * 1000;
+        }
+      }
+
+      float p_term = 0;
+      float i_term = 0;
+      float d_term = 0;
+      for (int i = 0; i < motor_number_; i++)
+      {
+        for (int axis = 0; axis < 3; axis++)
+        {
+          if (is_attitude_ctrl_)
           {
-            error_angle[axis] = target_angle_[axis] - angles[axis];
-            if(integrate_flag_) error_angle_i_[axis] += error_angle[axis] * DELTA_T;
-
-            if(axis == X)
-              {
-                control_feedback_state_msg_.roll_p = error_angle[axis] * 1000;
-                control_feedback_state_msg_.roll_i = error_angle_i_[axis] * 1000;
-                control_feedback_state_msg_.roll_d = vel[axis]  * 1000;
-              }
-            if(axis == Y)
-              {
-                control_feedback_state_msg_.pitch_p = error_angle[axis] * 1000;
-                control_feedback_state_msg_.pitch_i = error_angle_i_[axis] * 1000;
-                control_feedback_state_msg_.pitch_d = vel[axis]  * 1000;
-
-              }
-            if(axis == Z)
-              {
-                control_feedback_state_msg_.yaw_d = vel[axis] * 1000;
-              }
+            p_term = error_angle[axis] * thrust_p_gain_[i][axis];
+            i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis];
+            d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis];
+          }
+          else
+          {
+            p_term = 0;
+            i_term = 0;
+            d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis];
           }
 
-        float p_term = 0;
-        float i_term = 0;
-        float d_term = 0;
-        for(int i = 0; i < motor_number_; i++)
+          if (axis == X)
+          {
+            roll_pitch_term_[i] = p_term + i_term + d_term;  // [N]
+            control_term_msg_.motors[i].roll_p = p_term * 1000;
+            control_term_msg_.motors[i].roll_i = i_term * 1000;
+            control_term_msg_.motors[i].roll_d = d_term * 1000;
+          }
+          if (axis == Y)
+          {
+            roll_pitch_term_[i] += (p_term + i_term + d_term);  // [N]
+            control_term_msg_.motors[i].pitch_p = p_term * 1000;
+            control_term_msg_.motors[i].pitch_i = i_term * 1000;
+            control_term_msg_.motors[i].pitch_d = d_term * 1000;
+          }
+          if (axis == Z)
           {
-            for(int axis = 0; axis < 3; axis++)
-              {
-                p_term = error_angle[axis] * thrust_p_gain_[i][axis];
-                i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis];
-                d_term = -vel[axis] * thrust_d_gain_[i][axis];
-                if(axis == X)
-                  {
-                    roll_pitch_term_[i] = p_term + i_term + d_term; // [N]
-                    control_term_msg_.motors[i].roll_p = p_term * 1000;
-                    control_term_msg_.motors[i].roll_i= i_term * 1000;
-                    control_term_msg_.motors[i].roll_d = d_term * 1000;
-                  }
-                if(axis == Y)
-                  {
-                    roll_pitch_term_[i] += (p_term + i_term + d_term); // [N]
-                    control_term_msg_.motors[i].pitch_p = p_term * 1000;
-                    control_term_msg_.motors[i].pitch_i = i_term * 1000;
-                    control_term_msg_.motors[i].pitch_d = d_term * 1000;
-                  }
-                if(axis == Z)
-                  {
-                    yaw_term_[i] = extra_yaw_pi_term_[i] + d_term;
-                    control_term_msg_.motors[i].yaw_d = d_term * 1000; //d_term;
-                  }
-              }
+            if (is_attitude_ctrl_)
+              yaw_term_[i] = extra_yaw_pi_term_[i] + d_term;
+            else
+              yaw_term_[i] = d_term;
+
+            control_term_msg_.motors[i].yaw_d = d_term * 1000;  // d_term;
+          }
+        }
 
             /* gyro moment compensation */
             float gyro_moment_compensate =
@@ -395,10 +424,11 @@ void AttitudeController::reset(void)
         }
     }
 
-  for(int i = 0; i < 3; i++)
-    {
-      target_angle_[i] = 0;
-      error_angle_i_[i] = 0;
+  for (int i = 0; i < 3; i++)
+  {
+    target_angle_[i] = 0;
+    target_ang_vel_[i] = 0;
+    error_angle_i_[i] = 0;
 
       torque_p_gain_[i] = 0;
       torque_i_gain_[i] = 0;
@@ -467,13 +497,21 @@ void AttitudeController::fourAxisCommandCallback( const spinal::FourAxisCommand
   if(!failsafe_) failsafe_ = true;
   flight_command_last_stamp_ = HAL_GetTick();
 
+  // get msg data from spinal::FourAxisCommand
   target_angle_[X] = cmd_msg.angles[0];
   target_angle_[Y] = cmd_msg.angles[1];
 
-  for(int i = 0; i < motor_number_; i++)
-    {
-      // base thrust is about the z control
-      base_thrust_term_[i] = cmd_msg.base_thrust[i];
+  if (is_body_rate_ctrl_)
+  {
+    target_ang_vel_[X] = cmd_msg.body_rates[0];
+    target_ang_vel_[Y] = cmd_msg.body_rates[1];
+    target_ang_vel_[Z] = cmd_msg.body_rates[2];
+  }
+
+  for (int i = 0; i < motor_number_; i++)
+  {
+    // base thrust is about the z control
+    base_thrust_term_[i] = cmd_msg.base_thrust[i];
 
       // reconstruct the pi term for yaw (temporary measure for pwm saturation avoidance)
       if(max_yaw_term_index_ != -1)

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h

@@ -45,6 +45,7 @@
 #include <spinal/UavInfo.h>
 #include <spinal/PMatrixPseudoInverseWithInertia.h>
 #include <spinal/TorqueAllocationMatrixInv.h>
+#include <spinal/SetControlMode.h>
 
 #define IDLE_DUTY 0.5f
 #define FORCE_LANDING_INTEGRAL 0.0025f // 500Hz * 0.0025 = 1.25 N / sec
@@ -129,6 +130,20 @@ class AttitudeController
   void setSimVolCallback(const std_msgs::Float32 vol_msg) { sim_voltage_ = vol_msg.data; }
   float sim_voltage_;
 
+  ros::ServiceServer control_mode_srv_;
+  bool setControlModeCallback(spinal::SetControlMode::Request& req, spinal::SetControlMode::Response& res)
+  {
+    if (req.is_attitude == false && req.is_body_rate==false)
+    {
+      ROS_ERROR("invalid: attitude and body rate control mode can not be set both to false.");
+      return false;
+    }
+
+    is_attitude_ctrl_ = req.is_attitude;
+    is_body_rate_ctrl_ = req.is_body_rate;
+    return true;
+  }
+
 #else
   ros::Subscriber<spinal::FourAxisCommand, AttitudeController> four_axis_cmd_sub_;
   ros::Subscriber<spinal::PwmInfo, AttitudeController> pwm_info_sub_;
@@ -140,6 +155,16 @@ class AttitudeController
 
   void setAttitudeControlCallback(const std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { att_control_flag_ = req.data; }
 
+  ros::ServiceServer<spinal::SetControlMode::Request, spinal::SetControlMode::Response, AttitudeController> control_mode_srv_;
+  void setControlModeCallback(const spinal::SetControlMode::Request& req, spinal::SetControlMode::Response& res) {
+    if (req.is_attitude == false && req.is_body_rate == false)
+    {
+      ROS_ERROR("invalid: attitude and body rate control mode can not be set both to false.");
+    }
+    is_attitude_ctrl_ = req.is_attitude;
+    is_body_rate_ctrl_ = req.is_body_rate;
+  }
+
   BatteryStatus* bat_;
   osMutexId* mutex_;
 #endif
@@ -156,6 +181,7 @@ class AttitudeController
 
 
   float target_angle_[3];
+  float target_ang_vel_[3];
   float error_angle_i_[3];
   float error_angle_i_limit_[3];
 
@@ -176,6 +202,10 @@ class AttitudeController
   float p_matrix_pseudo_inverse_[MAX_MOTOR_NUMBER][4];
   ap::Matrix3f inertia_;
 
+  // control level
+  bool is_attitude_ctrl_;
+  bool is_body_rate_ctrl_;
+
   // Failsafe
   bool failsafe_;
   uint32_t flight_command_last_stamp_;
@@ -225,9 +255,14 @@ class AttitudeController
 public:
   void useGroundTruth(bool flag) { use_ground_truth_ = flag; }
   void setTrueRPY(float r, float p, float y) {true_angles_.x = r; true_angles_.y = p; true_angles_.z = y; }
-  void setTrueAngular(float wx, float wy, float wz) {true_vel_.x = wx; true_vel_.y = wy; true_vel_.z = wz; }
+  void setTrueAngular(float wx, float wy, float wz)
+  {
+    true_ang_vel_.x = wx;
+    true_ang_vel_.y = wy;
+    true_ang_vel_.z = wz;
+  }
   ap::Vector3f true_angles_;
-  ap::Vector3f true_vel_;
+  ap::Vector3f true_ang_vel_;
   float DELTA_T;
   double prev_time_;
 #endif

aerial_robot_nerve/spinal/msg/FourAxisCommand.msg

@@ -1,2 +1,3 @@
-float32[3] angles
-float32[] base_thrust
+float32[3] angles  # rad  target roll angle, pitch angle, and yaw PI term (not target yaw angle)
+float32[3] body_rates  # rad/s  target roll rate, pitch rate, yaw rate
+float32[] base_thrust  # N

aerial_robot_nerve/spinal/srv/SetControlMode.srv

@@ -0,0 +1,12 @@
+# set Control Mode for Spinal
+#
+# reference: http://docs.ros.org/en/api/mavros_msgs/html/srv/SetMode.html
+# and https://github.com/PX4/px4_msgs/blob/main/msg/OffboardControlMode.msg
+
+# if all false, raise error. if all true, body_rate will be used as a feedforward term.
+
+bool is_attitude
+bool is_body_rate
+
+---
+bool is_success