From b48f99b601f96321f7200971550ba07177bad5a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jan 2013 22:22:22 +0100 Subject: Updated state switching to most recent state machine rev --- apps/controllib/fixedwing.cpp | 42 +++++++++++++++++++++++++++++------------- 1 file changed, 29 insertions(+), 13 deletions(-) (limited to 'apps/controllib/fixedwing.cpp') diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 695653714..03da35a0a 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -293,17 +293,20 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + _guide.update(_pos, _att, _posCmd, _lastPosCmd); + } + + // XXX handle STABILIZED (loiter on spot) as well + // once the system switches from manual or auto to stabilized + // the setpoint should update to loitering around this position + // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_STABILIZED) { - _stabilization.update( - _ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.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; + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { - } else if (_status.state_machine == SYSTEM_STATE_AUTO) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); @@ -325,10 +328,23 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; + + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + _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( + _ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.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 -- cgit v1.2.3