mavlink: implement MAV_CMD_REQUEST_FLIGHT_INFORMATION

Signed-off-by: Sugnan Prabhu S <sugnan.prabhu.s@intel.com>

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

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 */

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;

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;