Browse Source

switch the modules implementing the block structure to px4_poll such that in posix_sitl simulation the poll works as expected, blocks the module and doesn't overload the CPU

sbg
MaEtUgR 9 years ago
parent
commit
4f29c7c4db
  1. 2
      src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
  2. 3
      src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
  3. 2
      src/modules/segway/BlockSegwayController.cpp
  4. 3
      src/modules/segway/BlockSegwayController.hpp

2
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@ -221,7 +221,7 @@ void BlockLocalPositionEstimator::update() @@ -221,7 +221,7 @@ void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = poll(_polls, 3, 100);
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
/* poll error, count it in perf */

3
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@ -1,5 +1,6 @@ @@ -1,5 +1,6 @@
#pragma once
#include <px4_posix.h>
#include <controllib/uorb/blocks.hpp>
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
@ -262,7 +263,7 @@ private: @@ -262,7 +263,7 @@ private:
//BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
// misc
struct pollfd _polls[3];
px4_pollfd_struct_t _polls[3];
uint64_t _timeStamp;
//uint64_t _time_last_hist;
uint64_t _time_last_xy;

2
src/modules/segway/BlockSegwayController.cpp

@ -3,7 +3,7 @@ @@ -3,7 +3,7 @@
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 (px4_poll(&_attPoll, 1, 100) < 0) { return; } // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;

3
src/modules/segway/BlockSegwayController.hpp

@ -1,5 +1,6 @@ @@ -1,5 +1,6 @@
#pragma once
#include <px4_posix.h>
#include <controllib/uorb/blocks.hpp>
using namespace control;
@ -22,7 +23,7 @@ private: @@ -22,7 +23,7 @@ private:
enum {CH_LEFT, CH_RIGHT};
BlockPI th2v;
BlockP q2v;
struct pollfd _attPoll;
px4_pollfd_struct_t _attPoll;
uint64_t _timeStamp;
};

Loading…
Cancel
Save