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

Checkpoint: implement new state machine, compiling, WIP

Browse Source
This commit is contained in:
Julian Oes
2013-02-16 13:40:09 -08:00
parent 2d1009a897
commit 0e2db0beb9
14 changed files with 920 additions and 936 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/controllib/fixedwing.cpp
+17 -16
View File
@@ -39,6 +39,7 @@
#include "fixedwing.hpp"
namespace control
{
@@ -294,7 +295,7 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
if (_status.navigation_state == NAVIGATION_STATE_MISSION) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
}
@@ -304,8 +305,8 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
if (_status.navigation_state == NAVIGATION_STATE_MISSION ||
_status.navigation_state == NAVIGATION_STATE_MANUAL) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
@@ -340,25 +341,25 @@ void BlockMultiModeBacksideAutopilot::update()
}
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
#warning fix the different manual modes
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = - _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}
// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
//
// _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
// _att.rollspeed, _att.pitchspeed, _att.yawspeed);
//
// _actuators.control[CH_AIL] = _stabilization.getAileron();
// _actuators.control[CH_ELV] = - _stabilization.getElevator();
// _actuators.control[CH_RDR] = _stabilization.getRudder();
// _actuators.control[CH_THR] = _manual.throttle;
// }
}
// update all publications