From 9d052eadd6fde190ec7e18c71ec09c31ee14ec58 Mon Sep 17 00:00:00 2001 From: neo Date: Fri, 7 Jun 2024 17:29:24 +0200 Subject: [PATCH] afts status stream dev --- .gitmodules | 2 +- boards/px4/fts-auto/default.px4board | 1 + clear.sh | 3 + src/modules/mavlink/mavlink_main.cpp | 4 +- src/modules/mavlink/mavlink_messages.cpp | 6 +- .../mavlink/streams/AUTO_FTS_MESSAGE.hpp | 106 ++++++++++++++++++ 6 files changed, 118 insertions(+), 4 deletions(-) create mode 100755 clear.sh create mode 100644 src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp diff --git a/.gitmodules b/.gitmodules index 6ecc529..c10daa9 100644 --- a/.gitmodules +++ b/.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 diff --git a/boards/px4/fts-auto/default.px4board b/boards/px4/fts-auto/default.px4board index f371ebf..c62d845 100644 --- a/boards/px4/fts-auto/default.px4board +++ b/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 diff --git a/clear.sh b/clear.sh new file mode 100755 index 0000000..26617b4 --- /dev/null +++ b/clear.sh @@ -0,0 +1,3 @@ +#!/bin/bash -xe + +rm -rf build diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0241868..8bf87b3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e172024..309cc76 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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(), #endif // UAVIONIX_ADSB_OUT_CFG_HPP #if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP) - create_stream_list_item() + create_stream_list_item(), #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP +#if defined(AUTO_FTS_MESSAGE_HPP) + create_stream_list_item() +#endif // AUTO_FTS_MESSAGE_HPP }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp new file mode 100644 index 0000000..37cc991 --- /dev/null +++ b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp @@ -0,0 +1,106 @@ +#ifndef AUTO_FTS_MESSAGE_HPP +#define AUTO_FTS_MESSAGE_HPP + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +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