diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 36186ba5ef853c5c9ea38168e170e32f5b654f47..b7a2c651481e2895bfd32354e284fcc88f2cf369 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,7 +102,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_pos{}, _hil_land_detector{}, _control_mode{}, - _actuator_armed{}, _global_pos_pub(nullptr), _local_pos_pub(nullptr), _attitude_pub(nullptr), @@ -167,6 +166,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : MavlinkReceiver::~MavlinkReceiver() { orb_unsubscribe(_control_mode_sub); + orb_unsubscribe(_actuator_armed_sub); } void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, int ret) @@ -422,21 +422,23 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c void MavlinkReceiver::send_flight_information() { - bool updated; - mavlink_flight_information_t flight_info; - uuid_uint32_t uid; - board_get_uuid32(uid); + mavlink_flight_information_t flight_info{}; - flight_info.flight_uuid = (((uint64_t)uid[PX4_CPU_UUID_WORD32_UNIQUE_M]) << 32) | - uid[PX4_CPU_UUID_WORD32_UNIQUE_H]; + param_t param_flight_uuid = param_find("COM_FLIGHT_UUID"); - orb_check(_actuator_armed_sub, &updated); + if (param_flight_uuid != PARAM_INVALID) { + int32_t flight_uuid; + param_get(param_flight_uuid, &flight_uuid); + flight_info.flight_uuid = (uint64_t)flight_uuid; + } + + actuator_armed_s actuator_armed; + int ret = orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed); - if (updated) { - orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &_actuator_armed); + if (ret == 0 && actuator_armed.timestamp != 0) { + flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time_ms; } - flight_info.arming_time_utc = flight_info.takeoff_time_utc = _actuator_armed.armed_time_ms; flight_info.time_boot_ms = hrt_absolute_time() / 1000; mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index bee5d6d5276704a6bbbce4cc9c6d08eff3c857ca..f58c6b05efb4100e3fdcef53eab866eb343e7504 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -207,7 +207,6 @@ private: struct vehicle_local_position_s _hil_local_pos; struct vehicle_land_detected_s _hil_land_detector; struct vehicle_control_mode_s _control_mode; - struct actuator_armed_s _actuator_armed; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub;