fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 16:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Front transition timeout

Browse Source
This commit is contained in:
sander
2016-02-10 16:15:15 +01:00
committed by Andreas Antener
parent 519a7e2c8e
commit 2fa7380246
5 changed files with 74 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+40 -1
View File
@@ -41,10 +41,12 @@
* @author Roman Bapst <bapstr@ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
* @author Sander Smeets <sander@droneslab.com>
*
*/
#include "vtol_att_control_main.h"
#include <mavlink/mavlink_log.h>
namespace VTOL_att_control
{
@@ -58,6 +60,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
_task_should_exit(false),
_control_task(-1),
// mavlink log
_mavlink_fd(-1),
//init subscription handlers
_v_att_sub(-1),
_v_att_sp_sub(-1),
@@ -427,9 +432,31 @@ VtolAttitudeControl::is_fixed_wing_requested()
to_fw = _transition_command == vehicle_status_s::VEHICLE_VTOL_STATE_FW;
}
// handle abort request
if(_abort_front_transition) {
if(to_fw) {
to_fw = false;
} else {
// the state changed to mc mode, reset the abort request
_abort_front_transition = false;
}
}
return to_fw;
}
/*
* Abort front transition
*/
void
VtolAttitudeControl::abort_front_transition()
{
if(!_abort_front_transition) {
mavlink_log_critical(_mavlink_fd, "Front transition timeout occured, aborting");
_abort_front_transition = true;
}
}
/**
* Update parameters.
*/
@@ -530,6 +557,8 @@ void VtolAttitudeControl::task_main()
PX4_WARN("started");
fflush(stdout);
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
@@ -558,6 +587,9 @@ void VtolAttitudeControl::task_main()
// make sure we start with idle in mc mode
_vtol_type->set_idle_mc();
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source*/
px4_pollfd_struct_t fds[3] = {}; /*input_mc, input_fw, parameters*/
@@ -595,6 +627,13 @@ void VtolAttitudeControl::task_main()
continue;
}
if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
/* try to reopen the mavlink log device with specified interval */
mavlink_open_time = hrt_abstime() + mavlink_open_interval;
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
}
if (fds[2].revents & POLLIN) { //parameters were updated, read them now
/* read from param to clear updated flag */
struct parameter_update_s update;