From 2cef6c462b2b62cf7fb406152fb4f164dfaa5245 Mon Sep 17 00:00:00 2001 From: neo Date: Thu, 13 Jun 2024 19:24:20 +0200 Subject: [PATCH] ground FTS pairing WIP --- boards/px4/fts-can/init/rc.board_defaults | 2 +- boards/px4/fts-can/init/rc.board_sensors | 3 +- msg/CMakeLists.txt | 3 + msg/EmergencyCommand.msg | 6 ++ msg/GroundFtsMessage.msg | 4 ++ msg/GroundFtsResponse.msg | 4 ++ src/modules/fts_control/fts_control.cpp | 50 +++++++++++-- src/modules/fts_control/fts_control.hpp | 36 ++++++++-- src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 6 +- src/modules/mavlink/mavlink_receiver.cpp | 42 ++++++++--- src/modules/mavlink/mavlink_receiver.h | 6 ++ .../mavlink/streams/GROUND_FTS_RESPONSE.hpp | 70 +++++++++++++++++++ 13 files changed, 209 insertions(+), 24 deletions(-) create mode 100644 msg/EmergencyCommand.msg create mode 100644 msg/GroundFtsMessage.msg create mode 100644 msg/GroundFtsResponse.msg create mode 100644 src/modules/mavlink/streams/GROUND_FTS_RESPONSE.hpp diff --git a/boards/px4/fts-can/init/rc.board_defaults b/boards/px4/fts-can/init/rc.board_defaults index 98a1e6a..6c7236c 100644 --- a/boards/px4/fts-can/init/rc.board_defaults +++ b/boards/px4/fts-can/init/rc.board_defaults @@ -10,7 +10,7 @@ param set-default SYS_AUTOSTART 1 param set-default MAV_PROTO_VER 2 # Log from boot until shutdown -param set-default SDLOG_MODE 2 +#param set-default SDLOG_MODE 2 param set-default SYS_USE_IO 0 param set-default SENS_IMU_MODE 0 diff --git a/boards/px4/fts-can/init/rc.board_sensors b/boards/px4/fts-can/init/rc.board_sensors index 2935cad..2a7e0d5 100644 --- a/boards/px4/fts-can/init/rc.board_sensors +++ b/boards/px4/fts-can/init/rc.board_sensors @@ -2,5 +2,4 @@ # # PX4 FTS-CAN specific board sensors init #------------------------------------------------------------------------------ -board_adc start -fts_control start +board_adc start \ No newline at end of file diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index dbfc3b6..b7e7ada 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -68,6 +68,7 @@ set(msg_files DifferentialPressure.msg DistanceSensor.msg Ekf2Timestamps.msg + EmergencyCommand.msg EscReport.msg EscStatus.msg EstimatorAidSource1d.msg @@ -103,6 +104,8 @@ set(msg_files GpsInjectData.msg GimbalControls.msg Gripper.msg + GroundFtsMessage.msg + GroundFtsResponse.msg HealthReport.msg HeaterStatus.msg HomePosition.msg diff --git a/msg/EmergencyCommand.msg b/msg/EmergencyCommand.msg new file mode 100644 index 0000000..ee7e7c6 --- /dev/null +++ b/msg/EmergencyCommand.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component +uint8 emergency_command +uint32 param diff --git a/msg/GroundFtsMessage.msg b/msg/GroundFtsMessage.msg new file mode 100644 index 0000000..8891467 --- /dev/null +++ b/msg/GroundFtsMessage.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 message +uint32 fts_uid diff --git a/msg/GroundFtsResponse.msg b/msg/GroundFtsResponse.msg new file mode 100644 index 0000000..8891467 --- /dev/null +++ b/msg/GroundFtsResponse.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 message +uint32 fts_uid diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index dd66a50..cdcc36a 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/src/modules/fts_control/fts_control.cpp @@ -1,11 +1,9 @@ -/** - * - * Controller of Nimbus Charging FTS - * - * @author Flavio Tonelli - */ #include "fts_control.hpp" +#include "px4_platform_common/board_common.h" + + +#include "../mavlink/streams/GROUND_FTS_RESPONSE.hpp" FTSController::FTSController() : ModuleParams(nullptr), @@ -30,6 +28,12 @@ FTSController::init() PX4_WARN("fts_control failed to get necessary parameters."); return false; } + uint8_t guid[PX4_GUID_BYTE_LENGTH]; + board_get_px4_guid(guid); + fts_id.uidBytes[0]=guid[0]; + fts_id.uidBytes[1]=guid[1]; + fts_id.uidBytes[2]=guid[2]; + fts_id.uidBytes[3]=guid[3]; // run every 1s ScheduleOnInterval(1000_ms); @@ -73,7 +77,27 @@ FTSController::Run() } } - // Publish FTS controller state + // Update GROUND-FTS status + if(_ground_fts_message_sub.updated()) { + + _ground_fts_message_sub.copy(&_ground_fts_message); + + if (_ground_fts_message.message == PING) { + //PING COMMAND + if(_ground_fts_message.fts_uid == fts_id.uid){ + + _ground_fts_response_pub.publish(_ground_fts_response); + + } + }else if(_ground_fts_message.message == PAIR){ + //PAIR COMMAND + _ground_fts_response.message = PAIR; + _ground_fts_response.fts_uid = fts_id.uid; + _ground_fts_response_pub.publish(_ground_fts_response); + MavlinkStreamGroundftsResponse.send(); + } + } + // Publish FTS controller state _fts_state.timestamp = hrt_absolute_time(); perf_end(_cycle_perf); @@ -170,6 +194,16 @@ FTSController::custom_command(int argc, char *argv[]) return 0; } + if (!strcmp(argv[0], "arm")) { + get_instance()->arm(); + return 0; + } + + if (!strcmp(argv[0], "disarm")) { + get_instance()->disarm(); + return 0; + } + return print_usage("unknown command"); } @@ -201,6 +235,8 @@ FTSController::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("fts_controller", "system"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND("status"); + PRINT_MODULE_USAGE_COMMAND("arm"); + PRINT_MODULE_USAGE_COMMAND("disarm"); PRINT_MODULE_USAGE_COMMAND("fire"); PRINT_MODULE_USAGE_COMMAND("reset"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/modules/fts_control/fts_control.hpp b/src/modules/fts_control/fts_control.hpp index f7c47b2..543041b 100644 --- a/src/modules/fts_control/fts_control.hpp +++ b/src/modules/fts_control/fts_control.hpp @@ -15,12 +15,33 @@ #include #include #include +#include +#include +#include #include #include #include #include using namespace time_literals; +// Helper union to construct the UID field of the Mavlink message. +union FTSIDUnion{ + uint32_t uid; + uint8_t uidBytes[4]; +}; + +typedef enum FTS_MESSAGE_TYPE +{ + PING=1, /* Send from the host to the FTS. | */ + PONG=2, /* Send from the FTS to the host. | */ + PAIR=3, /* A Pairing operation. Send from host to FTS means the host want to start a pairing procedure. + The FTS will ignore the FTS_UID field check when it receives this message and send back this + same message as a response. Then the host shall register the FTS_UID field in the response. + | */ + FTS_MESSAGE_TYPE_ENUM_END=4, /* | */ +} FTS_MESSAGE_TYPE; + + class FTSController : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -56,18 +77,25 @@ private: perf_counter_t _cycle_perf; perf_counter_t _interval_perf; - uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::SubscriptionData _auto_fts_message_sub{ORB_ID(auto_fts_message)}; - uORB::SubscriptionData _home_position_sub{ORB_ID(home_position)}; - uORB::SubscriptionData _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _auto_fts_message_sub{ORB_ID(auto_fts_message)}; + uORB::SubscriptionData _ground_fts_message_sub{ORB_ID(ground_fts_message)}; + uORB::SubscriptionData _emergency_command_sub{ORB_ID(emergency_command)}; uORB::Publication _fts_state_pub{ORB_ID(fts_state)}; + uORB::Publication _ground_fts_response_pub{ORB_ID(ground_fts_response)}; FTS fts_drv; hrt_abstime _last_fts_state_update_time{0}; fts_state_s _fts_state{}; auto_fts_message_s _auto_fts_message{}; + ground_fts_message_s _ground_fts_message{}; + ground_fts_response_s _ground_fts_response{}; + emergency_command_s _emergency_command{}; uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz + + // Global variable storing the UID of the chip. + union FTSIDUnion fts_id; + }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8b75786..9ae4302 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1844,6 +1844,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) break; //MODE 14 MFTS TO GFTS case MAVLINK_MODE_MFTS_GFTS: + configure_stream_local("GROUND_FTS_RESPONSE", 0.0f); break; //MODE 15 AFTS TO MFTS case MAVLINK_MODE_AFTS_MFTS: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 309cc76..a064508 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -82,6 +82,7 @@ #include "streams/GPS_RAW_INT.hpp" #include "streams/GPS_RTCM_DATA.hpp" #include "streams/GPS_STATUS.hpp" +#include "streams/GROUND_FTS_RESPONSE.hpp" #include "streams/HEARTBEAT.hpp" #include "streams/HIGHRES_IMU.hpp" #include "streams/HIL_ACTUATOR_CONTROLS.hpp" @@ -485,8 +486,11 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP #if defined(AUTO_FTS_MESSAGE_HPP) - create_stream_list_item() + create_stream_list_item(), #endif // AUTO_FTS_MESSAGE_HPP +#if defined(GROUND_FTS_RESPONSE_HPP) + create_stream_list_item() +#endif // GROUND_FTS_RESPONSE_HPP }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0cb37d8..778c5a4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -316,6 +316,12 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_AUTO_FTS_MESSAGE: handle_message_auto_fts_message(msg); break; + case MAVLINK_MSG_ID_FTS_MESSAGE: + handle_message_ground_fts_message(msg); + break; + case MAVLINK_MSG_ID_EMERGENCY_COMMAND: + handle_message_emergency_command(msg); + break; default: break; @@ -3066,16 +3072,34 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t void MavlinkReceiver::handle_message_auto_fts_message(mavlink_message_t *msg) { -mavlink_auto_fts_message_t auto_fts_message_msg; -mavlink_msg_auto_fts_message_decode(msg, &auto_fts_message_msg); + mavlink_auto_fts_message_t auto_fts_message_msg; + mavlink_msg_auto_fts_message_decode(msg, &auto_fts_message_msg); + + auto_fts_message_s auto_fts_message{}; + auto_fts_message.timestamp = hrt_absolute_time(); + auto_fts_message.config_status=auto_fts_message_msg.config_status; + auto_fts_message.geofence_loaded=auto_fts_message_msg.geofence_loaded; + auto_fts_message.gps_lock=auto_fts_message_msg.gps_lock; + auto_fts_message.geofence_violated=auto_fts_message_msg.geofence_violated; + _auto_fts_message_pub.publish(auto_fts_message); +} + +void +MavlinkReceiver::handle_message_ground_fts_message(mavlink_message_t *msg) +{ + mavlink_fts_message_t ground_fts_message_mavlink; + mavlink_msg_fts_message_decode(msg, &ground_fts_message_mavlink); + + + ground_fts_message_s ground_fts_message{}; + ground_fts_message.timestamp = hrt_absolute_time(); + ground_fts_message.message = ground_fts_message_mavlink.message; + _ground_fts_message_pub.publish(ground_fts_message); +} + +void +MavlinkReceiver::handle_message_emergency_command(mavlink_message_t *msg) { -auto_fts_message_s auto_fts_message{}; -auto_fts_message.timestamp = hrt_absolute_time(); -auto_fts_message.config_status=auto_fts_message_msg.config_status; -auto_fts_message.geofence_loaded=auto_fts_message_msg.geofence_loaded; -auto_fts_message.gps_lock=auto_fts_message_msg.gps_lock; -auto_fts_message.geofence_violated=auto_fts_message_msg.geofence_violated; -_auto_fts_message_pub.publish(auto_fts_message); } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fe996f2..f1e3f39 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -84,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -166,6 +168,7 @@ private: void handle_message_command_int(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); void handle_message_distance_sensor(mavlink_message_t *msg); + void handle_message_emergency_command(mavlink_message_t *msg); void handle_message_follow_target(mavlink_message_t *msg); void handle_message_generator_status(mavlink_message_t *msg); void handle_message_set_gps_global_origin(mavlink_message_t *msg); @@ -178,6 +181,7 @@ private: void handle_message_landing_target(mavlink_message_t *msg); void handle_message_logging_ack(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_ground_fts_message(mavlink_message_t *msg); void handle_message_obstacle_distance(mavlink_message_t *msg); void handle_message_odometry(mavlink_message_t *msg); void handle_message_onboard_computer_status(mavlink_message_t *msg); @@ -296,6 +300,7 @@ private: uORB::Publication _cellular_status_pub{ORB_ID(cellular_status)}; uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _emergency_command_pub{ORB_ID(emergency_command)}; uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; uORB::Publication _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)}; @@ -304,6 +309,7 @@ private: uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; uORB::Publication _log_message_pub{ORB_ID(log_message)}; + uORB::Publication _ground_fts_message_pub{ORB_ID(ground_fts_message)}; uORB::Publication _mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; diff --git a/src/modules/mavlink/streams/GROUND_FTS_RESPONSE.hpp b/src/modules/mavlink/streams/GROUND_FTS_RESPONSE.hpp new file mode 100644 index 0000000..6d53ec5 --- /dev/null +++ b/src/modules/mavlink/streams/GROUND_FTS_RESPONSE.hpp @@ -0,0 +1,70 @@ +#ifndef GROUND_FTS_RESPONSE_HPP +#define GROUND_FTS_RESPONSE_HPP + +#include +#include +#include + +#include +#include +#include + +class MavlinkStreamGroundftsResponse : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGroundftsResponse(mavlink); } + + static constexpr const char *get_name_static() { return "GROUND_FTS_RESPONSE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FTS_MESSAGE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + uORB::Subscription _ground_fts_response_sub{ORB_ID(ground_fts_response)}; + + hrt_abstime _last_reset_time{0}; + hrt_abstime _last_update_time{0}; + + unsigned get_size() override + { + return MAVLINK_MSG_ID_FTS_MESSAGE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() override { return true; } + +private: + explicit MavlinkStreamGroundftsResponse(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_fts_message_t msg{}; + + ground_fts_response_s response; + + if (_ground_fts_response_sub.update(&response)) { + msg.message = response.message; + msg.FTS_UID = response.fts_uid; + + return true; + } + + return false; + + mavlink_msg_fts_message_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + return false; + + } + +}; + +#endif