Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
HorizontalController/HorizontalController.cpp
- Committer:
- fbob
- Date:
- 2018-10-05
- Revision:
- 17:f682b4a5686d
- Parent:
- 16:54d2f299e404
- Child:
- 21:169cc2b1d2ff
File content as of revision 17:f682b4a5686d:
#include "mbed.h" #include "HorizontalController.h" // Class constructor HorizontalController::HorizontalController() { phi_r = 0.0f; theta_r = 0.0f; } // Control reference roll and pitch angles (rad) given reference horizontal velocities (m/s) and current horizontal velocities (m/s) void HorizontalController::control(float u_r, float v_r, float u, float v) { phi_r = -control_single(v_r,v,T_v)/g; theta_r = control_single(u_r,u,T_u)/g; } // Control roll and pitch references (rad) given reference horizontal velocities (m/s) and current horizontal velocities (m/s) on take-off void HorizontalController::control_take_off(float u_r, float v_r, float u, float v) { phi_r = -control_single(v_r,v,T_v_take_off)/g; theta_r = control_single(u_r,u,T_u_take_off)/g; } // Control reference angle (rad) given reference horizontal velocity (m/s) and current horizontal velocity (m/s) with given time constant (s) float HorizontalController::control_single(float velocity_r, float velocity, float T_velocity) { return (1.0f/T_velocity)*(velocity_r-velocity); }