added motor-plugin and DroneCAN ESC telemetry script by Bhajneet-Singh-Bedi · Pull Request #146 · ArduPilot/ardupilot_gazebo

Bhajneet-Singh-Bedi · 2025-08-27T17:58:24Z

Description

This PR introduces a gazebo plugin of an electro-mechanical model of a motor. This plugin enhances the motor control in a model.

The electro-mechanical model of a motor has been referred from this document:- MIT QPROP
It also contains a script to publish motor variables (rpm, current, voltage, temperature) on a DroneCAN ESC type telemetry topic.
The plugin has been tested on iris quadcopter.

Details

The ArduPilotPlugin in "COMMAND" mode sends pwm data on a topic when multiplier is set to one.
The plugin then calculates the desired torque to be given to the gazebo joint. The plugin takes current velocity of the joint as a feedback for torque calculation.
This addition also makes the joint back driven. In the previous implementation, the PID controller takes desired velocity as input and drives the motor to reach that velocity. This conflicts to make the joint self driven.
The new implementation controls the torque using electro-mechanical motor equations based on the PWM input which comes from ArduPilot SITL and ArduPilot SITL takes Flight Dynamics as a feedback to update PWM signals, making the joint self-driven when zero input is provided which was not supported in previous implementation as zero PWM would drive the joint to achieve zero velocity.
Their is a custom gazebo message which stores motor status data (rpm, current, voltage and temperature) which publishes on a topic.
The published topics are then subscribed by a python script which publishes the motor status data on DroneCAN ESC type telemetry topic. This could act as a better approach for communication between ArduPilot SITL and gazebo. More details are mentioned in Sending Gazebo sensor data to ArduPilot using DroneCAN blog for choosing approach.

How to use this plugin?

Include this in the sdf file:-

<plugin filename="MotorPlugin" name="MotorPlugin">
        <control channel="0">
          <joint_name>joint_0</joint_name> <!--name of the joint -->
          <voltage_bat>16.4</voltage_bat> <!-- Battery voltage in Volts -->
          <speed_constant>920</speed_constant> <!-- Motor KV rating in RPM/Volt -->
          <resistance>0.115</resistance> <!-- Winding resistance in Ohms -->
          <no_load_current>0.8</no_load_current> <!-- No-load current in Amps -->
          <cmd_topic>joint_0</cmd_topic> <!-- Output topic for motor status -->
          <thermal_resistance>1.4</thermal_resistance> <!-- °C/W, estimated for a motor --> 
          <thermal_capacitance>214.28</thermal_capacitance> <!-- °C/W, estimated for a motor -->
          <ambient_temperature>25.0</ambient_temperature> <!-- Normal room temperature in Celsius-->
        </control>
</plugin>

You can keep adding more channels and sub parameters based on number of motors.
The parameters of the above given example motor are taken from AIR 2216 920KVmotor's datasheet which is being used in iris quadcopter.

For calculating thermal parameters:-

Power loss calculation:-

P_loss = P_resistive + P_friction
P_resistive = I^2 * R = 16.37^2 * 0.115
P_friction  = i_0 * v_m   (no-load current * back-EMF)
P_loss = 39.38 W   (at full throttle)

Thermal resistance

R_th = (T - T_amb) / P_loss
= (80 - 25) / 39.38
= 1.40 °C/W
(T = temperature at full throttle, given in datasheet)

Thermal capacitance

C_th = τ / R_th
     = 300 / 1.40
     = 214.28 J/°C
(τ = thermal time constant, estimated from datasheet)

This PR includes a model_model.sdf in worlds/ folder which includes this plugin. When you run that using
- Terminal 1: gz sim -v4 -r motor_model.sdf
- Terminal 2: gz topic -t /joint_0_topic -m gz.msgs.Double -p 'data: 0.3
You will see a "/model/motor_model_demo/joint/joint_0/motor_stats" topic being published for that joint.
To see the logs of the topics, go to build directory in ardupilot_gazebo/ workspace and run:
export GZ_DESCRIPTOR_PATH=`pwd
This will set the GZ_DESCRIPTOR_PATH and you'll be able to see the output in terminal.
To run the python script which subscribes to these motor stats topic and publishes DroneCAN ESC type telemetry data. First run this:
- export PYTHONPATH=$PYTHONPATH:`pwd`/build/ardupilot_gazebo-msgs_genmsg/python/ in ardupilot_gazebo/ workspace.
- python3 scripts/motor_status.py from the same ardupilot_gazebo/ workspace.

Future works

The current model only supports BLDC motors. The model based approach can be extended for servo motors as well which are used by models like gimbals.

srmainwaring · 2025-08-27T19:25:16Z

+/*
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/


Should be LGPLv3. The original ArduPilotPlugin code was Apache, but new works should match the repo license (https://github.com/ArduPilot/ardupilot_gazebo/blob/main/LICENSE.md). See https://github.com/ArduPilot/ardupilot_gazebo/blob/main/include/SocketUDP.hh for example text to include in source files.

srmainwaring · 2025-08-27T19:27:15Z

+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems {
+
+


Include a doc string describing the plugins purpose and SDF parameters. See for example: https://github.com/ArduPilot/ardupilot_gazebo/blob/main/include/GstCameraPlugin.hh

srmainwaring · 2025-08-27T19:28:08Z

+	/// \brief Load control channels
+	private: void LoadControlChannels(
+		sdf::ElementPtr _sdf,
+		gz::sim::EntityComponentManager &_ecm);


Can be in the private Impl class as this function is not called by the Gazebo system.

srmainwaring · 2025-08-27T19:29:59Z

+/*
+   Copyright (C) 2025 ArduPilot.org


Use this license text in MotorPlugin.hh as well.

srmainwaring · 2025-08-27T19:31:02Z

+	public: double speedConstant;
+
+	/// \brief motor internal resistance  
+	public: double internal_resistance;


Suggested change

public: double internal_resistance;

public: double internalResistance;

srmainwaring · 2025-08-27T19:31:30Z

+	public: double offset;
+
+	/// \brief thermal resistance of the motor
+	public: double thermal_resistance;


Suggested change

public: double thermal_resistance;

public: double thermalResistance;

srmainwaring · 2025-08-27T19:31:47Z

+	public: double thermal_resistance;
+
+	/// \brief thermal capacitance of the motor
+	public: double thermal_capacitance;


Suggested change

public: double thermal_capacitance;

public: double thermalCapacitance;

srmainwaring · 2025-08-27T19:32:00Z

+	public: double thermal_capacitance;
+
+	/// \brief ambient working temperature 
+	public: double ambient_temperature;


Suggested change

public: double ambient_temperature;

public: double ambientTemperature;

srmainwaring · 2025-08-27T19:32:39Z

+	/// \param controlIndex -> Index of the message 
+	/// \param _msg -> message itself.
+  	/// The command message is a target Velocity.
+	public: void OnVelMsg(int controlIndex, const gz::msgs::Double &_msg);


Suggested change

public: void OnVelMsg(int controlIndex, const gz::msgs::Double &_msg);

public: void OnVelMsg(int _controlIndex, const gz::msgs::Double &_msg);

srmainwaring · 2025-08-27T19:33:13Z

+};
+
+//////////////////////////////////////////////////
+void MotorPlugin::Impl::OnVelMsg(int controlIndex, const gz::msgs::Double &_msg)


Suggested change

void MotorPlugin::Impl::OnVelMsg(int controlIndex, const gz::msgs::Double &_msg)

void MotorPlugin::Impl::OnVelMsg(int _controlIndex, const gz::msgs::Double &_msg)

srmainwaring · 2025-08-27T19:35:49Z

+}
+
+/////////////////////////////////////////////////
+void MotorPlugin::LoadControlChannels(


Suggested change

void MotorPlugin::LoadControlChannels(

void MotorPlugin::Impl::LoadControlChannels(

And move declaration from MotorPlugin.hh.

srmainwaring · 2025-08-27T19:42:14Z

@@ -215,14 +215,15 @@
        <offset>0</offset>


I'd prefer to see the changes to the models split into a separate PR. That way we can proceed with including the motor plugin without altering the existing models and their behaviour. When we're more comfortable with the plugin we can consider migrating the examples over to it.

In the meanwhile a small test model (one of the iris motors and prop fixed to a thrust stand would be good). That would allow testing and demonstrations. It could even have a force torque sensor plugin included in the joint for additional feedback.

srmainwaring · 2025-08-27T19:46:36Z

+  // Current drawn by the motor in Amperes.
+  double current = 4;
+
+  // Motor Temperature in Celsius.


Temperature in Kelvin.

srmainwaring · 2025-08-27T19:48:37Z

+package ardupilot_gazebo.msgs;
+
+// Message to hold the status of a single motor.
+message MotorStats {


Suggested change

message MotorStats {

message MotorStatus {

MotorStatus better aligns with the DroneCAN message names (uavcan.equipment.esc.Status and uavcan.equipment.esc.StatusExtended). See for example: https://github.com/dronecan/DSDL/blob/master/uavcan/equipment/esc/1034.Status.uavcan

srmainwaring · 2025-08-27T19:51:44Z

+# Basic setup for dronecan:-
+# sudo modprobe vcan
+# sudo ip link add dev vcan0 type vcan
+# sudo ip link set up vcan0
+# To check:- ip link show vcan0


We should be able to run this without needing to use sudo?

For example:

https://github.com/srmainwaring/ardupilot_gazebo-1/blob/wips/wip-dronecan/scripts/dronecan_sensor.py

(that script following the examples in the dronecan repo and tutorials)

srmainwaring · 2025-08-27T19:55:11Z

+
+GZ_ADD_PLUGIN_ALIAS(
+    gz::sim::systems::MotorPlugin,
+    "MotorPlugin")


Add blank line at end.

srmainwaring · 2025-08-27T20:03:18Z

+	/// \brief Array of msg command topics.
+	public: std::vector<std::string> topics;
+
+	/// \brief Stores target velocity values.


I was not expecting to see a target velocity here. My understanding of how the motor model would work after reviewing the JSBSim implementation (https://github.com/JSBSim-Team/jsbsim/blob/b005a91d3151624eb26ae5595570bd188ef49702/src/models/propulsion/FGBrushLessDCMotor.cpp) using the same paper http://web.mit.edu/drela/Public/web/qprop/motor1_theory.pdf, is that the input to the model is a throttle with range [0, 1].

The input pwm [1000, 2000] scaled with multiplier 1 and offset 0 would provide the mapping.

The maximum power output is then determined by the max battery voltage (throttle = 1), and the subsequent motor velocity a result of the prop details and motor Kv and friction etc. This was summarised here: https://discord.com/channels/@me/1376945277940011098/1406220672027590696

…ded motor_model.sdf, corrected temperature units

added motor-plugin and DroneCAN ESC telemetry

2816f02

srmainwaring reviewed Aug 27, 2025

View reviewed changes

Bhajneet-Singh-Bedi force-pushed the prs/pr-motor-plugin-DroneCAN branch from c3ded1c to db542c8 Compare August 29, 2025 01:38

formatted files, corrected MotorPlugin, restored iris_with_gimbal, ad…

d7b7960

…ded motor_model.sdf, corrected temperature units

Bhajneet-Singh-Bedi force-pushed the prs/pr-motor-plugin-DroneCAN branch from db542c8 to d7b7960 Compare August 29, 2025 01:53

added deadzone and clamped throttle input

04a3e33

Bhajneet-Singh-Bedi mentioned this pull request Aug 29, 2025

Add motor plugin #141

Closed

8 tasks

		inline namespace GZ_SIM_VERSION_NAMESPACE {
		namespace systems {

	public: double internal_resistance;
	public: double internalResistance;

	public: double thermal_resistance;
	public: double thermalResistance;

	public: double thermal_capacitance;
	public: double thermalCapacitance;

	public: double ambient_temperature;
	public: double ambientTemperature;

	public: void OnVelMsg(int controlIndex, const gz::msgs::Double &_msg);
	public: void OnVelMsg(int _controlIndex, const gz::msgs::Double &_msg);

	void MotorPlugin::Impl::OnVelMsg(int controlIndex, const gz::msgs::Double &_msg)
	void MotorPlugin::Impl::OnVelMsg(int _controlIndex, const gz::msgs::Double &_msg)

	void MotorPlugin::LoadControlChannels(
	void MotorPlugin::Impl::LoadControlChannels(

Uh oh!

Conversation

Bhajneet-Singh-Bedi commented Aug 27, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Details

How to use this plugin?

Future works

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Bhajneet-Singh-Bedi commented Aug 27, 2025 •

edited

Loading