Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
HorizontalController/HorizontalController.cpp@24:7b9e3beb61d5, 2018-12-06 (annotated)
- Committer:
- fbob
- Date:
- Thu Dec 06 16:44:40 2018 +0000
- Revision:
- 24:7b9e3beb61d5
- Parent:
- 21:169cc2b1d2ff
Adjust
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 8:d05fe9f8bfb6 | 1 | #include "mbed.h" |
fbob | 8:d05fe9f8bfb6 | 2 | #include "HorizontalController.h" |
fbob | 8:d05fe9f8bfb6 | 3 | |
fbob | 8:d05fe9f8bfb6 | 4 | // Class constructor |
fbob | 8:d05fe9f8bfb6 | 5 | HorizontalController::HorizontalController() |
fbob | 8:d05fe9f8bfb6 | 6 | { |
fbob | 15:155ca63b7884 | 7 | phi_r = 0.0f; |
fbob | 15:155ca63b7884 | 8 | theta_r = 0.0f; |
fbob | 8:d05fe9f8bfb6 | 9 | } |
fbob | 8:d05fe9f8bfb6 | 10 | |
fbob | 21:169cc2b1d2ff | 11 | // Control reference roll and pitch angles (rad) given reference positions (m) and current positions (m) and velocities (m/s) |
fbob | 21:169cc2b1d2ff | 12 | void HorizontalController::control(float x_r, float y_r, float x, float y, float u, float v) |
fbob | 8:d05fe9f8bfb6 | 13 | { |
fbob | 21:169cc2b1d2ff | 14 | phi_r = -control_state_regulator(y_r,y,v,kp_y,kd_y)/g; |
fbob | 21:169cc2b1d2ff | 15 | theta_r = control_state_regulator(x_r,x,u,kp_x,kd_x)/g; |
fbob | 16:54d2f299e404 | 16 | } |
fbob | 15:155ca63b7884 | 17 | |
fbob | 21:169cc2b1d2ff | 18 | // Control acceleration given reference position (m) and current position (m) and velocity (m/s) with given controller gains |
fbob | 21:169cc2b1d2ff | 19 | float HorizontalController::control_state_regulator(float pos_r, float pos, float vel, float kp, float kd) |
fbob | 15:155ca63b7884 | 20 | { |
fbob | 21:169cc2b1d2ff | 21 | float acc_r = kp*(pos_r-pos) + kd*(0.0f-vel); |
fbob | 21:169cc2b1d2ff | 22 | return acc_r; |
fbob | 8:d05fe9f8bfb6 | 23 | } |