diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index ee94224..885b3c4 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -143,6 +143,7 @@ namespace mavlink_interface void MagnetometerCallback(const gz::msgs::Magnetometer &_msg); void GpsCallback(const gz::msgs::NavSat &_msg); void SendSensorMessages(const gz::sim::UpdateInfo &_info); + void SendStatusMessages(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm); void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm, const Eigen::VectorXd &_vels); void handle_actuator_controls(const gz::sim::UpdateInfo &_info); diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index a631998..d047a25 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -70,6 +70,8 @@ static constexpr auto kDefaultBaudRate = 921600; static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16; static constexpr size_t MAX_TXQ_SIZE = 1000; +static constexpr ssize_t MAX_N_ESCS = 16; + //! Rx packer framing status. (same as @p mavlink::mavlink_framing_t) enum class Framing : uint8_t { incomplete = MAVLINK_FRAMING_INCOMPLETE, @@ -110,6 +112,20 @@ namespace SensorData { }; } +namespace StatusData { + struct EscReport { + int rpm; + double voltage; + double current; + }; + + struct EscStatus { + int esc_count; + int esc_active_input; + struct EscReport esc[MAX_N_ESCS]; + }; +} + /* struct HILData { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -143,6 +159,7 @@ class MavlinkInterface { void close(); void Load(); void SendSensorMessages(const uint64_t time_usec); + void SendEscStatusMessages(const uint64_t time_usec, struct StatusData::EscStatus &status); void UpdateBarometer(const SensorData::Barometer &data); void UpdateAirspeed(const SensorData::Airspeed &data); void UpdateIMU(const SensorData::Imu &data); diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index 67a8107..038258c 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -26,10 +26,12 @@ #include #include +#include #include #include #include #include +#include GZ_ADD_PLUGIN( mavlink_interface::GazeboMavlinkInterface, @@ -255,6 +257,8 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info, if (received_first_actuator_) { PublishRotorVelocities(_ecm, input_reference_); } + + SendStatusMessages(_info, _ecm); } void GazeboMavlinkInterface::PostUpdate(const gz::sim::UpdateInfo &_info, @@ -424,6 +428,65 @@ void GazeboMavlinkInterface::SendSensorMessages(const gz::sim::UpdateInfo &_info mavlink_interface_->SendSensorMessages(time_usec); } +void GazeboMavlinkInterface::SendStatusMessages(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) { + uint64_t time_usec = std::chrono::duration_cast>(_info.simTime * 1e6).count(); + struct StatusData::EscStatus status; + + // TOOD: find out how to properly get the RPM for each motor from the motor model. Multicopter motor model probably + // doesn't support that. The joint velocity (some draft code left below) might be only for visuals. + // There is additionally the scaler "rotorVelocitySlowdownSim" that should be taken into account if + // using the joint velocity. + + // Buth actually both below are wrong; the motor constant doesn't affect the + // visual rotation speed in gazebo, it is only used for thrust calculation. + +#if 1 + + // Read the joint velocities from gazebo + // Note: CW values are positive, CCW negative + + std::vector vels; + char joint_name_c[] = "rotor_0_joint"; + gz::sim::Entity jointEntity = _ecm.EntityByComponents(gz::sim::components::Name(joint_name_c), gz::sim::components::Joint()); + + double vel; + int i = 0; + while (jointEntity != gz::sim::kNullEntity) { + gz::sim::Joint joint(jointEntity); + + std::optional> jointVelocity = joint.Velocity(_ecm); + if (jointVelocity && (*jointVelocity).size() > 0) { + vel = (*jointVelocity)[0] + / 1000 // maxRotVelocity + * 10; // rotorVelocitySlowdownSim + + vels.push_back(vel); + } + + i++; + joint_name_c[6] = '0' + i; + jointEntity = _ecm.EntityByComponents(gz::sim::components::Name(joint_name_c), gz::sim::components::Joint()); + } +#else + + // Just use the actuator controls directly + // Note: this bypasses the motor model entirely and "fakes" the RPM. + // All RPM values are positive + + auto vels = mavlink_interface_->GetActuatorControls(); +#endif + + status.esc_count = vels.size(); + + for (int i = 0; i < vels.size(); i++) { + status.esc[i].rpm = vels[i] + / 0.00000854858 // motorConstant + / (2 * 3.14); // rad/s -> RPM + } + + mavlink_interface_->SendEscStatusMessages(time_usec, status); +} + void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo &_info) { bool armed = mavlink_interface_->GetArmedState(); @@ -557,4 +620,4 @@ void GazeboMavlinkInterface::RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NE // final rotation composition q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse(); -} \ No newline at end of file +} diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 2e13ae8..b00fc01 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -417,6 +417,36 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec) { PushSendMessage(msg_shared); } +void MavlinkInterface::SendEscStatusMessages(uint64_t time_usec, struct StatusData::EscStatus &status) { + mavlink_esc_status_t esc_status_msg{}; + mavlink_esc_info_t esc_info_msg{}; + static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN; + static uint16_t counter = 0; + + esc_status_msg.time_usec = time_usec; + for (int i = 0; i < batch_size; i++) { + esc_status_msg.rpm[i] = status.esc[i].rpm; + + esc_info_msg.failure_flags[i] = 0; + esc_info_msg.error_count[i] = 0; + esc_info_msg.temperature[i] = 20; + } + + mavlink_message_t msg; + mavlink_msg_esc_status_encode_chan(254, 25, MAVLINK_COMM_0, &msg, &esc_status_msg); + auto msg_shared = std::make_shared(msg); + PushSendMessage(msg_shared); + + esc_info_msg.counter = counter++; + esc_info_msg.count = status.esc_count; + esc_info_msg.connection_type = 0; // TODO: use two highest bits for selected input + esc_info_msg.info = (1u << status.esc_count) - 1; + + mavlink_msg_esc_info_encode_chan(254, 25, MAVLINK_COMM_0, &msg, &esc_info_msg); + msg_shared = std::make_shared(msg); + PushSendMessage(msg_shared); +} + void MavlinkInterface::UpdateBarometer(const SensorData::Barometer &data) { const std::lock_guard lock(sensor_msg_mutex_); temperature_ = data.temperature;