From 77341fa4fe52ec1fca60f708ce76efcc7048ba4a Mon Sep 17 00:00:00 2001 From: dirksavage88 Date: Tue, 30 May 2023 18:37:55 -0400 Subject: [PATCH] ODID prototype Signed-off-by: dirksavage88 --- .../HealthAndArmingChecks.hpp | 33 ++++++++----------- src/modules/mavlink/mavlink_main.cpp | 4 +++ src/modules/mavlink/mavlink_messages.cpp | 4 +++ src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 ++ 5 files changed, 42 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index 5cd35cfe7d8c..898d79bf4cdc 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -37,37 +37,37 @@ #include #include -#include #include +#include #include "checks/accelerometerCheck.hpp" #include "checks/airspeedCheck.hpp" #include "checks/baroCheck.hpp" -#include "checks/batteryCheck.hpp" #include "checks/cpuResourceCheck.hpp" #include "checks/distanceSensorChecks.hpp" #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" -#include "checks/flightTimeCheck.hpp" -#include "checks/geofenceCheck.hpp" #include "checks/gyroCheck.hpp" -#include "checks/homePositionCheck.hpp" #include "checks/imuConsistencyCheck.hpp" #include "checks/magnetometerCheck.hpp" #include "checks/manualControlCheck.hpp" -#include "checks/missionCheck.hpp" +#include "checks/homePositionCheck.hpp" #include "checks/modeCheck.hpp" -#include "checks/offboardCheck.hpp" #include "checks/parachuteCheck.hpp" #include "checks/powerCheck.hpp" -#include "checks/rcAndDataLinkCheck.hpp" #include "checks/rcCalibrationCheck.hpp" #include "checks/remoteidCheck.hpp" #include "checks/sdcardCheck.hpp" #include "checks/systemCheck.hpp" -#include "checks/vtolCheck.hpp" +#include "checks/batteryCheck.hpp" #include "checks/windCheck.hpp" +#include "checks/geofenceCheck.hpp" +#include "checks/flightTimeCheck.hpp" +#include "checks/missionCheck.hpp" +#include "checks/rcAndDataLinkCheck.hpp" +#include "checks/vtolCheck.hpp" +#include "checks/offboardCheck.hpp" class HealthAndArmingChecks : public ModuleParams { @@ -96,16 +96,12 @@ class HealthAndArmingChecks : public ModuleParams /** * Query the mode requirements: check if a mode prevents arming */ - bool modePreventsArming(uint8_t nav_state) const - { - return _reporter.modePreventsArming(nav_state); - } + bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); } const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; } protected: void updateParams() override; - private: failsafe_flags_s _failsafe_flags{}; @@ -114,8 +110,7 @@ class HealthAndArmingChecks : public ModuleParams orb_advert_t _mavlink_log_pub{nullptr}; uORB::Publication _health_report_pub{ORB_ID(health_report)}; - uORB::Publication _failsafe_flags_pub{ - ORB_ID(failsafe_flags)}; + uORB::Publication _failsafe_flags_pub{ORB_ID(failsafe_flags)}; // all checks AccelerometerChecks _accelerometer_checks; @@ -163,14 +158,12 @@ class HealthAndArmingChecks : public ModuleParams &_home_position_checks, &_mission_checks, &_offboard_checks, // must be after _estimator_checks - &_mode_checks, // must be after _estimator_checks, _home_position_checks, - // _mission_checks, _offboard_checks + &_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks &_parachute_checks, &_power_checks, &_rc_calibration_checks, &_sd_card_checks, - &_system_checks, // must be after _estimator_checks & - // _home_position_checks + &_system_checks, // must be after _estimator_checks & _home_position_checks &_battery_checks, &_wind_checks, &_geofence_checks, // must be after _home_position_checks diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 40f6cbf8191b..dbbada94ae60 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1515,6 +1515,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("LOCAL_POSITION_NED", 1.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f); configure_stream_local("OBSTACLE_DISTANCE", 1.0f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f); @@ -1577,6 +1578,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("HYGROMETER_SENSOR", 1.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); @@ -1729,6 +1731,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("MAG_CAL_REPORT", 1.0f); configure_stream_local("MANUAL_CONTROL", 5.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); @@ -1812,6 +1815,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_RAW_INT", unlimited_rate); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 43f7fab01645..17cd15ed0d3c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,6 +94,7 @@ #include "streams/MOUNT_ORIENTATION.hpp" #include "streams/NAV_CONTROLLER_OUTPUT.hpp" #include "streams/OBSTACLE_DISTANCE.hpp" +#include "streams/OPEN_DRONE_ID_ARM_STATUS.hpp" #include "streams/OPEN_DRONE_ID_BASIC_ID.hpp" #include "streams/OPEN_DRONE_ID_LOCATION.hpp" #include "streams/OPEN_DRONE_ID_SYSTEM.hpp" @@ -429,6 +430,9 @@ static const StreamListItem streams_list[] = { #if defined(OBSTACLE_DISTANCE_HPP) create_stream_list_item(), #endif // OBSTACLE_DISTANCE_HPP +#if defined(OPEN_DRONE_ID_ARM_STATUS_HPP) + create_stream_list_item(), +#endif // OPEN_DRONE_ID_ARM_STATUS_HPP #if defined(OPEN_DRONE_ID_BASIC_ID_HPP) create_stream_list_item(), #endif // OPEN_DRONE_ID_BASIC_ID_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 26103f656aea..9410f3050bdb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -284,6 +284,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_statustext(msg); break; + case MAVLINK_MSG_OPEN_DRONE_ID_ARM_STATUS: + handle_message_open_drone_id_arm_status(msg); + break; + #if !defined(CONSTRAINED_FLASH) case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: @@ -3069,7 +3073,21 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } +void MavlinkReceiver::handle_message_open_drone_id_arm_status( + mavlink_message_t *msg) +{ + mavlink_open_drone_id_arm_status_t odid_module; + mavlink_msg_open_drone_id_arm_status_decode(msg, &odid_module); + open_drone_id_arm_status_s odid_arm{}; + memset(&odid_arm, 0, sizeof(odid_arm)); + + odid_arm.timestamp = hrt_absolute_time(); + odid_arm.status = odid_module.status; + // odid_module.error; + + _open_drone_id_arm_status_pub.publish(odid_arm); +} void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7939e813ae8a..d6ee5a976569 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -87,6 +87,7 @@ #include #include #include +#include #include #include #include @@ -179,6 +180,7 @@ class MavlinkReceiver : public ModuleParams 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); + void handle_message_open_drone_id_arm_status(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_play_tune(mavlink_message_t *msg); @@ -305,6 +307,7 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; + uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};