dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: implement MAV_CMD_REQUEST_FLIGHT_INFORMATION 

Signed-off-by: Sugnan Prabhu S <sugnan.prabhu.s@intel.com>
This commit is contained in:
Sugnan Prabhu S 2017-08-30 23:12:18 +05:30 committed by Lorenz Meier
parent 8a1d8f2162
commit 6562dd496b
4 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/actuator_armed.msg
View File

@ -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

5
src/modules/commander/state_machine_helper.cpp
View File

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

27
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;

4
src/modules/mavlink/mavlink_receiver.h
View File

@ -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;