Skip to content

Custom Message

Jump to bottom
altwaitan edited this page Dec 25, 2022 · 2 revisions

Custom Message

This repo shows an example for declaring a erl_quad_states message in PX4 and sends it to ROS.

ROS Packages

  1. Add ERLQuadStates.msg in ~/catkin_ws/src/mavros/mavros_msgs/msg 
# ERL Quad States 
uint64 timestamp		        # time since system start (microseconds)
float32[3] position             	# position x, y, z in world-frame 
float32[4] orientation          	# orientation in quaternion [w, x, y, z]
float32[3] velocity             	# velocity in body-frame 
float32[3] angular_velocity     	# angular velocity in body-frame 
float32[4] controls	            	# thrust, torque x, torque y, torque z 
float32[4] controls_scaled	    	# thrust, torque x, torque y, torque z (after battery scaling)
  1. Create erl_quad_states.cpp in ~/catkin_ws/src/mavros/mavros_extras/src/plugins 
#include <mavros/mavros_plugin.h>
#include <mavros_msgs/ERLQuadStates.h>

namespace mavros {
namespace extra_plugins {

class ERLQuadStatesPlugin : public plugin::PluginBase {
public:
	ERLQuadStatesPlugin() : PluginBase(),
		erl_quad_states_nh("~erl")
	{ }

	void initialize(UAS &uas_) override
	{
		PluginBase::initialize(uas_);

		erl_quad_states_pub = erl_quad_states_nh.advertise<mavros_msgs::ERLQuadStates>("erl_quad_states", 10);
	}

	Subscriptions get_subscriptions() override
	{
		return {
			make_handler(&ERLQuadStatesPlugin::handle_erl_quad_states)
		};
	}

private:
	ros::NodeHandle erl_quad_states_nh;

	ros::Publisher erl_quad_states_pub;


	void handle_erl_quad_states(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ERL_QUAD_STATES &erl_quad_states_msg)
	{
        auto erl_quad_states = boost::make_shared<mavros_msgs::ERLQuadStates>();
        erl_quad_states->timestamp = erl_quad_states_msg.timestamp; 
	erl_quad_states->position[0] = erl_quad_states_msg.position[0];
	erl_quad_states->position[1] = erl_quad_states_msg.position[1];
	erl_quad_states->position[2] = erl_quad_states_msg.position[2];
	erl_quad_states->orientation[0] = erl_quad_states_msg.orientation[0];
	erl_quad_states->orientation[1] = erl_quad_states_msg.orientation[1];
	erl_quad_states->orientation[2] = erl_quad_states_msg.orientation[2];
	erl_quad_states->orientation[3] = erl_quad_states_msg.orientation[3];
	erl_quad_states->velocity[0] = erl_quad_states_msg.velocity[0];
	erl_quad_states->velocity[1] = erl_quad_states_msg.velocity[1];
	erl_quad_states->velocity[2] = erl_quad_states_msg.velocity[2];
	erl_quad_states->angular_velocity[0] = erl_quad_states_msg.angular_velocity[0];
	erl_quad_states->angular_velocity[1] = erl_quad_states_msg.angular_velocity[1];
	erl_quad_states->angular_velocity[2] = erl_quad_states_msg.angular_velocity[2];
        erl_quad_states->controls[0] = erl_quad_states_msg.controls[0];
        erl_quad_states->controls[1] = erl_quad_states_msg.controls[1];
        erl_quad_states->controls[2] = erl_quad_states_msg.controls[2];
        erl_quad_states->controls[3] = erl_quad_states_msg.controls[3];
	erl_quad_states->controls_scaled[0] = erl_quad_states_msg.controls_scaled[0];
        erl_quad_states->controls_scaled[1] = erl_quad_states_msg.controls_scaled[1];
        erl_quad_states->controls_scaled[2] = erl_quad_states_msg.controls_scaled[2];
        erl_quad_states->controls_scaled[3] = erl_quad_states_msg.controls_scaled[3];

        //! Publish the data
        erl_quad_states_pub.publish(erl_quad_states);
	}
};
}	// namespace extra_plugins
}	// namespace mavros

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::ERLQuadStatesPlugin, mavros::plugin::PluginBase)
  1. Edit ~/catkin_ws/src/mavros/mavros_extras/CMakelists.txt 
add_library(mavros_extras
	src/plugins/debug_value.cpp
  	src/plugins/distance_sensor.cpp
	...
	src/plugins/erl_quad_states.cpp
  1. Edit ~/catkin_ws/src/mavros/mavros_extras/mavros_plugins.xml 
<class name="erl_quad_states" type="mavros::extra_plugins::ERLQuadStatesPlugin" base_class_type="mavros::plugin::PluginBase">
      <description>Publish ERL States to ROS.</description>
</class>
  1. Edit ~/catkin_ws/src/mavros/mavros_msgs/CMakelists.txt 
add_message_files(
	OnboardComputerStatus.msg
	DebugValue.msg
	...
	ERLQuadStates.msg
  1. Edit ~/catkin_ws/src/mavlink/message_definitions/v1.0/common.xml 
<!-- The message ids 369: ERL Quadrotor States-->
<message id="369" name="ERL_QUAD_STATES">
      <description>This message sends quadrotor states and controls</description>
      <field type="uint64_t" name="timestamp">time since system start (microseconds)</field>
      <field type="float[3]" name="position">position</field>
      <field type="float[4]" name="orientation">orientation in quaternion</field>
      <field type="float[3]" name="velocity">velocity</field>
      <field type="float[3]" name="angular_velocity">angular velocity</field>
      <field type="float[4]" name="controls">thrust and torques</field>
      <field type="float[4]" name="controls_scaled">thrust and torques after battery scaling</field>
</message>

PX4 Firmware

  1. Clone PX4 Firmware
$ cd ~ 
$ git clone https://github.com/PX4/PX4-Autopilot.git
$ cd PX4-Autopilot
$ git checkout e0f016c2b3db160284bbaaadaf70bef84592d81e
  1. Create uORB message erl_quad_states.msg and add it to PX4-Autopilot/msg 
# ERL Quad States 
uint64 timestamp		        # time since system start (microseconds)
float32[3] position             	# position x, y, z in world-frame 
float32[4] orientation          	# orientation in quaternion [w, x, y, z]
float32[3] velocity             	# velocity in body-frame 
float32[3] angular_velocity     	# angular velocity in body-frame 
float32[4] controls	            	# thrust, torque x, torque y, torque z 
float32[4] controls_scaled	    	# thrust, torque x, torque y, torque z (after battery scaling)
  1. Add the message erl_quad_states.msg to PX4-Autopilot/msg/CMakeLists.txt 
set(msg_files
	ekf2_innovations.msg
	ekf2_timestamps.msg
	ekf_gps_drift.msg
	ekf_gps_position.msg
	...
  	erl_quad_states.msg
)
  1. Build target for Pixhawk 2 (Cube Black) (FMUv3) 
$ cd ~/PX4-Autopilot
$ make px4_fmu-v3_default
  1. Edit common.xml in ~/PX4-Autopilot/mavlink/include/mavlink/v2.0/message_definitions 
<message id="365" name="STATUSTEXT_LONG">
      <description>Status text message (use only for important status and error messages). The full message payload can be used for status text, but we recommend that updates be kept concise. Note: The message is intended as a less restrictive replacement for STATUSTEXT.</description>
      <field type="uint8_t" name="severity" enum="MAV_SEVERITY">Severity of status. Relies on the definitions within RFC-5424.</field>
      <field type="char[254]" name="text">Status text message, without null termination character.</field>
    </message>

...
<!-- The message ids 369: ERL Quadrotor States -->
<message id="369" name="ERL_QUAD_STATES">
	<description>This message sends quadrotor states and controls</description>
	<field type="uint64_t" name="timestamp">time since system start (microseconds)</field>
	<field type="float[3]" name="position">position</field>
	<field type="float[4]" name="orientation">orientation in quaternion</field>
	<field type="float[3]" name="velocity">velocity</field>
	<field type="float[3]" name="angular_velocity">angular velocity</field>
	<field type="float[4]" name="controls">thrust and torques</field>
	<field type="float[4]" name="controls_scaled">thrust and torques after battery scaling</field>
</message>

  1. Delete both common and standard folders in ~/PX4-Autopilot/mavlink/include/mavlink/v2.0/

  2. Generate MAVLink files by the following:

$ cd ~/catkin_ws/src/mavlink/
$ python mavgenerate.py

and a pop-up window will appear

xml: ~/PX4-Autopilot/mavlink/include/mavlink/v2.0/message_definitions/standard.xml 
out: ~/PX4-Autopilot/mavlink/include/mavlink/v2.0
Language: C
Version: 2.0 
Validate
  1. Go to ~/PX4-Autopilot/.gitmodules and delete
[submodule "mavlink/include/mavlink/v2.0"]
	path = mavlink/include/mavlink/v2.0
	url = https://github.com/mavlink/c_library_v2.git
	branch = master
  1. Edit mavlink_messages.cpp in ~/PX4-Autopilot/src/modules/mavlink
  • Add the header file
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
...
#include <uORB/topics/erl_quad_states.h>
  • Add message class
class MavlinkStreamERLQuadStates : public MavlinkStream
{
public:
    const char *get_name() const
    {
        return MavlinkStreamERLQuadStates::get_name_static();
    }
    static const char *get_name_static()
    {
        return "ERL_QUAD_STATES";
    }
    static uint16_t get_id_static()
    {
        return MAVLINK_MSG_ID_ERL_QUAD_STATES;
    }
    uint16_t get_id()
    {
        return get_id_static();
    }
    static MavlinkStream *new_instance(Mavlink *mavlink)
    {
        return new MavlinkStreamERLQuadStates(mavlink);
    }
    unsigned get_size()
    {
        return MAVLINK_MSG_ID_ERL_QUAD_STATES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
    }

private:
    uORB::Subscription _sub{ORB_ID(erl_quad_states)};

    /* do not allow top copying this class */
    MavlinkStreamERLQuadStates(MavlinkStreamERLQuadStates &);
    MavlinkStreamERLQuadStates& operator = (const MavlinkStreamERLQuadStates &);

protected:
    explicit MavlinkStreamERLQuadStates(Mavlink *mavlink) : MavlinkStream(mavlink)
    {}

    bool send(const hrt_abstime t) override
    {
        struct erl_quad_states_s _erl_quad_states;    

        if (_sub.update(&_erl_quad_states)) {
		 mavlink_erl_quad_states_t _msg_erl_quad_states;  

		_msg_erl_quad_states.timestamp = _erl_quad_states.timestamp;
		_msg_erl_quad_states.position[0] = _erl_quad_states.position[0];
		_msg_erl_quad_states.position[1] = _erl_quad_states.position[1];
		_msg_erl_quad_states.position[2] = _erl_quad_states.position[2];
		_msg_erl_quad_states.orientation[0] = _erl_quad_states.orientation[0];
		_msg_erl_quad_states.orientation[1] = _erl_quad_states.orientation[1];
		_msg_erl_quad_states.orientation[2] = _erl_quad_states.orientation[2];
		_msg_erl_quad_states.orientation[3] = _erl_quad_states.orientation[3];
		_msg_erl_quad_states.velocity[0] = _erl_quad_states.velocity[0];
		_msg_erl_quad_states.velocity[1] = _erl_quad_states.velocity[1];
		_msg_erl_quad_states.velocity[2] = _erl_quad_states.velocity[2];
		_msg_erl_quad_states.angular_velocity[0] = _erl_quad_states.angular_velocity[0];
		_msg_erl_quad_states.angular_velocity[1] = _erl_quad_states.angular_velocity[1];
		_msg_erl_quad_states.angular_velocity[2] = _erl_quad_states.angular_velocity[2];
            	_msg_erl_quad_states.controls[0]  = _erl_quad_states.controls[0];
		_msg_erl_quad_states.controls[1]  = _erl_quad_states.controls[1];
		_msg_erl_quad_states.controls[2]  = _erl_quad_states.controls[2];
		_msg_erl_quad_states.controls[3]  = _erl_quad_states.controls[3];
		_msg_erl_quad_states.controls_scaled[0]  = _erl_quad_states.controls_scaled[0];
		_msg_erl_quad_states.controls_scaled[1]  = _erl_quad_states.controls_scaled[1];
		_msg_erl_quad_states.controls_scaled[2]  = _erl_quad_states.controls_scaled[2];
		_msg_erl_quad_states.controls_scaled[3]  = _erl_quad_states.controls_scaled[3];

            	mavlink_msg_erl_quad_states_send_struct(_mavlink->get_channel(), &_msg_erl_quad_states);
            
            	return true;
        }
        return false;
    }
};
  • Add to streams_list[] 
StreamListItem(&MavlinkStreamERLQuadStates::new_instance, &MavlinkStreamERLQuadStates::get_name_static, &MavlinkStreamERLQuadStates::get_id_static)
  1. Edit mavlink_main.cpp in ~/PX4-Autopilot/src/modules/mavlink 
switch (_mode) {
	case MAVLINK_MODE_NORMAL:
	configure_stream_local("ADSB_VEHICLE", unlimited_rate);
	...
	configure_stream_local("ERL_QUAD_STATES", 100.0f);

for all cases

  1. Go to PX4-Autopilot/src/modules/mc_att_control/mc_att_control.hpp 
#include <uORB/topics/erl_quad_states.h> 
#include <uORB/topics/vehicle_local_position.h>

uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};	/**< vehicle local position */
uORB::Publication<erl_quad_states_s>	_erl_quad_states_pub{ORB_ID(erl_quad_states)};

struct vehicle_local_position_s _local_pos{};			/**< vehicle local position */
struct erl_quad_states_s _erl_quad_states {}; 			/**< erl quad states */
  1. Go to PX4-Autopilot/src/modules/mc_att_control/mc_att_control_main.cpp 
void
MulticopterAttitudeControl::publish_actuator_controls()
{
	_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
	_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
	_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
	_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
	_actuators.control[7] = (float)_landing_gear.landing_gear;
	// note: _actuators.timestamp_sample is set in MulticopterAttitudeControl::Run()
	_actuators.timestamp = hrt_absolute_time();

	// ERL Quadrotor States 
	_erl_quad_states.timestamp = _actuators.timestamp; 
	_erl_quad_states.controls[0] = _actuators.control[3];
	_erl_quad_states.controls[1] = _actuators.control[0];
	_erl_quad_states.controls[2] = _actuators.control[1];
	_erl_quad_states.controls[3] = _actuators.control[2];

	/* scale effort by battery status */
	if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) {
		for (int i = 0; i < 4; i++) {
			_actuators.control[i] *= _battery_status.scale;
		}
	}

	// ERL Quadrotor States 
	_erl_quad_states.controls_scaled[0] = _actuators.control[3];
	_erl_quad_states.controls_scaled[1] = _actuators.control[0];
	_erl_quad_states.controls_scaled[2] = _actuators.control[1];
	_erl_quad_states.controls_scaled[3] = _actuators.control[2];

	_erl_quad_states_pub.publish(_erl_quad_states);

	if (!_actuators_0_circuit_breaker_enabled) {
		orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT);
	}
}

void
MulticopterAttitudeControl::Run()
{
	if (should_exit()) {
		_vehicle_angular_velocity_sub.unregisterCallback();
		exit_and_cleanup();
		return;
	}

	perf_begin(_loop_perf);

	/* run controller on gyro changes */
	vehicle_angular_velocity_s angular_velocity;

	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
		const hrt_abstime now = hrt_absolute_time();

		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
		const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
		_last_run = now;

		const Vector3f rates{angular_velocity.xyz};

		_actuators.timestamp_sample = angular_velocity.timestamp_sample;

		// ERL Quadrotor States 
		_local_pos_sub.update(&_local_pos); 
		_erl_quad_states.position[0] = _local_pos.x;
		_erl_quad_states.position[1] = _local_pos.y;
		_erl_quad_states.position[2] = _local_pos.z; 
		_erl_quad_states.orientation[0] = _v_att.q[0];
		_erl_quad_states.orientation[1] = _v_att.q[1];
		_erl_quad_states.orientation[2] = _v_att.q[2]; 
		_erl_quad_states.orientation[3] = _v_att.q[3]; 
		_erl_quad_states.velocity[0] = _local_pos.vx;
		_erl_quad_states.velocity[1] = _local_pos.vy;
		_erl_quad_states.velocity[2] = _local_pos.vz; 
		_erl_quad_states.angular_velocity[0] = angular_velocity.xyz[0];
		_erl_quad_states.angular_velocity[1] = angular_velocity.xyz[1];
		_erl_quad_states.angular_velocity[2] = angular_velocity.xyz[2]; 


		/* run the rate controller immediately after a gyro update */
		if (_v_control_mode.flag_control_rates_enabled) {
			control_attitude_rates(dt, rates);

			publish_actuator_controls();
			publish_rate_controller_status();
		}

		/* check for updates in other topics */
		_v_control_mode_sub.update(&_v_control_mode);
		_battery_status_sub.update(&_battery_status);
		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
		_landing_gear_sub.update(&_landing_gear);
		vehicle_status_poll();
		vehicle_motor_limits_poll();
		const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
		const bool attitude_updated = vehicle_attitude_poll();

		_attitude_dt += dt;

		/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
			* or roll (yaw can rotate 360 in normal att control). If both are true don't
			* even bother running the attitude controllers */
		if (_v_control_mode.flag_control_rattitude_enabled) {
			_v_control_mode.flag_control_attitude_enabled =
				fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() &&
				fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get();
		}

		bool attitude_setpoint_generated = false;

		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
					 && !_vehicle_status.in_transition_mode;

		// vehicle is a tailsitter in transition mode
		const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;

		bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);


		if (run_att_ctrl) {
			if (attitude_updated) {
				// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
				if (_v_control_mode.flag_control_manual_enabled &&
				    !_v_control_mode.flag_control_altitude_enabled &&
				    !_v_control_mode.flag_control_velocity_enabled &&
				    !_v_control_mode.flag_control_position_enabled) {
					generate_attitude_setpoint(_attitude_dt, _reset_yaw_sp);
					attitude_setpoint_generated = true;
				}

				control_attitude();

				if (_v_control_mode.flag_control_yawrate_override_enabled) {
					/* Yaw rate override enabled, overwrite the yaw setpoint */
					_v_rates_sp_sub.update(&_v_rates_sp);
					const auto yawrate_reference = _v_rates_sp.yaw;
					_rates_sp(2) = yawrate_reference;
				}

				publish_rates_setpoint();
			}

		} else {
			/* attitude controller disabled, poll rates setpoint topic */
			if (_v_control_mode.flag_control_manual_enabled && is_hovering) {
				if (manual_control_updated) {
					/* manual rates control - ACRO mode */
					Vector3f man_rate_sp(
						math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
						math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
						math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get()));
					_rates_sp = man_rate_sp.emult(_acro_rate_max);
					_thrust_sp = _manual_control_sp.z;
					publish_rates_setpoint();
				}

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_rates_sp_sub.update(&_v_rates_sp)) {
					_rates_sp(0) = _v_rates_sp.roll;
					_rates_sp(1) = _v_rates_sp.pitch;
					_rates_sp(2) = _v_rates_sp.yaw;
					_thrust_sp = -_v_rates_sp.thrust_body[2];
				}
			}
		}

		if (_v_control_mode.flag_control_termination_enabled) {
			if (!_vehicle_status.is_vtol) {
				_rates_sp.zero();
				_rate_control.resetIntegral();
				_thrust_sp = 0.0f;
				_att_control.zero();
				publish_actuator_controls();
			}
		}

		if (attitude_updated) {
			// reset yaw setpoint during transitions, tailsitter.cpp generates
			// attitude setpoint for the transition
			_reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
					_vehicle_land_detected.landed ||
					(_vehicle_status.is_vtol && _vehicle_status.in_transition_mode);

			_attitude_dt = 0.f;
		}

		/* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */
		if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) {
			_dt_accumulator += dt;
			++_loop_counter;

			if (_dt_accumulator > 1.f) {
				const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
				_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
				_dt_accumulator = 0;
				_loop_counter = 0;
				_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true);
			}
		}

		parameter_update_poll();
	}

	perf_end(_loop_perf);
}
  1. Build firmware
$ cd ~/PX4-Autopilot
$ make px4_fmu-v3_default
Clone this wiki locally