Fabio Bobrow / CrazyflieController

Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer

AttitudeController/AttitudeController.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 0:b1f2c9e88e32 1 #include "mbed.h"
fbob 0:b1f2c9e88e32 2 #include "AttitudeController.h"
fbob 0:b1f2c9e88e32 3
fbob 0:b1f2c9e88e32 4 // Class constructor
fbob 0:b1f2c9e88e32 5 AttitudeController::AttitudeController()
fbob 0:b1f2c9e88e32 6 {
fbob 15:155ca63b7884 7 tau_phi = 0.0f;
fbob 15:155ca63b7884 8 tau_theta = 0.0f;
fbob 15:155ca63b7884 9 tau_psi = 0.0f;
fbob 0:b1f2c9e88e32 10 }
fbob 0:b1f2c9e88e32 11
fbob 17:f682b4a5686d 12 // Control torques (N.m) given reference angles (rad) and current angles (rad) and angular velocities (rad/s)
fbob 2:7e01bc32bf4c 13 void AttitudeController::control(float phi_r, float theta_r, float psi_r, float phi, float theta, float psi, float p, float q, float r)
fbob 0:b1f2c9e88e32 14 {
fbob 21:169cc2b1d2ff 15 tau_phi = control_state_regulator(phi_r, phi, p, kp_phi, kd_phi, I_xx);
fbob 21:169cc2b1d2ff 16 tau_theta = control_state_regulator(theta_r, theta, q, kp_theta, kd_theta, I_yy);
fbob 21:169cc2b1d2ff 17 tau_psi = control_state_regulator(psi_r, psi, r, kp_psi, kd_psi, I_zz);
fbob 0:b1f2c9e88e32 18 }
fbob 0:b1f2c9e88e32 19
fbob 17:f682b4a5686d 20 // Control torque (N.m) given reference angle (rad) and current angle (rad) and angular velocity (rad/s) with given time constants (s) and moment of inertia (kg.m^2)
fbob 21:169cc2b1d2ff 21 float AttitudeController::control_state_regulator(float angle_r, float angle, float rate, float kp, float kd, float I)
fbob 0:b1f2c9e88e32 22 {
fbob 21:169cc2b1d2ff 23 float acc_r = I*(kp*(angle_r-angle)+kd*(0.0f-rate));
fbob 21:169cc2b1d2ff 24 return acc_r;
fbob 3:e782fe31ace2 25 }