|
|
|
@ -1,8 +1,9 @@
@@ -1,8 +1,9 @@
|
|
|
|
|
#include "BlockSegwayController.hpp" |
|
|
|
|
|
|
|
|
|
void BlockSegwayController::update() { |
|
|
|
|
void BlockSegwayController::update() |
|
|
|
|
{ |
|
|
|
|
// wait for a sensor update, check for exit condition every 100 ms
|
|
|
|
|
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
|
|
|
|
|
if (poll(&_attPoll, 1, 100) < 0) { return; } // poll error
|
|
|
|
|
|
|
|
|
|
uint64_t newTimeStamp = hrt_absolute_time(); |
|
|
|
|
float dt = (newTimeStamp - _timeStamp) / 1.0e6f; |
|
|
|
@ -10,20 +11,21 @@ void BlockSegwayController::update() {
@@ -10,20 +11,21 @@ void BlockSegwayController::update() {
|
|
|
|
|
|
|
|
|
|
// check for sane values of dt
|
|
|
|
|
// to prevent large control responses
|
|
|
|
|
if (dt > 1.0f || dt < 0) return; |
|
|
|
|
if (dt > 1.0f || dt < 0) { return; } |
|
|
|
|
|
|
|
|
|
// set dt for all child blocks
|
|
|
|
|
setDt(dt); |
|
|
|
|
|
|
|
|
|
// check for new updates
|
|
|
|
|
if (_param_update.updated()) updateParams(); |
|
|
|
|
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++) |
|
|
|
|
for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) { |
|
|
|
|
_actuators.control[i] = 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// only update guidance in auto mode
|
|
|
|
|
if (_status.main_state == MAIN_STATE_AUTO) { |
|
|
|
|