feat: update offboard command message to reflect new vector notation

* feat: move mixing matrix print logic to rosflight_io and add a service call

Author: JMoore5353

rosflight_firmware

@@ -519,6 +519,18 @@ class ROSflightIO : public rclcpp::Node,
    */
   bool checkIfAllParamsReceivedCallback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
                                         const std_srvs::srv::Trigger::Response::SharedPtr & res);
+  /**
+   * @brief "print_mixing_matrix" service callback.
+   *
+   * This function is called anytime the "print_mixing_matrix" ROS service is called. It returns
+   * true.
+   *
+   * @param req ROS Trigger service request.
+   * @param res ROS Trigger service response.
+   * @return True
+   */
+  bool printMixingMatrixCallback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
+                                 const std_srvs::srv::Trigger::Response::SharedPtr & res);
 
   // timer callbacks
   /**
@@ -587,6 +599,13 @@ class ROSflightIO : public rclcpp::Node,
    * as a convenience for easy read/write access from ROS2
    */
   void load_convenience_parameters();
+  /**
+   * @brief logs the mixing matrix parameters to the ROS2 logger. This usually prints
+   * to the screen and is a helpful debugging tool.
+   *
+   * @return true if all mixer params are received from the firmware.
+   */
+  bool log_mixer_params();
   /**
   * @brief Handles any parameter changes to the node.
   *
@@ -664,6 +683,8 @@ class ROSflightIO : public rclcpp::Node,
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reboot_bootloader_srv_;
   /// "all_params_received" ROS service.
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr check_if_all_params_received_srv_;
+  /// "print_mixing_matrix" ROS service.
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr print_mixing_matrix_srv_;
 
   /// Handle for the ROS2 parameter callback
   OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;

rosflight_io/include/rosflight_io/rosflight_io.hpp

@@ -125,6 +125,10 @@ ROSflightIO::ROSflightIO()
     "all_params_received",
     std::bind(&ROSflightIO::checkIfAllParamsReceivedCallback, this, std::placeholders::_1,
               std::placeholders::_2));
+  print_mixing_matrix_srv_ = this->create_service<std_srvs::srv::Trigger>(
+    "print_mixing_matrix",
+    std::bind(&ROSflightIO::printMixingMatrixCallback, this, std::placeholders::_1,
+              std::placeholders::_2));
 
   declare_parameters();
   parameter_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&ROSflightIO::parameters_callback, this, std::placeholders::_1));
@@ -276,6 +280,58 @@ void ROSflightIO::load_convenience_parameters()
   }
 }
 
+bool ROSflightIO::log_mixer_params()
+{
+  // Get the primary and secondary mixer type
+  std::string param_name = "PRIMARY_MIXER";
+  double primary_mixer{0};
+  if (!mavrosflight_->param.get_param_value(param_name, &primary_mixer)) {
+    RCLCPP_WARN(this->get_logger(), "Failed to get parameter '%s' from the firmware!", param_name.c_str());
+    return false;
+  }
+
+  param_name = "SECONDARY_MIXER";
+  double secondary_mixer{0};
+  if (!mavrosflight_->param.get_param_value(param_name, &secondary_mixer)) {
+    RCLCPP_WARN(this->get_logger(), "Failed to get parameter '%s' from the firmware!", param_name.c_str());
+    return false;
+  }
+
+  // Get the relevant mixer header parameters
+  Eigen::Matrix<double, 1, 10> mixer_header_vals{Eigen::Matrix<double,1,10>::Zero()};
+  for (int i=0; i<10; ++i) {
+    param_name = "PRI_MIXER_OUT_" + std::to_string(i);
+    if (!mavrosflight_->param.get_param_value(param_name, &mixer_header_vals[i])) {
+      RCLCPP_WARN(this->get_logger(), "Failed to get parameter '%s' from the firmware!", param_name.c_str());
+      return false;
+    }
+  }
+
+  // Get the mixer matrix parameters
+  Eigen::Matrix<double, 10, 10> primary_mixing_matrix{Eigen::Matrix<double,10,10>::Zero()};
+  Eigen::Matrix<double, 10, 10> secondary_mixing_matrix{Eigen::Matrix<double,10,10>::Zero()};
+  for (int i=0; i<10; ++i) {
+    for (int j=0; j<10; ++j) {
+      param_name = "PRI_MIXER_" + std::to_string(i) + "_" + std::to_string(j);
+      if (!mavrosflight_->param.get_param_value(param_name, &primary_mixing_matrix(i,j))) {
+        RCLCPP_WARN(this->get_logger(), "Failed to get parameter '%s' from the firmware!", param_name.c_str());
+        return false;
+      }
+
+      param_name = "SEC_MIXER_" + std::to_string(i) + "_" + std::to_string(j);
+      if (!mavrosflight_->param.get_param_value(param_name, &secondary_mixing_matrix(i,j))) {
+        RCLCPP_WARN(this->get_logger(), "Failed to get parameter '%s' from the firmware!", param_name.c_str());
+        return false;
+      }
+    }
+  }
+
+  RCLCPP_INFO_STREAM(this->get_logger(), "Mixer output channel types: " << mixer_header_vals);
+  RCLCPP_INFO_STREAM(this->get_logger(), "Primary mixer:\n" << primary_mixing_matrix);
+  RCLCPP_INFO_STREAM(this->get_logger(), "Secondary mixer:\n" << secondary_mixing_matrix);
+  return true;
+}
+
 ROSflightIO::~ROSflightIO()
 {
   delete mavrosflight_;
@@ -872,15 +928,13 @@ void ROSflightIO::commandCallback(const rosflight_msgs::msg::Command::ConstShare
   auto mode = (OFFBOARD_CONTROL_MODE) msg->mode;
   auto ignore = (OFFBOARD_CONTROL_IGNORE) msg->ignore;
 
-  float Qx = msg->qx;
-  float Qy = msg->qy;
-  float Qz = msg->qz;
-  float Fx = msg->fx;
-  float Fy = msg->fy;
-  float Fz = msg->fz;
+  float command_vect[10] = {0,0,0,0,0,0,0,0,0,0};
+  for (int i=0; i<10; ++i) {
+    command_vect[i] = msg->u[i];
+  }
 
   mavlink_message_t mavlink_msg;
-  mavlink_msg_offboard_control_pack(1, 50, &mavlink_msg, mode, ignore, Qx, Qy, Qz, Fx, Fy, Fz);
+  mavlink_msg_offboard_control_pack(1, 50, &mavlink_msg, mode, ignore, command_vect);
   mavrosflight_->comm.send_message(mavlink_msg);
 }
 
@@ -984,6 +1038,7 @@ void ROSflightIO::paramTimerCallback()
     RCLCPP_INFO(this->get_logger(), "Received all parameters");
 
     load_convenience_parameters();
+    log_mixer_params();
   } else {
     mavrosflight_->param.request_params();
     RCLCPP_ERROR(this->get_logger(),
@@ -1142,4 +1197,12 @@ bool ROSflightIO::checkIfAllParamsReceivedCallback(
   return true;
 }
 
+bool ROSflightIO::printMixingMatrixCallback(
+  const std_srvs::srv::Trigger::Request::SharedPtr & req,
+  const std_srvs::srv::Trigger::Response::SharedPtr & res)
+{
+  res->success = log_mixer_params();
+  return true;
+}
+
 } // namespace rosflight_io

rosflight_io/src/rosflight_io.cpp

@@ -6,20 +6,19 @@ uint8 MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 1
 uint8 MODE_ROLL_PITCH_YAWRATE_THROTTLE = 2
 
 # ignore field bitmasks
-uint8 IGNORE_NONE = 0
-uint8 IGNORE_QX = 1
-uint8 IGNORE_QY = 2
-uint8 IGNORE_QZ = 4
-uint8 IGNORE_FX = 8
-uint8 IGNORE_FY = 16
-uint8 IGNORE_FZ = 32
+uint16 IGNORE_NONE = 0x0
+uint16 IGNORE_VALUE0 = 0x1
+uint16 IGNORE_VALUE1 = 0x2
+uint16 IGNORE_VALUE2 = 0x4
+uint16 IGNORE_VALUE3 = 0x8
+uint16 IGNORE_VALUE4 = 0x10
+uint16 IGNORE_VALUE5 = 0x20
+uint16 IGNORE_VALUE6 = 0x40
+uint16 IGNORE_VALUE7 = 0x80
+uint16 IGNORE_VALUE8 = 0x100
+uint16 IGNORE_VALUE9 = 0x200
 
 std_msgs/Header header
-uint8 mode # offboard control mode for interpreting value fields
-uint8 ignore # bitmask for ignore specific setpoint values
-float32 qx
-float32 qy
-float32 qz
-float32 fx
-float32 fy
-float32 fz
+uint8 mode      # offboard control mode for interpreting value fields
+uint16 ignore    # bitmask for ignore specific setpoint values
+float32[10] u   # Vector of control commands

rosflight_msgs/msg/Command.msg

@@ -64,8 +64,8 @@ class ForcesAndMomentsInterface : public rclcpp::Node
   static constexpr uint8_t NUM_MIXER_OUTPUTS = 10;
 
   // Matrix to store the current mixing matrix
-  Eigen::Matrix<double, 6, NUM_MIXER_OUTPUTS> primary_mixing_matrix_;
-  Eigen::Matrix<double, 6, NUM_MIXER_OUTPUTS> secondary_mixing_matrix_;
+  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS> primary_mixing_matrix_;
+  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS> secondary_mixing_matrix_;
   Eigen::Matrix<int, 1, NUM_MIXER_OUTPUTS> mixer_header_vals_;
   int primary_mixer_ = 255;
   int secondary_mixer_ = 255;
@@ -131,7 +131,7 @@ class ForcesAndMomentsInterface : public rclcpp::Node
   void get_mixer_firmware_parameters();
   double send_get_param_service_to_firmware(std::string param_name);
   bool send_check_params_service_to_firmware();
-  void invert_matrix(Eigen::Matrix<double, 6, NUM_MIXER_OUTPUTS> &mixer_to_invert);
+  void invert_matrix(Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS> &mixer_to_invert);
 
 public:
 

rosflight_sim/include/rosflight_sim/forces_and_moments_interface.hpp

@@ -40,8 +40,8 @@ namespace rosflight_sim
 
 ForcesAndMomentsInterface::ForcesAndMomentsInterface()
   : rclcpp::Node("mav_forces_and_moments")
-  , primary_mixing_matrix_(Eigen::Matrix<double, 6, NUM_MIXER_OUTPUTS>::Zero())
-  , secondary_mixing_matrix_(Eigen::Matrix<double, 6, NUM_MIXER_OUTPUTS>::Zero())
+  , primary_mixing_matrix_(Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS>::Zero())
+  , secondary_mixing_matrix_(Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS>::Zero())
   , mixer_header_vals_(Eigen::Matrix<int, 1, NUM_MIXER_OUTPUTS>::Zero())
 {
   // Note that we don't define the parameter callback routine here.
@@ -149,7 +149,7 @@ void ForcesAndMomentsInterface::get_mixer_firmware_parameters()
   }
 
   // Get the mixer matrix parameters and relevant header parameters
-  for (int i=0; i<6; ++i) {
+  for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
     for (int j=0; j<NUM_MIXER_OUTPUTS; ++j) {
       param_name = "PRI_MIXER_" + std::to_string(i) + "_" + std::to_string(j);
       param_val = send_get_param_service_to_firmware(param_name);
@@ -160,10 +160,6 @@ void ForcesAndMomentsInterface::get_mixer_firmware_parameters()
       secondary_mixing_matrix_(i,j) = param_val;
     }
   }
-
-  RCLCPP_INFO_STREAM(this->get_logger(), "Primary mixer used in sim:\n" << primary_mixing_matrix_);
-  RCLCPP_INFO_STREAM(this->get_logger(), "Secondary mixer used in sim:\n" << secondary_mixing_matrix_);
-  RCLCPP_INFO_STREAM(this->get_logger(), "Mixer headers used in sim:\n" << mixer_header_vals_);
 }
 
 double ForcesAndMomentsInterface::send_get_param_service_to_firmware(std::string param_name)
@@ -225,25 +221,22 @@ bool ForcesAndMomentsInterface::send_check_params_service_to_firmware()
   return response->success;
 }
 
-void ForcesAndMomentsInterface::invert_matrix(Eigen::Matrix<double, 6, NUM_MIXER_OUTPUTS> &mixer_to_invert)
+void ForcesAndMomentsInterface::invert_matrix(Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS> &mixer_to_invert)
 {
   // Calculate the pseudoinverse of the mixing matrix using the SVD
-  Eigen::JacobiSVD<Eigen::Matrix<double, 6, NUM_MIXER_OUTPUTS>> svd(
+  Eigen::JacobiSVD<Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS>> svd(
     mixer_to_invert,
     Eigen::FullPivHouseholderQRPreconditioner | Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, 6> Sig;
+  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS> Sig;
   Sig.setZero();
 
   // Avoid dividing by zero in the Sigma matrix
-  if (svd.singularValues()[0] != 0.0) { Sig(0, 0) = 1.0 / svd.singularValues()[0]; }
-  if (svd.singularValues()[1] != 0.0) { Sig(1, 1) = 1.0 / svd.singularValues()[1]; }
-  if (svd.singularValues()[2] != 0.0) { Sig(2, 2) = 1.0 / svd.singularValues()[2]; }
-  if (svd.singularValues()[3] != 0.0) { Sig(3, 3) = 1.0 / svd.singularValues()[3]; }
-  if (svd.singularValues()[4] != 0.0) { Sig(4, 4) = 1.0 / svd.singularValues()[4]; }
-  if (svd.singularValues()[5] != 0.0) { Sig(5, 5) = 1.0 / svd.singularValues()[5]; }
+  for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
+    if (svd.singularValues()[i] != 0.0) { Sig(i, i) = 1.0 / svd.singularValues()[i]; }
+  }
 
   // Pseudoinverse of the mixing matrix
-  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, 6> mixer_pinv = svd.matrixV() * Sig * svd.matrixU().transpose();
+  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS> mixer_pinv = svd.matrixV() * Sig * svd.matrixU().transpose();
 
   // Save the inverted matrix
   mixer_to_invert = mixer_pinv.transpose();