Fabio Bobrow / CrazyflieController

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

AttitudeController/AttitudeController.cpp@0:b1f2c9e88e32, 2018-08-31 (annotated)

Committer:
fbob
Date:
Fri Aug 31 18:41:31 2018 +0000
Revision:
0:b1f2c9e88e32
Child:
2:7e01bc32bf4c
Include attitude controller, attitude estimator and mixer

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 0:b1f2c9e88e32 7 }
fbob 0:b1f2c9e88e32 8
fbob 0:b1f2c9e88e32 9 // Calculate torques given reference angles and current angles and rates
fbob 0:b1f2c9e88e32 10 void AttitudeController::calculate(float phi_r, float theta_r, float psi_r, float phi, float theta, float psi, float p, float q, float r)
fbob 0:b1f2c9e88e32 11 {
fbob 0:b1f2c9e88e32 12 // Calculate torque given reference angle and current angle and rate (with given gains and moment of inertia)
fbob 0:b1f2c9e88e32 13 tau_phi = calculate_single(phi_r, phi, p, K_phi, K_p, I_xx);
fbob 0:b1f2c9e88e32 14 tau_theta = calculate_single(theta_r, theta, q, K_theta, K_q, I_yy);
fbob 0:b1f2c9e88e32 15 tau_psi = calculate_single(psi_r, psi, r, K_psi, K_r, I_zz);
fbob 0:b1f2c9e88e32 16 }
fbob 0:b1f2c9e88e32 17
fbob 0:b1f2c9e88e32 18 // Calculate torque given reference angle and current angle and rate (with given gains and moment of inertia)
fbob 0:b1f2c9e88e32 19 float AttitudeController::calculate_single(float angle_r, float angle, float rate, float K_angle, float K_rate, float I)
fbob 0:b1f2c9e88e32 20 {
fbob 0:b1f2c9e88e32 21 // Calculate torque
fbob 0:b1f2c9e88e32 22 float rate_r = K_angle*(angle_r-angle);
fbob 0:b1f2c9e88e32 23 float accel_r = K_rate*(rate_r-rate);
fbob 0:b1f2c9e88e32 24 float tau = accel_r*I;
fbob 0:b1f2c9e88e32 25 return tau;
fbob 0:b1f2c9e88e32 26 }