fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improved controllib delay block.

Browse Source
This commit is contained in:
James Goppert 2016-03-08 07:27:34 -05:00
parent 5c09c8ede7
commit 02454d0cdc
2 changed files with 46 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/controllib/blocks.cpp
View File

@ -562,7 +562,7 @@ int blockRandGaussTest()
int blockStatsTest()
{
printf("Test BlockStats\t\t: ");
printf("Test BlockStats\t\t\t: ");
BlockStats<float, 1> stats(NULL, "TEST");
ASSERT_CL(equal(0.0f, stats.getMean()(0)));
ASSERT_CL(equal(0.0f, stats.getStdDev()(0)));
@ -579,19 +579,28 @@ int blockStatsTest()
int blockDelayTest()
{
printf("Test BlockDelay\t\t: ");
printf("Test BlockDelay\t\t\t: ");
using namespace matrix;
BlockDelay<float, 2, 3> delay(NULL, "TEST");
Vector2f u1(1, 2);
Vector2f y1 = delay.update(u1);
ASSERT_CL(equal(y1(0), u1(0)));
ASSERT_CL(equal(y1(1), u1(1)));
Vector2f u2(4, 5);
Vector2f y2 = delay.update(u2);
ASSERT_CL(equal(y2(0), u1(0)));
ASSERT_CL(equal(y2(1), u1(1)));
Vector2f u3(7, 8);
Vector2f y3 = delay.update(u3);
ASSERT_CL(equal(y3(0), u1(0)));
ASSERT_CL(equal(y3(1), u1(1)));
Vector2f u4(9, 10);
Vector2f y4 = delay.update(u4);
ASSERT_CL(equal(u1(0), y4(0)));
ASSERT_CL(equal(u1(1), y4(1)));
ASSERT_CL(equal(y4(0), u2(0)));
ASSERT_CL(equal(y4(1), u2(1)));
printf("PASS\n");
return 0;
}

37
src/modules/controllib/blocks.hpp
View File

@ -610,24 +610,53 @@ public:
const char *name) :
Block(parent, name),
_h(),
_index(0)
_index(0),
_delay(-1)
{
};
virtual ~BlockDelay() {};
matrix::Vector<Type, M> update(const matrix::Vector<Type, M> &u)
{
matrix::Vector<Type, M> val = _h[_index];
// store current value
_h[_index] = u;
// delay starts at zero, then increases to LEN
_delay += 1;
if (_delay > (LEN - 1)) {
_delay = LEN - 1;
}
// compute position of delayed value
int j = _index - _delay;
if (j < 0) {
j += LEN;
}
// increment storage position
_index += 1;
if (_index >= LEN) { _index = 0; }
if (_index > (LEN - 1)) {
_index = 0;
}
return val;
// get delayed value
return _h[j];
}
matrix::Vector<Type, M> get()
{
int j = _index - _delay;
if (j < 0) { j += LEN; }
return _h[j];
}
private:
// attributes
matrix::Vector<Type, M> _h[LEN];
size_t _index;
int _delay;
};
int __EXPORT blockDelayTest();