Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
HorizontalController/HorizontalController.cpp@17:f682b4a5686d, 2018-10-05 (annotated)
- Committer:
- fbob
- Date:
- Fri Oct 05 19:32:35 2018 +0000
- Revision:
- 17:f682b4a5686d
- Parent:
- 16:54d2f299e404
- Child:
- 21:169cc2b1d2ff
Comments;
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 | 17:f682b4a5686d | 11 | // Control reference roll and pitch angles (rad) given reference horizontal velocities (m/s) and current horizontal velocities (m/s) |
fbob | 8:d05fe9f8bfb6 | 12 | void HorizontalController::control(float u_r, float v_r, float u, float v) |
fbob | 8:d05fe9f8bfb6 | 13 | { |
fbob | 16:54d2f299e404 | 14 | phi_r = -control_single(v_r,v,T_v)/g; |
fbob | 8:d05fe9f8bfb6 | 15 | theta_r = control_single(u_r,u,T_u)/g; |
fbob | 16:54d2f299e404 | 16 | } |
fbob | 15:155ca63b7884 | 17 | |
fbob | 17:f682b4a5686d | 18 | // Control roll and pitch references (rad) given reference horizontal velocities (m/s) and current horizontal velocities (m/s) on take-off |
fbob | 16:54d2f299e404 | 19 | void HorizontalController::control_take_off(float u_r, float v_r, float u, float v) |
fbob | 15:155ca63b7884 | 20 | { |
fbob | 16:54d2f299e404 | 21 | phi_r = -control_single(v_r,v,T_v_take_off)/g; |
fbob | 16:54d2f299e404 | 22 | theta_r = control_single(u_r,u,T_u_take_off)/g; |
fbob | 8:d05fe9f8bfb6 | 23 | } |
fbob | 8:d05fe9f8bfb6 | 24 | |
fbob | 17:f682b4a5686d | 25 | // Control reference angle (rad) given reference horizontal velocity (m/s) and current horizontal velocity (m/s) with given time constant (s) |
fbob | 8:d05fe9f8bfb6 | 26 | float HorizontalController::control_single(float velocity_r, float velocity, float T_velocity) |
fbob | 8:d05fe9f8bfb6 | 27 | { |
fbob | 8:d05fe9f8bfb6 | 28 | return (1.0f/T_velocity)*(velocity_r-velocity); |
fbob | 8:d05fe9f8bfb6 | 29 | } |