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-12-06
- Revision:
- 24:7b9e3beb61d5
- Parent:
- 21:169cc2b1d2ff
File content as of revision 24:7b9e3beb61d5:
#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 positions (m) and current positions (m) and velocities (m/s) void HorizontalController::control(float x_r, float y_r, float x, float y, float u, float v) { phi_r = -control_state_regulator(y_r,y,v,kp_y,kd_y)/g; theta_r = control_state_regulator(x_r,x,u,kp_x,kd_x)/g; } // Control acceleration given reference position (m) and current position (m) and velocity (m/s) with given controller gains float HorizontalController::control_state_regulator(float pos_r, float pos, float vel, float kp, float kd) { float acc_r = kp*(pos_r-pos) + kd*(0.0f-vel); return acc_r; }