diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-08-15 10:33:45 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-08-15 10:33:45 +0200 |
commit | 7476b03543f879df5ea29d44be147ff4926f8216 (patch) | |
tree | e1c056fa3e12b2c2c3ae601ae2835ac086b60338 /src/modules/segway/BlockSegwayController.cpp | |
parent | 39ae01dd07d53e3509826ae3737fc6a509adec34 (diff) | |
parent | d2f19c7d84030ad6ed1f6c17538fa96864c5dcef (diff) | |
download | px4-firmware-7476b03543f879df5ea29d44be147ff4926f8216.tar.gz px4-firmware-7476b03543f879df5ea29d44be147ff4926f8216.tar.bz2 px4-firmware-7476b03543f879df5ea29d44be147ff4926f8216.zip |
Merge branch 'master' into new_state_machine_drton
Diffstat (limited to 'src/modules/segway/BlockSegwayController.cpp')
-rw-r--r-- | src/modules/segway/BlockSegwayController.cpp | 57 |
1 files changed, 57 insertions, 0 deletions
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp new file mode 100644 index 000000000..b1dc39445 --- /dev/null +++ b/src/modules/segway/BlockSegwayController.cpp @@ -0,0 +1,57 @@ +#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 + } + + // compute speed command + float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + 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 + updatePublications(); + +} + |