PX4 Firmware
PX4 Autopilot Software http://px4.io
BlockDelay.hpp
Go to the documentation of this file.
1
/****************************************************************************
2
*
3
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
4
*
5
* Redistribution and use in source and binary forms, with or without
6
* modification, are permitted provided that the following conditions
7
* are met:
8
*
9
* 1. Redistributions of source code must retain the above copyright
10
* notice, this list of conditions and the following disclaimer.
11
* 2. Redistributions in binary form must reproduce the above copyright
12
* notice, this list of conditions and the following disclaimer in
13
* the documentation and/or other materials provided with the
14
* distribution.
15
* 3. Neither the name PX4 nor the names of its contributors may be
16
* used to endorse or promote products derived from this software
17
* without specific prior written permission.
18
*
19
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30
* POSSIBILITY OF SUCH DAMAGE.
31
*
32
****************************************************************************/
33
34
/**
35
* @file blocks.h
36
*
37
* Controller library code
38
*/
39
40
#pragma once
41
42
#include <px4_platform_common/defines.h>
43
#include <assert.h>
44
#include <time.h>
45
#include <stdlib.h>
46
#include <math.h>
47
#include <
mathlib/math/test/test.hpp
>
48
#include <
mathlib/math/filter/LowPassFilter2p.hpp
>
49
50
#include "
block/Block.hpp
"
51
#include "
block/BlockParam.hpp
"
52
53
#include "
matrix/math.hpp
"
54
55
namespace
control
56
{
57
58
template
<
class
Type,
size_t
M,
size_t
N,
size_t
LEN>
59
class
__EXPORT
BlockDelay
:
public
Block
60
{
61
public
:
62
// methods
63
BlockDelay
(
SuperBlock
*parent,
const
char
*
name
) :
64
Block
(parent, name),
65
_h(),
66
_index(0),
67
_delay(-1)
68
{}
69
virtual
~
BlockDelay
() =
default
;
70
matrix::Matrix<Type, M, N>
update
(
const
matrix::Matrix<Type, M, N>
&u)
71
{
72
// store current value
73
_h[_index] = u;
74
75
// delay starts at zero, then increases to LEN
76
_delay += 1;
77
78
if
(_delay > (
int
)(
LEN
- 1)) {
79
_delay =
LEN
- 1;
80
}
81
82
// compute position of delayed value
83
int
j = _index - _delay;
84
85
if
(j < 0) {
86
j +=
LEN
;
87
}
88
89
// increment storage position
90
_index += 1;
91
92
if
(_index > (
LEN
- 1)) {
93
_index = 0;
94
}
95
96
// get delayed value
97
return
_h[j];
98
}
99
matrix::Matrix<Type, M, N>
get
(
size_t
delay)
100
{
101
int
j = _index - delay;
102
103
if
(j < 0) { j +=
LEN
; }
104
105
return
_h[j];
106
}
107
private
:
108
// attributes
109
matrix::Matrix<Type, M, N>
_h[
LEN
];
110
size_t
_index
;
111
int
_delay
;
112
};
113
114
}
// namespace control
control::BlockDelay
Definition:
BlockDelay.hpp:59
__EXPORT
Definition:
I2C.hpp:51
control::BlockDelay::_delay
int _delay
Definition:
BlockDelay.hpp:111
test.hpp
Controller library code.
Block.hpp
Controller library code.
control::Block
Definition:
Block.hpp:59
control::SuperBlock
Definition:
Block.hpp:94
control::BlockDelay::update
matrix::Matrix< Type, M, N > update(const matrix::Matrix< Type, M, N > &u)
Definition:
BlockDelay.hpp:70
math.hpp
control
Definition:
Block.cpp:45
BlockParam.hpp
Controller library code.
LowPassFilter2p.hpp
name
const char * name
Definition:
tests_main.c:58
matrix::Matrix
Definition:
Matrix.hpp:36
control::BlockDelay::BlockDelay
BlockDelay(SuperBlock *parent, const char *name)
Definition:
BlockDelay.hpp:63
LEN
Definition:
drv_tap_esc.h:245
control::BlockDelay::_index
size_t _index
Definition:
BlockDelay.hpp:110
src
lib
controllib
BlockDelay.hpp
Generated by
1.8.13