afts status stream dev

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "src/modules/mavlink/mavlink"]
 	path = src/modules/mavlink/mavlink
-	url = https://github.com/mavlink/mavlink.git
+	url = https://gitlab.com/neosystems/nimbus/mavlinknimbus.git
 	branch = master
 [submodule "src/drivers/uavcan/libuavcan"]
 	path = src/drivers/uavcan/libuavcan

boards/px4/fts-auto/default.px4board

@@ -31,6 +31,7 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
+CONFIG_MAVLINK_DIALECT="customNimbus"
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y

clear.sh

@@ -0,0 +1,3 @@
+#!/bin/bash -xe
+
+rm -rf build

src/modules/mavlink/mavlink_main.cpp

@@ -1593,7 +1593,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 	/* fallthrough */
 	case MAVLINK_MODE_CUSTOM:
-		//stream nothing
+        configure_stream_local("AUTO_FTS_MESSAGE", 1.0f);
 		break;
 
 	case MAVLINK_MODE_CONFIG: // USB
@@ -1650,7 +1650,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("VFR_HUD", 20.0f);
 		configure_stream_local("VIBRATION", 2.5f);
 		configure_stream_local("WIND_COV", 10.0f);
-        configure_stream_local("HIGH_LATENCY2", 1.0f);
+        configure_stream_local("AUTO_FTS_MESSAGE", 1.0f);
 
 #if !defined(CONSTRAINED_FLASH)
 		configure_stream_local("DEBUG", 50.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -62,6 +62,7 @@
 #include "streams/ATTITUDE_QUATERNION.hpp"
 #include "streams/ATTITUDE_TARGET.hpp"
 #include "streams/AUTOPILOT_VERSION.hpp"
+#include "streams/AUTO_FTS_MESSAGE.hpp"
 #include "streams/BATTERY_STATUS.hpp"
 #include "streams/CAMERA_IMAGE_CAPTURED.hpp"
 #include "streams/CAMERA_TRIGGER.hpp"
@@ -481,8 +482,11 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
 #endif // UAVIONIX_ADSB_OUT_CFG_HPP
 #if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
-	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>()
+	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
 #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
+#if defined(AUTO_FTS_MESSAGE_HPP)
+        create_stream_list_item<MavlinkStreamAftsStatus>()
+#endif // AUTO_FTS_MESSAGE_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp

@@ -0,0 +1,106 @@
+#ifndef AUTO_FTS_MESSAGE_HPP
+#define AUTO_FTS_MESSAGE_HPP
+
+#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/estimator_selector_status.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/geofence_result.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/wind.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/failsafe_flags.h>
+#include <uORB/topics/health_report.h>
+
+#include <px4_platform_common/events.h>
+
+class MavlinkStreamAftsStatus : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAftsStatus(mavlink); }
+
+	static constexpr const char *get_name_static() { return "AUTO_FTS_MESSAGE"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AUTO_FTS_MESSAGE; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_AUTO_FTS_MESSAGE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+	bool const_rate() override { return true; }
+
+private:
+	explicit MavlinkStreamAftsStatus(Mavlink *mavlink) :
+		MavlinkStream(mavlink)
+	{
+	}
+
+	bool send() override
+	{
+		// only send the struct if transmitting is allowed
+		// this assures that the stream timer is only reset when actually a message is transmitted
+		if (_mavlink->should_transmit()) {
+			mavlink_auto_fts_message_t msg{};
+			set_default_values(msg);
+
+			write_geofence_result(&msg);
+
+
+            mavlink_msg_auto_fts_message_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+
+		}
+		return false;
+
+	}
+
+	bool write_geofence_result(mavlink_auto_fts_message_t *msg)
+	{
+		geofence_result_s geofence;
+
+		if (_geofence_sub.update(&geofence)) {
+			if (geofence.primary_geofence_breached) {
+				msg->geofence_violated = 1;
+			}else {
+                msg->geofence_violated = 0;
+            }
+
+			return true;
+		}
+
+		return false;
+	}
+
+
+	void set_default_values(mavlink_auto_fts_message_t &msg) const
+	{
+		msg.config_status = 0;
+		msg.geofence_loaded = 0;
+        msg.gps_lock = 0;
+        msg.geofence_violated = 0;
+	}
+
+    uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
+
+	hrt_abstime _last_reset_time{0};
+	hrt_abstime _last_update_time{0};
+
+};
+
+#endif