diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-08 22:22:22 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-08 22:22:22 +0100 |
| commit | b48f99b601f96321f7200971550ba07177bad5a1 (patch) | |
| tree | f1e0d9fef3629a0a937e9db6a1c984ec3fc3aea2 /apps/controllib/fixedwing.cpp | |
| parent | 780087acf008f1ec6b15d893bb22a546288fa53b (diff) | |
| download | px4-firmware-b48f99b601f96321f7200971550ba07177bad5a1.tar.gz px4-firmware-b48f99b601f96321f7200971550ba07177bad5a1.tar.bz2 px4-firmware-b48f99b601f96321f7200971550ba07177bad5a1.zip | |
Updated state switching to most recent state machine rev
Diffstat (limited to 'apps/controllib/fixedwing.cpp')
| -rw-r--r-- | apps/controllib/fixedwing.cpp | 42 |
1 files changed, 29 insertions, 13 deletions
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 |