From 6562dd496b588b4f329d096610a851b782bf21aa Mon Sep 17 00:00:00 2001 From: Sugnan Prabhu S Date: Wed, 30 Aug 2017 23:12:18 +0530 Subject: [PATCH] mavlink: implement MAV_CMD_REQUEST_FLIGHT_INFORMATION Signed-off-by: Sugnan Prabhu S --- msg/actuator_armed.msg | 1 + .../commander/state_machine_helper.cpp | 5 ++++ src/modules/mavlink/mavlink_receiver.cpp | 27 +++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 4 +++ 4 files changed, 37 insertions(+) diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index 7a33dccad1..f44ea729c2 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -1,4 +1,5 @@ +uint32 armed_time_ms # Arming timestamp bool armed # Set to true if system is armed bool prearmed # Set to true if the actuator safety is disabled but motors are not armed bool ready_to_arm # Set to true if system is ready to be armed diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 266c81832d..a50435b17c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -332,6 +332,11 @@ transition_result_t arming_state_transition(vehicle_status_s *status, || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; + if(new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + armed->armed_time_ms = hrt_absolute_time() / 1000; + } else { + armed->armed_time_ms = 0; + } } /* reset feedback state */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 87da425005..016e1c1e4f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,6 +102,7 @@ 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), @@ -143,6 +144,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _gps_inject_data_pub(nullptr), _command_ack_pub(nullptr), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), + _actuator_armed_sub(orb_subscribe(ORB_ID(actuator_armed))), _global_ref_timestamp(0), _hil_frames(0), _old_timestamp(0), @@ -398,6 +400,28 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c return target_ok; } +void +MavlinkReceiver::send_flight_information() +{ + bool updated; + mavlink_flight_information_t flight_info; + uuid_uint32_t uid; + board_get_uuid32(uid); + + flight_info.flight_uuid = (((uint64_t)uid[PX4_CPU_UUID_WORD32_UNIQUE_M]) << 32) | + uid[PX4_CPU_UUID_WORD32_UNIQUE_H]; + + orb_check(_actuator_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &_actuator_armed); + } + + 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); +} + void MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) { @@ -433,6 +457,9 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { get_message_interval((int)cmd_mavlink.param1); + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) { + send_flight_information(); + } else { send_ack = false; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index bb86564bc8..3265ffe91a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -195,6 +195,8 @@ private: bool evaluate_target_ok(int command, int target_system, int target_component); + void send_flight_information(); + Mavlink *_mavlink; MavlinkMissionManager _mission_manager; @@ -206,6 +208,7 @@ 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; @@ -248,6 +251,7 @@ private: orb_advert_t _gps_inject_data_pub; orb_advert_t _command_ack_pub; int _control_mode_sub; + int _actuator_armed_sub; uint64_t _global_ref_timestamp; int _hil_frames; uint64_t _old_timestamp;