diff --git a/CMakeLists.txt b/CMakeLists.txt
index c98b602d..35cc8eda 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,6 +31,9 @@ if("$ENV{GZ_VERSION}" STREQUAL "harmonic" OR NOT DEFINED "ENV{GZ_VERSION}")
gz_find_package(gz-sim8 REQUIRED)
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
+ gz_find_package(gz-msgs10 REQUIRED)
+ set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR})
+
message(STATUS "Compiling against Gazebo Harmonic")
elseif("$ENV{GZ_VERSION}" STREQUAL "ionic")
# Ionic
@@ -69,10 +72,29 @@ endif()
# --------------------------------------------------------------------------- #
find_package(RapidJSON REQUIRED)
find_package(OpenCV REQUIRED)
+find_package(Protobuf REQUIRED)
pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0)
-
+# --------------------------------------------------------------------------- #
+# Build proto custom messages
+set(ARDUPILOT_MSGS_PROTOS
+ ${CMAKE_CURRENT_SOURCE_DIR}/proto/ardupilot_gazebo/msgs/motor_status.proto
+)
+
+gz_msgs_generate_messages(
+ # The cmake target to be generated for libraries/executables to link
+ TARGET msgs
+ # The protobuf package to generate (Typically based on the path)
+ PROTO_PACKAGE "ardupilot_gazebo.msgs"
+ # The path to the base directory of the proto files
+ # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto)
+ MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto
+ # List of proto files to generate
+ MSGS_PROTOS ${ARDUPILOT_MSGS_PROTOS}
+ # List of message targets this library imports from
+ DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER}
+)
# --------------------------------------------------------------------------- #
# Build plugin.
@@ -89,6 +111,24 @@ target_link_libraries(ArduPilotPlugin PRIVATE
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
)
+add_library(MotorPlugin
+ SHARED
+ src/MotorPlugin.cc
+ src/Util.cc
+
+)
+target_include_directories(MotorPlugin PRIVATE
+ include
+ # ${CMAKE_CURRENT_BINARY_DIR}/ardupilot_gazebo-msgs_genmsg
+)
+target_link_libraries(MotorPlugin PRIVATE
+ gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+ ${PROJECT_NAME}-msgs
+ # gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER}
+ # ardupilot_gazebo-msgs
+)
+# add_dependencies(MotorPlugin ardupilot_gazebo-msgs)
+
add_library(ParachutePlugin
SHARED
src/ParachutePlugin.cc
@@ -134,6 +174,7 @@ target_link_libraries(GstCameraPlugin PRIVATE
install(
TARGETS
ArduPilotPlugin
+ MotorPlugin
ParachutePlugin
CameraZoomPlugin
GstCameraPlugin
diff --git a/include/MotorPlugin.hh b/include/MotorPlugin.hh
new file mode 100644
index 00000000..07e32c8e
--- /dev/null
+++ b/include/MotorPlugin.hh
@@ -0,0 +1,86 @@
+/*
+ Copyright (C) 2025 ArduPilot.org
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see .
+*/
+
+#ifndef MOTORPLUGIN_HH_
+#define MOTORPLUGIN_HH_
+
+#include
+
+#include
+
+namespace gz {
+namespace sim {
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems {
+
+/// \brief Plugin for an electro-mechanical model of a motor.
+/// \class MotorPlugin
+///
+/// A Gazebo plugin that simulates a brushless DC motor with thermal characteristics.
+/// The plugin calculates motor performance including voltage, current, RPM, and temperature
+/// based on physical parameters and publishes these as motor statistics.
+///
+/// Parameters for each control element:
+///
+/// Name of the joint to control
+/// Battery voltage (V)
+/// Motor Kv rating (RPM/V)
+/// Motor internal resistance (Ohm)
+/// No-load current draw (A)
+/// Topic for receiving motor commands
+/// Thermal resistance (°C/W)
+/// Thermal capacitance (J/°C)
+/// Ambient temperature (°C)
+///
+///
+/// Published Data:
+/// Motor statistics are published on topic:
+/// /model/[model_name]/joint/[joint_name]/motor_stats
+///
+class MotorPlugin :
+ public System,
+ public ISystemPreUpdate,
+ public ISystemConfigure
+{
+ /// \brief Destructor
+ public: virtual ~MotorPlugin();
+
+ /// \brief Constructor
+ public: MotorPlugin();
+
+ // Documentation inherited
+ public: void PreUpdate(const gz::sim::UpdateInfo &_info,
+ EntityComponentManager &_ecm) final;
+
+ // Documentation inherited
+ public: void Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &) final;
+
+ /// \internal
+ /// \brief Private implementation
+ private: class Impl;
+ private: std::unique_ptr impl;
+};
+
+} // namespace systems
+}
+} // namespace sim
+} // namespace gz
+
+#endif // MOTORPLUGIN_HH_
diff --git a/models/iris_with_gimbal/model.sdf b/models/iris_with_gimbal/model.sdf
index 70935b39..22353ac6 100755
--- a/models/iris_with_gimbal/model.sdf
+++ b/models/iris_with_gimbal/model.sdf
@@ -341,4 +341,4 @@
-
+
\ No newline at end of file
diff --git a/models/iris_with_standoffs/model.sdf b/models/iris_with_standoffs/model.sdf
index ad4980cc..b8b8a987 100755
--- a/models/iris_with_standoffs/model.sdf
+++ b/models/iris_with_standoffs/model.sdf
@@ -241,7 +241,7 @@
1e+16
- 0.004
+ 1.0e-4
@@ -315,7 +315,7 @@
1e+16
- 0.004
+ 1.0e-4
@@ -389,7 +389,7 @@
1e+16
- 0.004
+ 1.0e-4
@@ -463,7 +463,7 @@
1e+16
- 0.004
+ 1.0e-4
diff --git a/proto/ardupilot_gazebo/msgs/motor_status.proto b/proto/ardupilot_gazebo/msgs/motor_status.proto
new file mode 100644
index 00000000..9655626c
--- /dev/null
+++ b/proto/ardupilot_gazebo/msgs/motor_status.proto
@@ -0,0 +1,20 @@
+syntax = "proto3";
+package ardupilot_gazebo.msgs;
+
+// Message to hold the status of a single motor.
+message MotorStatus {
+ // Motor Id
+ int32 motor_id = 1;
+
+ // Rotational speed of the motor in revolutions per minute (RPM).
+ double rpm = 2;
+
+ // Voltage supplied to the motor in Volts.
+ double voltage = 3;
+
+ // Current drawn by the motor in Amperes.
+ double current = 4;
+
+ // Motor Temperature in Kelvin.
+ double temperature = 5;
+}
diff --git a/scripts/motor_status.py b/scripts/motor_status.py
new file mode 100644
index 00000000..5567df28
--- /dev/null
+++ b/scripts/motor_status.py
@@ -0,0 +1,94 @@
+#!/usr/bin/env python3
+
+# To run this script set PYTHONPATH environment variable to register custom motor_stats message.
+# Run this in main directory:- export PYTHONPATH=$PYTHONPATH:`pwd`/build/ardupilot_gazebo-msgs_genmsg/python/
+
+# Basic setup for dronecan:-
+# modprobe vcan
+# ip link add dev vcan0 type vcan
+# ip link set up vcan0
+# To check:- ip link show vcan0
+
+
+import time
+import argparse
+import threading
+import dronecan
+from gz.transport13 import Node
+
+
+try:
+ from ardupilot_gazebo.msgs.motor_stats_pb2 import MotorStats
+except ImportError:
+ print("Error: Could not import the MotorStats protobuf message.")
+ print("Please ensure you have compiled MotorStats.proto and the resulting")
+ print("Python module is in your PYTHONPATH.")
+ exit(1)
+
+class MotorStatsSubscriber:
+ def __init__(self, node_id, dronecan_port, model_name, joint_name):
+ self._model_name = model_name
+ self._joint_name = joint_name
+ self._node_id = node_id
+ self._dronecan_port = dronecan_port
+ self._lock = threading.Lock()
+ self._node = Node()
+ self._dronecan_node = None
+ self._initialized = False
+
+
+ print(f"Initializing DroneCAN node {node_id} on '{dronecan_port}'...")
+ try:
+ self._dronecan_node = dronecan.make_node(dronecan_port, node_id=node_id, bitrate=1000000)
+ except Exception as e:
+ print(f"Error initializing DroneCAN node: {e}")
+ print("Please ensure the CAN interface exists and is configured correctly.")
+ return
+
+
+ self._topic = f"/model/{self._model_name}/joint/{self._joint_name}/motor_stats"
+ if self._node.subscribe(MotorStats, self._topic, self.motor_stats_cb):
+ print("Subscribing to type {} on topic [{}]".format(MotorStats, self._topic))
+ self._initialized = True
+ else:
+ print("Error subscribing to topic [{}]".format(self._topic))
+ return
+
+ def motor_stats_cb(self, msg: MotorStats):
+ if not self._initialized or self._dronecan_node is None:
+ return
+
+ try:
+ with self._lock:
+ dronecan_msg = dronecan.uavcan.equipment.esc.Status()
+ dronecan_msg.esc_index = int(msg.motor_id)
+ dronecan_msg.rpm = int(msg.rpm)
+ dronecan_msg.voltage = float(msg.voltage)
+ dronecan_msg.current = float(msg.current)
+ dronecan_msg.temperature = float(msg.temperature)
+
+ self._dronecan_node.broadcast(dronecan_msg)
+ except Exception as e:
+ print(f"Error broadcasting DroneCAN message: {e}")
+
+def main():
+ parser = argparse.ArgumentParser(description="Gazebo to DroneCAN ESC bridge.")
+ parser.add_argument('dronecan_port', type=str, help="CAN interface name (e.g., vcan0, slcan:/dev/ttyACM0).")
+ parser.add_argument('--node-id', type=int, default=100, help="DroneCAN node ID for this bridge.")
+ parser.add_argument('--model-name', type=str, default="iris_with_gimbal", help="Name of the model in Gazebo.")
+ parser.add_argument('--num-motors', type=int, default=4, help="Number of motors on the model.")
+ args = parser.parse_args()
+
+ print(f"Number of motors: {args.num_motors}")
+
+ for i in range(args.num_motors):
+ joint_name = f"iris_with_standoffs::rotor_{i}_joint"
+ MotorStatsSubscriber(args.node_id, args.dronecan_port, args.model_name, joint_name)
+ try:
+ while True:
+ time.sleep(0.1)
+ except KeyboardInterrupt:
+ print("\nShutting down...")
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/MotorPlugin.cc b/src/MotorPlugin.cc
new file mode 100644
index 00000000..55ce18f4
--- /dev/null
+++ b/src/MotorPlugin.cc
@@ -0,0 +1,534 @@
+/*
+ Copyright (C) 2025 ArduPilot.org
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see .
+*/
+
+#include "MotorPlugin.hh"
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "Util.hh"
+
+
+namespace gz {
+namespace sim {
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems {
+
+/// \brief class Control is responsible for controlling a joint
+class Control
+{
+ /// \brief Constructor
+ public: Control() {}
+
+ /// \brief Desctuctor
+ public: ~Control() {}
+
+ /// \brief The PWM channel used to command this control
+ public: int channel = 0;
+
+ /// \brief name of the joint being controlled
+ public: std::string jointName;
+
+ /// \brief battery voltage
+ public: double voltageBat;
+
+ /// \brief speed constant of motor (Kv)
+ public: double speedConstant;
+
+ /// \brief motor internal resistance
+ public: double internalResistance;
+
+ /// \brief dynamic motor resistance while working
+ public: double resistance;
+
+ /// \brief no load current of motor
+ public: double noLoadCurrent;
+
+ /// \brief thermal resistance of the motor
+ public: double thermalResistance;
+
+ /// \brief thermal capacitance of the motor
+ public: double thermalCapacitance;
+
+ /// \brief ambient working temperature
+ public: double ambientTemperature;
+
+ /// \brief Motor temperature
+ public: double temperature;
+
+ /// \brief joint being controlled
+ public: gz::sim::Entity joint;
+
+ /// \brief Publisher for motor stats
+ public: gz::transport::Node::Publisher motorStatusPub;
+};
+
+//////////////////////////////////////////////////
+class MotorPlugin::Impl
+{
+ /// \brief Callback for subscription for Velocity msg.
+ ///
+ /// \param _controlIndex -> Index of the message
+ /// \param _msg -> message itself.
+ /// The command message is a PWM signal.
+ public: void OnPwmMsg(int _controlIndex, const gz::msgs::Double &_msg);
+
+ /// \brief Load motors
+ ///
+ /// \param _sdf -> sdf pointer
+ /// \param _ecm -> entity component manager
+ public: void LoadControlChannels(sdf::ElementPtr _sdf, gz::sim::EntityComponentManager &_ecm);
+
+ /// \brief World occupied by the parent model.
+ public: World world{kNullEntity};
+
+ /// \brief Name of the world entity.
+ public: std::string worldName;
+
+ /// \brief Model entity of Motor Model.
+ public: Model parentModel{kNullEntity};
+
+ /// \brief Name of the model entity.
+ public: std::string parentModelName;
+
+ /// \brief Array of controllers
+ public: std::vector controls;
+
+ /// \brief Array of msg command topics.
+ public: std::vector topics;
+
+ /// \brief Stores target cmd values.
+ public: std::vector pwmValues;
+
+ /// \brief Mutex to protect pwmValues
+ public: std::mutex pwmMutex;
+
+ /// \brief Check to see if the config is valid
+ public: bool validConfig{false};
+
+ /// \brief gz-transport Node to subscribe to velValues data coming from ArduPilotPlugin
+ gz::transport::Node node;
+
+ /// \brief Joint Entity
+ public: Joint joint{kNullEntity};
+};
+
+//////////////////////////////////////////////////
+void MotorPlugin::Impl::OnPwmMsg(int _controlIndex, const gz::msgs::Double &_msg)
+{
+ std::lock_guard lock(this->pwmMutex);
+ // Bounds checking
+ if (_controlIndex >= 0 && _controlIndex < static_cast(this->pwmValues.size()))
+ {
+ this->pwmValues[_controlIndex] = _msg.data();
+ }
+ else
+ {
+ gzwarn << "Invalid control index " << _controlIndex << " for PWM message. Expected [0, "
+ << (this->pwmValues.size() - 1) << "]" << std::endl;
+ }
+}
+
+
+//////////////////////////////////////////////////
+MotorPlugin::~MotorPlugin() = default;
+
+//////////////////////////////////////////////////
+MotorPlugin::MotorPlugin() : impl(std::make_unique())
+{
+}
+
+//////////////////////////////////////////////////
+void MotorPlugin::Configure(
+ const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &)
+{
+
+ // Make a clone so that we can call non-const methods
+ sdf::ElementPtr sdfClone = _sdf->Clone();
+
+ // retrieve world entity
+ this->impl->world = World(
+ _ecm.EntityByComponents(components::World()));
+ if (!this->impl->world.Valid(_ecm))
+ {
+ gzerr << "MotorPlugin - world not found. "
+ "Failed to initialize.\n";
+ return;
+ }
+ this->impl->worldName = this->impl->world.Name(_ecm).value();
+
+ // capture model entity
+ this->impl->parentModel = Model(_entity);
+ if (!this->impl->parentModel.Valid(_ecm))
+ {
+ gzerr << "MotorPlugin should be attached to a model. "
+ "Failed to initialize.\n";
+ return;
+ }
+ this->impl->parentModelName = this->impl->parentModel.Name(_ecm);
+
+ // Load control channel params
+ this->impl->LoadControlChannels(sdfClone, _ecm);
+
+ // Initialize pwmValues vector for safety
+ this->impl->pwmValues.resize(this->impl->controls.size(), 0.0);
+
+ // create components and subscriptions.
+ for (int i = 0; i < this->impl->controls.size(); ++i)
+ {
+ auto &control = this->impl->controls[i];
+
+ // Create joint components
+ if (!_ecm.Component(control.joint))
+ {
+ _ecm.CreateComponent(control.joint, gz::sim::components::JointVelocity({0.0}));
+ }
+ if (!_ecm.Component(control.joint))
+ {
+ _ecm.CreateComponent(control.joint, gz::sim::components::JointForceCmd({0.0}));
+ }
+
+ // Subscriber
+ std::string topic = this->impl->topics[i];
+ this->impl->node.Subscribe(
+ topic,
+ [this, i](const gz::msgs::Double &_msg, const gz::transport::MessageInfo &_info) {
+ this->impl->OnPwmMsg(i, _msg);
+ });
+
+ gzdbg << "MotorPlugin subscribing to PWM messages on [" << topic
+ << "] for control index " << i << std::endl;
+ }
+ this->impl->validConfig = true;
+
+}
+
+/////////////////////////////////////////////////
+void MotorPlugin::Impl::LoadControlChannels(
+ sdf::ElementPtr _sdf,
+ gz::sim::EntityComponentManager &_ecm)
+{
+ // per control channel
+ sdf::ElementPtr controlSdf;
+ if (_sdf->HasElement("control"))
+ {
+ controlSdf = _sdf->GetElement("control");
+ }
+ while (controlSdf)
+ {
+ Control control;
+
+ if (controlSdf->HasAttribute("channel"))
+ {
+ control.channel = atoi(controlSdf->GetAttribute("channel")->GetAsString().c_str());
+ }
+ else
+ {
+ gzwarn << "[" << this->parentModelName << "] "
+ << "id/channel attribute not specified, use order parsed ["
+ << control.channel << "].\n";
+ }
+
+ // parameters
+ if (controlSdf->HasElement("joint_name"))
+ {
+ control.jointName = controlSdf->Get("joint_name");
+ }
+ else
+ {
+ gzerr << "[" << this->parentModelName << "] "
+ << "Please specify a joint_name,"
+ << " where the control channel is attached.\n";
+ }
+
+ // Get the pointer to the joint.
+ control.joint = JointByName(_ecm, this->parentModel.Entity(), control.jointName);
+ if (control.joint == gz::sim::kNullEntity)
+ {
+ gzerr << "Joint [" << control.joint << "] not found in model [" << this->parentModel.Name(_ecm) << "]" << "\n";
+ return;
+ }
+ else
+ {
+ gzmsg << "Got Joint [" << control.joint << "]\n";
+ }
+
+ if (controlSdf->HasElement("voltage_bat"))
+ {
+ control.voltageBat = controlSdf->Get("voltage_bat");
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'voltage_bat'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ if (controlSdf->HasElement("speed_constant"))
+ {
+ control.speedConstant = controlSdf->Get("speed_constant");
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'speed_constant'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ if (controlSdf->HasElement("resistance"))
+ {
+ control.internalResistance = controlSdf->Get("resistance");
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'resistance'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ if (controlSdf->HasElement("no_load_current"))
+ {
+ control.noLoadCurrent = controlSdf->Get("no_load_current");
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'no_load_current'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ if (controlSdf->HasElement("cmd_topic"))
+ {
+ this->topics.push_back(controlSdf->Get("cmd_topic"));
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'cmd_topic'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ if (controlSdf->HasElement("thermal_resistance"))
+ {
+ control.thermalResistance = controlSdf->Get("thermal_resistance");
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'thermal_resistance'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ if (controlSdf->HasElement("thermal_capacitance"))
+ {
+ control.thermalCapacitance = controlSdf->Get("thermal_capacitance");
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'thermal_capacitance'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ if (controlSdf->HasElement("ambient_temperature"))
+ {
+ control.ambientTemperature = controlSdf->Get("ambient_temperature");
+ }
+ else
+ {
+ gzerr << "MotorPlugin requires parameter 'ambient_temperature'. "
+ "Failed to initialize.\n";
+ return;
+ }
+
+ std::string motorStatusTopic = "/model/" + this->parentModelName + "/joint/" + control.jointName + "/motor_stats";
+ control.motorStatusPub = this->node.Advertise(motorStatusTopic);
+
+ this->controls.push_back(control);
+ controlSdf = controlSdf->GetNextElement("control");
+ }
+}
+
+
+//////////////////////////////////////////////////
+void MotorPlugin::PreUpdate(
+ const gz::sim::UpdateInfo &_info,
+ EntityComponentManager &_ecm)
+{
+ GZ_PROFILE("MotorPlugin::PreUpdate");
+ double voltage=0.0;
+ double current=0.0;
+
+ // gzdbg << "This is Dt:- " << dt << "\n";
+ for (size_t i = 0; i < this->impl->controls.size(); ++i)
+ {
+ auto &control = this->impl->controls[i];
+ double pwm = 0.0;
+
+ auto joint_vel_comp = _ecm.Component(control.joint);
+ if (!joint_vel_comp)
+ {
+ gzerr << "JointVelocity component missing for joint [" << control.jointName << "]\n";
+ return;
+ }
+
+ const auto &velocities = joint_vel_comp->Data();
+ if (velocities.empty())
+ {
+ gzwarn << "Empty velocities for joint [" << control.jointName << "]\n";
+ continue;
+ }
+
+ // current joint speed (rad/s)
+ double currSpeed = velocities[0];
+ std::lock_guard lock(this->impl->pwmMutex);
+ {
+ if (this->impl->pwmValues[i] > 1.0 || this->impl->pwmValues[i] < -1.0)
+ {
+ gzerr << "PWM exceeded the input limit for joint [" << control.jointName << "]\n";
+ return;
+ }
+ else
+ {
+ // Deadzone of ±0.02 to prevent motor chattering
+ if (std::abs(this->impl->pwmValues[i]) < 0.02)
+ {
+ pwm = 0.0;
+ }
+ else
+ {
+ pwm = this->impl->pwmValues[i];
+ }
+ }
+ }
+
+ double kv = (control.speedConstant * (2.0 * M_PI)) / 60.0;
+ voltage = control.voltageBat * pwm;
+ double backEmfV = currSpeed / kv ; // Ω/KV
+
+ // R_T = R_0 * (1 + a(T - T_0))
+ // a -> alpha (temperature coefficient for copper is 0.00393)
+ // T_0 -> reference temperature taken as ambient temperature for simplicity
+ control.resistance = control.internalResistance * (1.0 + 0.00393 * (control.temperature - control.ambientTemperature));
+ current = (voltage - backEmfV) / control.resistance;
+
+ double torque = 0.0;
+ if (std::abs(current) > control.noLoadCurrent)
+ {
+ torque = (current - (current > 0 ? 1 : -1) * control.noLoadCurrent) / kv;
+ }
+
+ // Temperature calculation
+ // Parameter calcultion
+ // P_loss = P_resistive + P_friction
+ // P_resistance = I^2 * R = 16.37^2 * 0.115;
+ // P_friction = i_0 * v_m; (no load current and backemf)
+ // so P_loss = 39.38W (at full throttle)
+
+ // R_th = (T - T_amb)/P_loss = (80 - 25)/39.38 = 1.40 deg C
+ // C_th = t/R_th = 300/1.40 = 214.28 (t -> tau (thermal time constant), estimated from datasheet)
+ // temperature eqns
+ // dT/dt = P_loss/C_th - (T - T_amb)/(R_th*C_th)
+ // C_th -> thermal capacitance, r_th -> therma resistance
+ // T -> current motor temperature
+ // T_amb -> ambient temperature
+
+ double p_resistive = std::pow(std::abs(current), 2) * control.resistance;
+ double p_friction = control.noLoadCurrent * std::abs(backEmfV);
+ double p_loss = p_resistive + p_friction;
+ double dt = std::chrono::duration(_info.dt).count();
+ double dT_dt = (p_loss / control.thermalCapacitance) - ((control.temperature - control.ambientTemperature) / (control.thermalResistance * control.thermalCapacitance));
+ control.temperature += dT_dt * dt;
+ if (control.temperature < control.ambientTemperature) {
+ control.temperature = control.ambientTemperature;
+ }
+ double temp_kelvin = control.temperature + 273.15;
+ currSpeed = (currSpeed * 60.0) / (2.0 * M_PI); // rad/s -> rpm
+
+ // Publish motor stats
+ ardupilot_gazebo::msgs::MotorStatus motorStatusMsg;
+ motorStatusMsg.set_motor_id(control.channel);
+ motorStatusMsg.set_rpm(currSpeed);
+ motorStatusMsg.set_voltage(voltage);
+ motorStatusMsg.set_current(current);
+ motorStatusMsg.set_temperature(temp_kelvin);
+ if (!control.motorStatusPub.Publish(motorStatusMsg))
+ {
+ gzerr << "Failed to publish motor status for joint [" << control.jointName << "]\n";
+ }
+
+ // debugging
+ // gzdbg << "Index:- " << i << " PWM:- " << pwm << " Curr Speed:- " << currSpeed << " Torque:- " << torque << " Voltage:- " << voltage << " Current:- " << current << " Temperature:- " << control.temperature << "\n";
+
+ // Apply torque to joint
+ auto jfcComp = _ecm.Component(control.joint);
+ if (jfcComp)
+ {
+ auto &forceCmd = jfcComp->Data();
+ forceCmd[0] = torque;
+ }
+ else
+ {
+ gzerr << "JointForceCmd component missing for joint [" << control.jointName << "]\n";
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////
+
+} // namespace systems
+}
+} // namespace sim
+} // namespace gz
+
+GZ_ADD_PLUGIN(
+ gz::sim::systems::MotorPlugin,
+ gz::sim::System,
+ gz::sim::systems::MotorPlugin::ISystemConfigure,
+ gz::sim::systems::MotorPlugin::ISystemPreUpdate)
+
+GZ_ADD_PLUGIN_ALIAS(
+ gz::sim::systems::MotorPlugin,
+ "MotorPlugin")
diff --git a/worlds/motor_model.sdf b/worlds/motor_model.sdf
new file mode 100644
index 00000000..62dad495
--- /dev/null
+++ b/worlds/motor_model.sdf
@@ -0,0 +1,188 @@
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+ 0 0 0 0 0 0
+
+ 0.0 0.0 0.0 0 0 0
+
+
+ 2.501
+ 0
+ 0
+ 2.501
+ 0
+ 5
+
+ 120.0
+
+
+ 0.0 0.0 0.0 0 0 0
+
+
+ 0.5 0.5 0.01
+
+
+
+
+ 0.0 0.0 0.0 0 0 0
+
+
+ 0.5 0.5 0.01
+
+
+
+
+
+
+ 0.0 0.0 0.1 0 0 0
+
+
+ 0.0001
+ 0
+ 0
+ 0.0001
+ 0
+ 0.0001
+
+ 0.2
+
+
+
+
+ 0.025
+ 0.05
+
+
+
+ 0.3 0.3 0.3 1
+ 0.7 0.7 0.7 1
+
+
+
+
+
+ 0.025
+ 0.05
+
+
+
+
+
+
+
+ 0.0 0.0 0.13 0 0 0
+
+
+ 0.00001
+ 0
+ 0
+ 0.00025
+ 0
+ 0.00025
+
+ 0.05
+
+
+
+
+ model://iris_with_standoffs/meshes/iris_prop_ccw.dae
+ 1 1 1
+
+
+
+ 0.1 0.1 0.1 1
+ 0.2 0.2 0.2 1
+
+
+
+
+
+ 0.128
+ 0.005
+
+
+
+
+
+
+ world
+ base_link
+
+
+
+
+ base_link
+ motor_housing
+
+
+
+
+ 0 0 0 0 0 0
+ motor_housing
+ propeller
+
+ 0 0 1
+
+ 1.0e-5
+
+
+ -1e+16
+ 1e+16
+
+
+
+
+
+ 0.3
+ 1.4
+ 4.2500
+ 0.10
+ 0.0
+ -0.025
+ 0.0
+ 0.0
+ 0.002
+ 1.2041
+ 0.084 0 0
+ 0 1 0
+ 0 0 1
+ propeller
+
+
+
+
+
+
+
+
+
+ joint_0
+ 16
+ 920
+ 0.115
+ 0.8
+ joint_0_topic
+ 1.40
+ 214.28
+ 25.0
+
+
+
+
+
\ No newline at end of file