DefaultControllerService.h 16.7 KB
Newer Older
beuchatp's avatar
beuchatp committed
1
//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
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
//
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//    
//    You should have received a copy of the GNU General Public License
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    The fall-back controller
//
//    ----------------------------------------------------------------------------------





//    ----------------------------------------------------------------------------------
//    III  N   N   CCCC  L      U   U  DDDD   EEEEE   SSSS
//     I   NN  N  C      L      U   U  D   D  E      S
//     I   N N N  C      L      U   U  D   D  EEE     SSS
//     I   N  NN  C      L      U   U  D   D  E          S
//    III  N   N   CCCC  LLLLL   UUU   DDDD   EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// These various headers need to be included so that this controller service can be
// connected with the D-FaLL system.

//some useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <ros/package.h>

// Include the standard message types
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
#include <std_msgs/String.h>

// Include the DFALL message types
59
60
61
62
63
64
65
66
67
#include "dfall_pkg/IntWithHeader.h"
//#include "dfall_pkg/StringWithHeader.h"
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/CustomButtonWithHeader.h"
#include "dfall_pkg/ViconData.h"
#include "dfall_pkg/Setpoint.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/Controller.h"
#include "dfall_pkg/DebugMsg.h"
68
69

// Include the DFALL service types
70
#include "dfall_pkg/IntIntService.h"
71
72
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
73
74
75

// Include the shared definitions
#include "nodes/Constants.h"
76
#include "nodes/DefaultControllerConstants.h"
77
78
79
80
81
82
83
84
85
86
87
88

// Include other classes
#include "classes/GetParamtersAndNamespaces.h"

// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>





// Namespacing the package
89
using namespace dfall_pkg;
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104





//    ----------------------------------------------------------------------------------
//    DDDD   EEEEE  FFFFF  III  N   N  EEEEE   SSSS
//    D   D  E      F       I   NN  N  E      S
//    D   D  EEE    FFF     I   N N N  EEE     SSS
//    D   D  E      F       I   N  NN  E          S
//    DDDD   EEEEE  F      III  N   N  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// These constants are defined to make the code more readable and adaptable.

105
106
// NOTE: many constants are already defined in the "Constant.h" header file

107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
// These constants define the method used for computing
// the control actions from the state error estimates.
// The following is a short description about each mode:
//
// CONTROLLER_METHOD_RATES
//       Uses the poisition, linear velocity and angle
//       error estimates to compute the rates
//
// CONTROLLER_METHOD_RATE_ANGLE_NESTED
//       Uses the position and linear velocity error
//       estimates to compute an angle, and then uses
//       this as a reference to construct an angle error
//       estimate and compute from that the rates
//
#define CONTROLLER_METHOD_RATES               1
#define CONTROLLER_METHOD_RATE_ANGLE_NESTED   2   // (DEFAULT)


// These constants define the method used for estimating
// the Inertial frame state.
// All methods are run at all times, this flag indicates
// which estimate is passed onto the controller.
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
// The following is a short description about each mode:
//
// ESTIMATOR_METHOD_FINITE_DIFFERENCE
//       Takes the poisition and angles directly as measured,
//       and estimates the velocities as a finite different to the
//       previous measurement
//
// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
//       Uses a 2nd order random walk estimator independently for
//       each of (x,y,z,roll,pitch,yaw)
//
// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
//       Uses the model of the quad-rotor and the previous inputs
//
#define ESTIMATOR_METHOD_FINITE_DIFFERENCE          1
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION   2   // (DEFAULT)


147
148
149
150
151
152
153
154
155
156
157
158
159





//    ----------------------------------------------------------------------------------
//    V   V    A    RRRR   III    A    BBBB   L      EEEEE   SSSS
//    V   V   A A   R   R   I    A A   B   B  L      E      S
//    V   V  A   A  RRRR    I   A   A  BBBB   L      EEE     SSS
//     V V   AAAAA  R  R    I   AAAAA  B   B  L      E          S
//      V    A   A  R   R  III  A   A  BBBB   LLLLL  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

160
161
162

// VARIABLES FOR PERFORMING THE TAKE-OFF AND LANDING MANOEUVRES

163
164
165
// The current state of the Default Controller
int m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;

166
167
168
169
170
// A flag for when the state is changed, this is used
// so that a "one-off" operation can be performed
// the first time after changing that state
bool m_current_state_changed = false;

171
172
173
174
// The elapased time, incremented by counting the motion
// capture callbacks
float m_time_in_seconds = 0.0;

175
176
177
178
179
180
// PARAMETERS FROM THE YAML FILE

// Max setpoint change per second
float yaml_max_setpoint_change_per_second_horizontal = 0.1;
float yaml_max_setpoint_change_per_second_vertical = 0.1;

181
182
183
// Max error for z
float yaml_max_setpoint_error_z = 0.4;

184
185
186
187
// Max error for yaw angle
float yaml_max_setpoint_error_yaw_degrees = 60.0;
float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;

188
189
190
191
192
193
194
195
196
// Max {roll,pitch} angle request
float yaml_max_roll_pitch_request_degrees = 30.0;
float yaml_max_roll_pitch_request_radians = 30.0 * DEG2RAD;

// Theshold for {roll,pitch} angle beyond
// which the motors are turned off
float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0;
float yaml_threshold_roll_pitch_for_turn_off_radians = 70.0 * DEG2RAD;

197
198
// The thrust for take off spin motors
float yaml_takeoff_spin_motors_thrust = 8000;
199
200
// The time for: take off spin(-up) motors
float yaml_takoff_spin_motors_time = 0.8;
201
202
203
204

// Height change for the take off move-up
float yaml_takeoff_move_up_start_height = 0.1;
float yaml_takeoff_move_up_end_height   = 0.4;
205
// The time for: take off spin motors
206
207
float yaml_takoff_move_up_time = 1.2;

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
// Minimum and maximum allowed time for: take off goto setpoint
float yaml_takoff_goto_setpoint_min_time = 1.2;
float yaml_takoff_goto_setpoint_max_time = 2.0;

// Box within which to keep the integrator on
// > Units of [meters]
// > The box consider is plus/minus this value
float yaml_takoff_integrator_on_box_horizontal = 0.25;
float yaml_takoff_integrator_on_box_vertical   = 0.15;
// The time for: take off integrator-on
float yaml_takoff_integrator_on_time = 1.5;


// Height change for the landing move-down
float yaml_landing_move_down_end_height_setpoint  = 0.05;
float yaml_landing_move_down_end_height_threshold = 0.10;
// The time for: landing move-down
float yaml_landing_move_down_time_max = 2.0;

// The thrust for landing spin motors
float yaml_landing_spin_motors_thrust = 10000;
// The time for: landing spin motors
float yaml_landing_spin_motors_time = 1.0;


233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252


// The setpoint to be tracked, the ordering is (x,y,z,yaw),
// with units [meters,meters,meters,radians]
float m_setpoint[4] = {0.0,0.0,0.4,0.0};

// The setpoint that is actually used by the controller, this
// differs from the setpoint when smoothing is turned on
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};

// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true;






// ------------------------------------------------------
// VARIABLES THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268

// The ID of the agent that this node is monitoring
int m_agentID;

// The ID of the agent that can coordinate this node
int m_coordID;

// NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string m_namespace_to_coordinator_parameter_service;




269
270
// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
// > the mass of the crazyflie, in [grams]
271
float yaml_cf_mass_in_grams = 25.0;
272
273
// > the weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
274

275
// > the frequency at which the controller is running
276
float yaml_control_frequency = 200.0;
277
float m_control_deltaT = (1.0/200.0);
278

279
280
281
// > the coefficients of the 16-bit command to thrust conversion
std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};

282
// > the min and max for saturating 16 bit thrust commands
283
284
285
float yaml_command_sixteenbit_min = 1000;
float yaml_command_sixteenbit_max = 65000;

286
287
// > the default setpoint, the ordering is (x,y,z,yaw),
//   with units [meters,meters,meters,radians]
288
289
std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};

290
291
292
// Boolean indiciating whether the "Debug Message" of this
// agent should be published or not
bool yaml_shouldPublishDebugMessage = false;
293

294
295
296
// Boolean indiciating whether the debugging ROS_INFO_STREAM
// should be displayed or not
bool yaml_shouldDisplayDebugInfo = false;
297
298
299



300
301
// VARIABLES FOR THE CONTROLLER

302
303
// > A flag for which controller to use:
int yaml_controller_method = CONTROLLER_METHOD_RATE_ANGLE_NESTED;
304

305
306
307
308
309
310
311
312
313
314
315
316
// The LQR Controller parameters for z-height
std::vector<float> yaml_gainMatrixThrust_2StateVector     =  { 0.98, 0.25};
// The LQR Controller parameters for "CONTROLLER_METHOD_RATES"
std::vector<float> yaml_gainMatrixRollRate_3StateVector   =  {-6.20,-3.00, 5.20};
std::vector<float> yaml_gainMatrixPitchRate_3StateVector  =  { 6.20, 3.00, 5.20};
// The LQR Controller parameters for "CONTROLLER_METHOD_RATE_ANGLE_NESTED"
std::vector<float> yaml_gainMatrixRollAngle_2StateVector  =  {-0.20,-0.20};
std::vector<float> yaml_gainMatrixPitchAngle_2StateVector =  { 0.20, 0.20};
float yaml_gainRollRate_fromAngle   =  4.00;
float yaml_gainPitchRate_fromAngle  =  4.00;
// The LQR Controller parameters for yaw
float yaml_gainYawRate_fromAngle    =  2.30;
317
318
319
320
321
322


// VARIABLES FOR THE ESTIMATOR

// Frequency at which the controller is running
float m_estimator_frequency = 200.0;
323

324
325
// > A flag for which estimator to use:
int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
326

327
328
// > The current state interial estimate,
//   for use by the controller
329
float m_current_stateInertialEstimate[9];
330

331
332
333
// > The measurement of the Crazyflie at the "current" time step,
//   to avoid confusion
float m_current_xzy_rpy_measurement[6];
334

335
336
337
338
339
340
// > The measurement of the Crazyflie at the "previous" time step,
//   used for computing finite difference velocities
float m_previous_xzy_rpy_measurement[6];

// > The full 12 state estimate maintained by the finite
//   difference state estimator
341
float m_stateInterialEstimate_viaFiniteDifference[9];
342
343
344

// > The full 12 state estimate maintained by the point mass
//   kalman filter state estimator
345
float m_stateInterialEstimate_viaPointMassKalmanFilter[9];
346
347
348
349
350
351
352
353
354

// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
std::vector<float> yaml_PMKF_Ahat_row1_for_positions  =  {  0.6723, 0.0034};
std::vector<float> yaml_PMKF_Ahat_row2_for_positions  =  {-12.9648, 0.9352};
std::vector<float> yaml_PMKF_Kinf_for_positions       =  {  0.3277,12.9648};


// VARIABLES RELATING TO PUBLISHING
355
356
357
358

// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;

359
360
// ROS Publisher to inform the network about
// changes to the setpoint
361
362
363
ros::Publisher m_setpointChangedPublisher;


364
365
366
367
368
// ROS Publisher for sending motors-off command
// to the flying agent client
ros::Publisher m_motorsOffToFlyingAgentClientPublisher;


369
370
371



372
373
374
375




376
377
378
379
380
381
382
383
384
385
386
387
388
389
//    ----------------------------------------------------------------------------------
//    FFFFF  U   U  N   N   CCCC  TTTTT  III   OOO   N   N
//    F      U   U  NN  N  C        T     I   O   O  NN  N
//    FFF    U   U  N N N  C        T     I   O   O  N N N
//    F      U   U  N  NN  C        T     I   O   O  N  NN
//    F       UUU   N   N   CCCC    T    III   OOO   N   N
//
//    PPPP   RRRR    OOO   TTTTT   OOO   TTTTT  Y   Y  PPPP   EEEEE   SSSS
//    P   P  R   R  O   O    T    O   O    T     Y Y   P   P  E      S
//    PPPP   RRRR   O   O    T    O   O    T      Y    PPPP   EEE     SSS
//    P      R  R   O   O    T    O   O    T      Y    P      E          S
//    P      R   R   OOO     T     OOO     T      Y    P      EEEEE  SSSS
//    ----------------------------------------------------------------------------------

390
391
392
393
394
395
// These function prototypes are not strictly required for this code
// to complile, but adding the function prototypes here means the
// functions can be written in any order in the ".cpp" file.
// (If the function prototypes are not included then the functions
// need to written below in an order that ensure each function is
// implemented before it is called from another function)
396

397
398
399
// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response);

400
401
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
402

403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
// > For the normal state
void computeResponse_for_normal(Controller::Response &response);
// > For the standby state (also used for unknown state)
void computeResponse_for_standby(Controller::Response &response);
// > For the take-off phases
void computeResponse_for_takeoff_move_up(Controller::Response &response);
void computeResponse_for_takeoff_spin_motors(Controller::Response &response);
void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response);
void computeResponse_for_takeoff_integrator_on(Controller::Response &response);
// > For the landing phases
void computeResponse_for_landing_move_down(Controller::Response &response);
void computeResponse_for_landing_spin_motors(Controller::Response &response);


// SMOOTHING SETPOINT CHANGES
void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)[4] );

420
421
422
423
// > This function constructs the error in the body frame
//   before calling the appropriate control function
void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateInertial[12], Controller::Response &response)
// > The various functions that implement an LQR controller
424
void calculateControlOutput_viaLQR_givenError(float stateErrorBody[12], Controller::Response &response);
425
426
427
428
429
430
431
432

// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();

// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
433

434
435
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
// INTO AN (x,y) BODY FRAME ERROR
436
437
438
439
440
441
442
443
444
445
446
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);

// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);

// REQUEST SETPOINT CHANGE CALLBACK
void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);

// CHANGE SETPOINT FUNCTION
void setNewSetpoint(float x, float y, float z, float yaw);

447
448
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
449

450
451
452
// PUBLISH THE CURRENT SETPOINT AND STATE
void publishCurrentSetpointAndState();

453
454
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
455

456
457
458
// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
void publish_motors_off_to_flying_agent_client();

459
// LOADING OF YAML PARAMETERS
460
461
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle);