DefaultControllerService.h 17.1 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
// These constants deine the behaviour of the intergrator
#define INTEGRATOR_FLAG_ON       1
#define INTEGRATOR_FLAG_OFF      2
#define INTEGRATOR_FLAG_RESET    3
151
152
153
154
155
156
157
158
159
160
161
162




//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------

163
164
165

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

166
167
168
// The current state of the Default Controller
int m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;

169
170
171
172
173
// 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;

174
175
176
177
// The elapased time, incremented by counting the motion
// capture callbacks
float m_time_in_seconds = 0.0;

178
179
180
// PARAMETERS FROM THE YAML FILE

// Max setpoint change per second
181
182
float yaml_max_setpoint_change_per_second_horizontal = 0.80;
float yaml_max_setpoint_change_per_second_vertical   = 0.30;
183

184
185
186
// Max error for z
float yaml_max_setpoint_error_z = 0.4;

187
188
189
190
// Max error for yaw angle
float yaml_max_setpoint_error_yaw_degrees = 60.0;
float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;

191
192
193
194
195
196
197
198
199
// 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;

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

// 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;
208
// The time for: take off spin motors
209
float yaml_takoff_move_up_time = 2.0;
210

211
// Minimum and maximum allowed time for: take off goto setpoint
212
213
float yaml_takoff_goto_setpoint_nearby_time = 1.0;
float yaml_takoff_goto_setpoint_max_time    = 4.0;
214
215
216
217
218
219
220

// 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
221
float yaml_takoff_integrator_on_time = 2.0;
222
223
224
225
226
227
228
229
230
231
232


// 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
233
float yaml_landing_spin_motors_time = 1.5;
234
235


236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254


// 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"
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270

// 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;




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

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

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

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

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

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

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



302
303
// VARIABLES FOR THE CONTROLLER

304
// > A flag for which controller to use:
305
int yaml_controller_method = CONTROLLER_METHOD_RATES;
306

307
308
309
310
311
312
313
314
315
316
317
318
// 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;
319
320
321
322
// Integrator gains
float yaml_integratorGain_forThrust = 0.0;
float yaml_integratorGain_forTauXY  = 0.0;
float yaml_integratorGain_forTauYaw = 0.0;
323
324
325
326
327
328


// VARIABLES FOR THE ESTIMATOR

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

330
331
// > A flag for which estimator to use:
int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
332

333
334
// > The current state interial estimate,
//   for use by the controller
335
float m_current_stateInertialEstimate[9];
336

337
338
339
// > The measurement of the Crazyflie at the "current" time step,
//   to avoid confusion
float m_current_xzy_rpy_measurement[6];
340

341
342
343
344
345
346
// > 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
347
float m_stateInterialEstimate_viaFiniteDifference[9];
348
349
350

// > The full 12 state estimate maintained by the point mass
//   kalman filter state estimator
351
float m_stateInterialEstimate_viaPointMassKalmanFilter[9];
352
353
354
355
356
357
358
359
360

// 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
361
362
363
364

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

365
366
// ROS Publisher to inform the network about
// changes to the setpoint
367
368
369
ros::Publisher m_setpointChangedPublisher;


370
371
372
373
374
// ROS Publisher for sending motors-off command
// to the flying agent client
ros::Publisher m_motorsOffToFlyingAgentClientPublisher;


375
376
377



378
379
380
381




382
383
384
385
386
387
388
389
390
391
392
393
394
395
//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------

396
397
398
399
400
401
// 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)
402

403
404
405
// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response);

406
407
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
408

409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
// > 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] );

425
426
// > This function constructs the error in the body frame
//   before calling the appropriate control function
427
void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateInertial[9], Controller::Response &response, int integrator_flag);
428
// > The various functions that implement an LQR controller
429
void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controller::Response &response, int integrator_flag);
430
431
432
433
434
435
436
437

// 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);
438

439
440
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
// INTO AN (x,y) BODY FRAME ERROR
441
442
443
444
445
446
447
448
449
450
451
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);

452
453
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
454

455
456
457
// PUBLISH THE CURRENT SETPOINT AND STATE
void publishCurrentSetpointAndState();

458
459
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
460

461
462
463
// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
void publish_motors_off_to_flying_agent_client();

464
// LOADING OF YAML PARAMETERS
465
void timerCallback_initial_load_yaml(const ros::TimerEvent&);
466
467
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle);