1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
|
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fixedwing.h
*
* Controller library code
*/
#pragma once
#include <controllib/blocks.hpp>
#include <controllib/uorb/blocks.hpp>
extern "C" {
#include <systemlib/geo/geo.h>
}
namespace control
{
namespace fixedwing
{
/**
* BlockYawDamper
*
* This block has more explations to help new developers
* add their own blocks. It includes a limited explanation
* of some C++ basics.
*
* Block: The generic class describing a typical block as you
* would expect in Simulink or ScicosLab. A block can have
* parameters. It cannot have other blocks.
*
* SuperBlock: A superblock is a block that can have other
* blocks. It has methods that manage the blocks beneath it.
*
* BlockYawDamper inherits from SuperBlock publically, this
* means that any public function in SuperBlock are public within
* BlockYawDamper and may be called from outside the
* class methods. Any protected function within block
* are private to the class and may not be called from
* outside this class. Protected should be preferred
* where possible to public as it is important to
* limit access to the bare minimum to prevent
* accidental errors.
*/
class __EXPORT BlockYawDamper : public SuperBlock
{
private:
/**
* Declaring other blocks used by this block
*
* In this section we declare all child blocks that
* this block is composed of. They are private
* members so only this block has direct access to
* them.
*/
BlockLowPass _rLowPass;
BlockHighPass _rWashout;
BlockP _r2Rdr;
/**
* Declaring output values and accessors
*
* If we have any output values for the block we
* declare them here. Output can be directly returned
* through the update function, but outputs should be
* declared here if the information will likely be requested
* again, or if there are multiple outputs.
*
* You should only be able to set outputs from this block,
* so the outputs are declared in the private section.
* A get accessor is provided
* in the public section for other blocks to get the
* value of the output.
*/
float _rudder;
public:
/**
* BlockYawDamper Constructor
*
* The job of the constructor is to initialize all
* parameter in this block and initialize all child
* blocks. Note also, that the output values must be
* initialized here. The order of the members in the
* member initialization list should follow the
* order in which they are declared within the class.
* See the private member declarations above.
*
* Block Construction
*
* All blocks are constructed with their parent block
* and their name. This allows parameters within the
* block to construct a fully qualified name from
* concatenating the two. If the name provided to the
* block is "", then the block will use the parent
* name as it's name. This is useful in cases where
* you have a block that has parameters "MIN", "MAX",
* such as BlockLimit and you do not want an extra name
* to qualify them since the parent block has no "MIN",
* "MAX" parameters.
*
* Block Parameter Construction
*
* Block parameters are named constants, they are
* constructed using:
* BlockParam::BlockParam(Block * parent, const char * name)
* This funciton takes both a parent block and a name.
* The constructore then uses the parent name and the name of
* the paramter to ask the px4 param library if it has any
* parameters with this name. If it does, a handle to the
* parameter is retrieved.
*
* Block/ BlockParam Naming
*
* When desigining new blocks, the naming of the parameters and
* blocks determines the fully qualified name of the parameters
* within the ground station, so it is important to choose
* short, easily understandable names. Again, when a name of
* "" is passed, the parent block name is used as the value to
* prepend to paramters names.
*/
BlockYawDamper(SuperBlock *parent, const char *name);
/**
* Block deconstructor
*
* It is always a good idea to declare a virtual
* deconstructor so that upon calling delete from
* a class derived from this, all of the
* deconstructors all called, the derived class first, and
* then the base class
*/
virtual ~BlockYawDamper();
/**
* Block update function
*
* The job of the update function is to compute the output
* values for the block. In a simple block with one output,
* the output may be returned directly. If the output is
* required frequenly by other processses, it might be a
* good idea to declare a member to store the temporary
* variable.
*/
void update(float rCmd, float r, float outputScale = 1.0);
/**
* Rudder output value accessor
*
* This is a public accessor function, which means that the
* private value _rudder is returned to anyone calling
* BlockYawDamper::getRudder(). Note thate a setRudder() is
* not provided, this is because the updateParams() call
* for a block provides the mechanism for updating the
* paramter.
*/
float getRudder() { return _rudder; }
};
/**
* Stability augmentation system.
* Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
*/
class __EXPORT BlockStabilization : public SuperBlock
{
private:
BlockYawDamper _yawDamper;
BlockLowPass _pLowPass;
BlockLowPass _qLowPass;
BlockP _p2Ail;
BlockP _q2Elv;
float _aileron;
float _elevator;
public:
BlockStabilization(SuperBlock *parent, const char *name);
virtual ~BlockStabilization();
void update(float pCmd, float qCmd, float rCmd,
float p, float q, float r,
float outputScale = 1.0);
float getAileron() { return _aileron; }
float getElevator() { return _elevator; }
float getRudder() { return _yawDamper.getRudder(); }
};
/**
* Frontside/ Backside Control Systems
*
* Frontside :
* velocity error -> throttle
* altitude error -> elevator
*
* Backside :
* velocity error -> elevator
* altitude error -> throttle
*
* Backside control systems are more resilient at
* slow speeds on the back-side of the power
* required curve/ landing etc. Less performance
* than frontside at high speeds.
*/
/**
* Waypoint Guidance block
*/
class __EXPORT BlockWaypointGuidance : public SuperBlock
{
private:
BlockLimitSym _xtYawLimit;
BlockP _xt2Yaw;
float _psiCmd;
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd);
float getPsiCmd() { return _psiCmd; }
};
/**
* Multi-mode Autopilot
*/
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
{
private:
// stabilization
BlockStabilization _stabilization;
// heading hold
BlockP _psi2Phi;
BlockP _phi2P;
BlockLimitSym _phiLimit;
// velocity hold
BlockPID _v2Theta;
BlockPID _theta2Q;
BlockLimit _theLimit;
BlockLimit _vLimit;
// altitude/ climb rate hold
BlockPID _h2Thr;
BlockPID _cr2Thr;
// guidance
BlockWaypointGuidance _guide;
// block params
BlockParam<float> _trimAil;
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
BlockParam<float> _trimV;
BlockParam<float> _vCmd;
BlockParam<float> _crMax;
struct pollfd _attPoll;
vehicle_global_position_set_triplet_s _lastPosCmd;
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
uint64_t _timeStamp;
public:
BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
void update();
virtual ~BlockMultiModeBacksideAutopilot();
};
} // namespace fixedwing
} // namespace control
|