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

vtol_att_control: ack transition commands 

Previously transition commands were not acked at all which meant that a
mavlink consumer such as a ground station would not get feedback if the
command arrived.

This solution is not optimal because it does not take into account if
the transition actually happened but at least it is feedback that the
command has arrived at the destination.
This commit is contained in:
Julian Oes 2017-11-09 16:31:35 -05:00
parent 4028692bfe
commit 585b03898f
2 changed files with 29 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013 - 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -88,6 +88,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
_vtol_vehicle_status_pub(nullptr),
_v_rates_sp_pub(nullptr),
_v_att_sp_pub(nullptr),
_v_cmd_ack_pub(nullptr),
_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC),
_abort_front_transition(false)
@ -483,6 +484,31 @@ VtolAttitudeControl::handle_command()
// update transition command if necessary
if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
_transition_command = int(_vehicle_cmd.param1 + 0.5f);
// Report that we have received the command no matter what we actually do with it.
// This might not be optimal but is better than no response at all.
if (_vehicle_cmd.from_external) {
vehicle_command_ack_s command_ack = {
.timestamp = hrt_absolute_time(),
.result_param2 = 0,
.command = _vehicle_cmd.command,
.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
.from_external = false,
.result_param1 = 0,
.target_system = _vehicle_cmd.source_system,
.target_component = _vehicle_cmd.source_component
};
if (_v_cmd_ack_pub == nullptr) {
_v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack);
}
}
}
}

2
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -72,6 +72,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
@ -159,6 +160,7 @@ private:
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
orb_advert_t _v_att_sp_pub;
orb_advert_t _v_cmd_ack_pub;
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude