Fabio Bobrow / CrazyflieController

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?

UserRevisionLine numberNew 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 }