diff options
author | James Goppert <james.goppert@gmail.com> | 2013-07-28 01:35:43 -0400 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-07-28 01:35:43 -0400 |
commit | 1980d9dd63e29390f7c3ba9b31be576c07706f73 (patch) | |
tree | 22b8cebc3474bc897555db19c2810e1e102a9f15 /src/modules/segway/BlockSegwayController.cpp | |
parent | 95aa82f586a8c44c53ae48517efdeb5e5673b7b5 (diff) | |
download | px4-firmware-1980d9dd63e29390f7c3ba9b31be576c07706f73.tar.gz px4-firmware-1980d9dd63e29390f7c3ba9b31be576c07706f73.tar.bz2 px4-firmware-1980d9dd63e29390f7c3ba9b31be576c07706f73.zip |
Working on segway controller, restructure of fixedwing.
Diffstat (limited to 'src/modules/segway/BlockSegwayController.cpp')
-rw-r--r-- | src/modules/segway/BlockSegwayController.cpp | 56 |
1 files changed, 56 insertions, 0 deletions
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp new file mode 100644 index 000000000..b7a0bbbcc --- /dev/null +++ b/src/modules/segway/BlockSegwayController.cpp @@ -0,0 +1,56 @@ +#include "BlockSegwayController.hpp" + +void BlockSegwayController::update() { + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 2; 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 + } + + // 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_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; + } + + // update all publications + updatePublications(); + +} + |