diff options
| author | James Goppert <james.goppert@gmail.com> | 2013-07-28 22:27:05 -0400 |
|---|---|---|
| committer | James Goppert <james.goppert@gmail.com> | 2013-07-28 22:27:05 -0400 |
| commit | dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4 (patch) | |
| tree | a3a3715da3a9bac871241df71a79d7687b862e3f /src/modules/segway/BlockSegwayController.cpp | |
| parent | 1980d9dd63e29390f7c3ba9b31be576c07706f73 (diff) | |
| download | px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.tar.gz px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.tar.bz2 px4-firmware-dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4.zip | |
Segway stabilized.
Diffstat (limited to 'src/modules/segway/BlockSegwayController.cpp')
| -rw-r--r-- | src/modules/segway/BlockSegwayController.cpp | 19 |
1 files changed, 10 insertions, 9 deletions
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b7a0bbbcc..682758a14 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -30,23 +30,24 @@ void BlockSegwayController::update() { // update guidance } - // 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 + // compute speed command + float spdCmd = -theta2spd.update(_att.pitch) - q2spd.update(_att.pitchspeed); // handle autopilot modes if (_status.state_machine == SYSTEM_STATE_AUTO || _status.state_machine == SYSTEM_STATE_STABILIZED) { - - float spdCmd = phi2spd.update(_att.phi); - - // output _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.roll; + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_LEFT] = _manual.throttle; + _actuators.control[CH_RIGHT] = _manual.pitch; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + } } // update all publications |