MAVLink Streams: added collision stream

msg/CMakeLists.txt

@@ -122,6 +122,7 @@ set(msg_file_names
 	wind_estimate.msg
 	vehicle_roi.msg
 	mount_status.msg
+	collision_report.msg
 	)
 
 # Get absolute paths

msg/collision_report.msg

@@ -0,0 +1,7 @@
+uint8 src
+uint32 id
+uint8 action
+uint8 threat_level
+float32 time_to_minimum_delta
+float32 altitude_minimum_delta
+float32 horizontal_minimum_delta

src/modules/mavlink/mavlink_main.cpp

@@ -1970,6 +1970,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ALTITUDE", 1.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("ADSB_VEHICLE", 2.0f);
+		configure_stream("COLLISION", 2.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		configure_stream("OPTICAL_FLOW_RAD", 1.0f);
 		configure_stream("VISION_POSITION_NED", 1.0f);
@@ -1995,6 +1996,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ALTITUDE", 10.0f);
 		configure_stream("GPS_RAW_INT", 5.0f);
 		configure_stream("ADSB_VEHICLE", 10.0f);
+		configure_stream("COLLISION", 10.0f);
 		configure_stream("DISTANCE_SENSOR", 10.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VISION_POSITION_NED", 10.0f);
@@ -2050,6 +2052,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ALTITUDE", 10.0f);
 		configure_stream("GPS_RAW_INT", 10.0f);
 		configure_stream("ADSB_VEHICLE", 20.0f);
+		configure_stream("COLLISION", 20.0f);
 		configure_stream("DISTANCE_SENSOR", 10.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VISION_POSITION_NED", 10.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -90,6 +90,7 @@
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/mount_status.h>
+#include <uORB/topics/collision_report.h>
 #include <uORB/uORB.h>
 
 
@@ -1292,6 +1293,73 @@ protected:
 	}
 };
 
+class MavlinkStreamCollision : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamCollision::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "COLLISION";
+	}
+
+	static uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_COLLISION;
+	}
+
+	uint16_t get_id()
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamCollision(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return (_collision_time > 0) ? MAVLINK_MSG_ID_COLLISION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	MavlinkOrbSubscription *_collision_sub;
+	uint64_t _collision_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamCollision(MavlinkStreamCollision &);
+	MavlinkStreamCollision &operator = (const MavlinkStreamCollision &);
+
+protected:
+	explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_collision_sub(_mavlink->add_orb_subscription(ORB_ID(collision_report))),
+		_collision_time(0)
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		struct collision_report_s report;
+
+		if (_collision_sub->update(&_collision_time, &report)) {
+			mavlink_collision_t msg = {};
+
+			msg.src = report.src;
+			msg.id = report.id;
+			msg.action = report.action;
+			msg.threat_level = report.threat_level;
+			msg.time_to_minimum_delta = report.time_to_minimum_delta;
+			msg.altitude_minimum_delta = report.altitude_minimum_delta;
+			msg.horizontal_minimum_delta = report.horizontal_minimum_delta;
+
+			mavlink_msg_collision_send_struct(_mavlink->get_channel(), &msg);
+		}
+	}
+};
+
 class MavlinkStreamCameraTrigger : public MavlinkStream
 {
 public:
@@ -3567,6 +3635,7 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
 	new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
 	new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
+	new StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static),
 	new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
 	new StreamListItem(&MavlinkStreamMountStatus::new_instance, &MavlinkStreamMountStatus::get_name_static, &MavlinkStreamMountStatus::get_id_static),
 	nullptr

src/modules/mavlink/mavlink_receiver.cpp

@@ -128,6 +128,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_time_offset_pub(nullptr),
 	_follow_target_pub(nullptr),
 	_transponder_report_pub(nullptr),
+	_collision_report_pub(nullptr),
 	_control_state_pub(nullptr),
 	_gps_inject_data_pub(nullptr),
 	_command_ack_pub(nullptr),
@@ -258,6 +259,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_adsb_vehicle(msg);
 		break;
 
+	case MAVLINK_MSG_ID_COLLISION:
+		handle_message_collision(msg);
+		break;
+
 	case MAVLINK_MSG_ID_GPS_RTCM_DATA:
 		handle_message_gps_rtcm_data(msg);
 		break;
@@ -1927,6 +1932,30 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
 	}
 }
 
+void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg)
+{
+	mavlink_collision_t collision;
+	collision_report_s t = { };
+
+	mavlink_msg_collision_decode(msg, &collision);
+
+	t.timestamp = hrt_absolute_time();
+	t.src = collision.src;
+	t.id = collision.id;
+	t.action = collision.action;
+	t.threat_level = collision.threat_level;
+	t.time_to_minimum_delta = collision.time_to_minimum_delta;
+	t.altitude_minimum_delta = collision.altitude_minimum_delta;
+	t.horizontal_minimum_delta = collision.horizontal_minimum_delta;
+
+	if (_collision_report_pub == nullptr) {
+		_collision_report_pub = orb_advertise(ORB_ID(collision_report), &t);
+
+	} else {
+		orb_publish(ORB_ID(collision_report), _collision_report_pub, &t);
+	}
+}
+
 void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
 {
 	mavlink_gps_rtcm_data_t gps_rtcm_data_msg;

src/modules/mavlink/mavlink_receiver.h

@@ -78,6 +78,7 @@
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/collision_report.h>
 
 
 #include "mavlink_ftp.h"
@@ -143,6 +144,7 @@ private:
 	void handle_message_distance_sensor(mavlink_message_t *msg);
 	void handle_message_follow_target(mavlink_message_t *msg);
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
+	void handle_message_collision(mavlink_message_t *msg);
 	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
 	void handle_message_battery_status(mavlink_message_t *msg);
 	void handle_message_serial_control(mavlink_message_t *msg);
@@ -223,6 +225,7 @@ private:
 	orb_advert_t _time_offset_pub;
 	orb_advert_t _follow_target_pub;
 	orb_advert_t _transponder_report_pub;
+	orb_advert_t _collision_report_pub;
 	orb_advert_t _control_state_pub;
 	static const int _gps_inject_data_queue_size = 6;
 	orb_advert_t _gps_inject_data_pub;