diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fc028ddb6f..f1892e0a7a 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/collision_report.msg b/msg/collision_report.msg new file mode 100644 index 0000000000..726c1fb786 --- /dev/null +++ b/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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ba9378f9bc..6ca56a1dda 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bf2845924c..f9be1a80c3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -90,6 +90,7 @@ #include #include #include +#include #include @@ -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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4064bfbd99..c5636f70e0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 145e3c696b..e92f166171 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -78,6 +78,7 @@ #include #include #include +#include #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;