David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.h@36:40b134328376, 2016-04-29 (annotated)

Committer:
ivo_david_michelle
Date:
Fri Apr 29 18:24:15 2016 +0000
Revision:
36:40b134328376
Parent:
35:35997980a8ba
Child:
42:d09dec5bb184
before testing performance

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 6:6f3ffd97d808 1 #ifndef QUADCOPTER_H
ivo_david_michelle 6:6f3ffd97d808 2 #define QUADCOPTER_H
ivo_david_michelle 6:6f3ffd97d808 3
ivo_david_michelle 7:f3f94eadc5b5 4 #include "mbed.h"
ivo_david_michelle 7:f3f94eadc5b5 5 #include "Adafruit_9DOF.h"
ivo_david_michelle 9:f1bd96708a21 6 #include "Serial_base.h"
ivo_david_michelle 14:64b06476d943 7 #include "MRF24J40.h"
ivo_david_michelle 34:eaea0ae92dfa 8 #include "Kalman.h"
ivo_david_michelle 36:40b134328376 9 #include "rtos.h"
ivo_david_michelle 14:64b06476d943 10
ivo_david_michelle 36:40b134328376 11 #define FILTER_SIZE 1
ivo_david_michelle 32:e12b01c94b4a 12
ivo_david_michelle 13:291ba30c7806 13 // a few struct declarations. Can possibly eliminated using the vector class in STL
ivo_david_michelle 7:f3f94eadc5b5 14 struct state {
ivo_david_michelle 10:e7d1801e966a 15 double phi; // roll
ivo_david_michelle 10:e7d1801e966a 16 double theta; // pitch
ivo_david_michelle 10:e7d1801e966a 17 double psi; // yaw
ivo_david_michelle 10:e7d1801e966a 18 double p; // omega x
ivo_david_michelle 10:e7d1801e966a 19 double q; // omega y
ivo_david_michelle 10:e7d1801e966a 20 double r; // omega z
ivo_david_michelle 6:6f3ffd97d808 21 };
ivo_david_michelle 7:f3f94eadc5b5 22
ivo_david_michelle 32:e12b01c94b4a 23 struct filter {
ivo_david_michelle 32:e12b01c94b4a 24 double phi[FILTER_SIZE]; // roll
ivo_david_michelle 32:e12b01c94b4a 25 double theta[FILTER_SIZE]; // pitch
ivo_david_michelle 32:e12b01c94b4a 26 double psi[FILTER_SIZE]; // yaw
ivo_david_michelle 32:e12b01c94b4a 27 double p[FILTER_SIZE]; // omega x
ivo_david_michelle 32:e12b01c94b4a 28 double q[FILTER_SIZE]; // omega y
ivo_david_michelle 32:e12b01c94b4a 29 double r[FILTER_SIZE]; // omega z
ivo_david_michelle 32:e12b01c94b4a 30 };
ivo_david_michelle 32:e12b01c94b4a 31
ivo_david_michelle 10:e7d1801e966a 32 struct controlInput {
ivo_david_michelle 10:e7d1801e966a 33 double f;
ivo_david_michelle 10:e7d1801e966a 34 double mx;
ivo_david_michelle 10:e7d1801e966a 35 double my;
ivo_david_michelle 10:e7d1801e966a 36 double mz;
ivo_david_michelle 7:f3f94eadc5b5 37 };
ivo_david_michelle 7:f3f94eadc5b5 38
ivo_david_michelle 14:64b06476d943 39 typedef struct {
ivo_david_michelle 24:e220fbb70ded 40 double gyro_x;
ivo_david_michelle 24:e220fbb70ded 41 double gyro_y;
ivo_david_michelle 24:e220fbb70ded 42 double gyro_z;
ivo_david_michelle 24:e220fbb70ded 43 double roll;
ivo_david_michelle 24:e220fbb70ded 44 double pitch;
ivo_david_michelle 24:e220fbb70ded 45 double heading;
ivo_david_michelle 14:64b06476d943 46 } offset;
ivo_david_michelle 7:f3f94eadc5b5 47
ivo_david_michelle 11:5c54826d23a7 48 struct motors {
ivo_david_michelle 11:5c54826d23a7 49 double m1;
ivo_david_michelle 11:5c54826d23a7 50 double m2;
ivo_david_michelle 11:5c54826d23a7 51 double m3;
ivo_david_michelle 11:5c54826d23a7 52 double m4;
ivo_david_michelle 13:291ba30c7806 53 };
ivo_david_michelle 10:e7d1801e966a 54
ivo_david_michelle 10:e7d1801e966a 55
ivo_david_michelle 24:e220fbb70ded 56 class Quadcopter
ivo_david_michelle 24:e220fbb70ded 57 {
ivo_david_michelle 6:6f3ffd97d808 58 private:
ivo_david_michelle 14:64b06476d943 59 Serial *pc_; // use for printing
ivo_david_michelle 24:e220fbb70ded 60
ivo_david_michelle 14:64b06476d943 61 // allow for remote communication
ivo_david_michelle 14:64b06476d943 62 MRF24J40 *mrf_;
ivo_david_michelle 14:64b06476d943 63 int rcIterations_;
ivo_david_michelle 14:64b06476d943 64 int rcLength_;
ivo_david_michelle 14:64b06476d943 65 Timer rcTimer_;
ivo_david_michelle 34:eaea0ae92dfa 66 Timer *controlTimer;
ivo_david_michelle 34:eaea0ae92dfa 67
ivo_david_michelle 36:40b134328376 68 Mutex *desired_mutex;
ivo_david_michelle 36:40b134328376 69
ivo_david_michelle 34:eaea0ae92dfa 70 Kalman kalmanRoll;
ivo_david_michelle 34:eaea0ae92dfa 71 Kalman kalmanPitch;
ivo_david_michelle 14:64b06476d943 72
ivo_david_michelle 32:e12b01c94b4a 73 filter filters_;
ivo_david_michelle 8:326e7009ce0c 74 state state_;
ivo_david_michelle 10:e7d1801e966a 75 state desiredState_;
ivo_david_michelle 29:ae765492fa8b 76 double F_des_; // desired thrust force (excluding weight compensation)
ivo_david_michelle 10:e7d1801e966a 77 controlInput controlInput_;
ivo_david_michelle 11:5c54826d23a7 78 motors motorPwm_;
ivo_david_michelle 24:e220fbb70ded 79
ivo_david_michelle 13:291ba30c7806 80 // declarations for IMU use
ivo_david_michelle 10:e7d1801e966a 81 Adafruit_9DOF dof_;
ivo_david_michelle 10:e7d1801e966a 82 Adafruit_LSM303_Accel_Unified accel_;
ivo_david_michelle 10:e7d1801e966a 83 Adafruit_LSM303_Mag_Unified mag_;
ivo_david_michelle 10:e7d1801e966a 84 Adafruit_L3GD20_Unified gyro_;
ivo_david_michelle 10:e7d1801e966a 85 sensors_event_t accel_event_;
ivo_david_michelle 9:f1bd96708a21 86 sensors_event_t mag_event_;
ivo_david_michelle 9:f1bd96708a21 87 sensors_event_t gyro_event_;
ivo_david_michelle 9:f1bd96708a21 88 sensors_vec_t orientation_;
ivo_david_michelle 24:e220fbb70ded 89 offset* initial_offsets_;
ivo_david_michelle 9:f1bd96708a21 90
ivo_david_michelle 10:e7d1801e966a 91 double g_; // gravity [m/s^2]
ivo_david_michelle 10:e7d1801e966a 92 double m_; // mass [kg]
ivo_david_michelle 20:efa15ed008b4 93 double l_; // arm lenght [m]
ivo_david_michelle 20:efa15ed008b4 94 double gamma_; // relation between moment coefficient and force coefficient.
ivo_david_michelle 24:e220fbb70ded 95
ivo_david_michelle 10:e7d1801e966a 96 // proportional attitude control gains
ivo_david_michelle 21:336faf452989 97 double kp_f_; // gain converting radio input to thrust.
ivo_david_michelle 10:e7d1801e966a 98 double kp_phi_;
ivo_david_michelle 10:e7d1801e966a 99 double kp_theta_;
ivo_david_michelle 10:e7d1801e966a 100 double kp_psi_;
ivo_david_michelle 10:e7d1801e966a 101
ivo_david_michelle 10:e7d1801e966a 102 // derivative attitude control gains
ivo_david_michelle 10:e7d1801e966a 103 double kd_phi_;
ivo_david_michelle 10:e7d1801e966a 104 double kd_theta_;
ivo_david_michelle 10:e7d1801e966a 105 double kd_psi_;
ivo_david_michelle 27:11116aa69f32 106
ivo_david_michelle 31:d473eacfc271 107 double ki_phi_;
ivo_david_michelle 31:d473eacfc271 108 double ki_theta_;
ivo_david_michelle 31:d473eacfc271 109 //double ki_psi_;
ivo_david_michelle 31:d473eacfc271 110 double i_e_phi_;
ivo_david_michelle 31:d473eacfc271 111 double i_e_theta_;
ivo_david_michelle 31:d473eacfc271 112 float prev_time_;
ivo_david_michelle 34:eaea0ae92dfa 113
ivo_david_michelle 34:eaea0ae92dfa 114 float prev_kalman_time;
ivo_david_michelle 34:eaea0ae92dfa 115 double compAngleX;
ivo_david_michelle 34:eaea0ae92dfa 116 double compAngleY;
ivo_david_michelle 31:d473eacfc271 117
ivo_david_michelle 31:d473eacfc271 118
ivo_david_michelle 27:11116aa69f32 119 double zeroVelPwm;
ivo_david_michelle 27:11116aa69f32 120 double maxPwm;
ivo_david_michelle 31:d473eacfc271 121 double max_integral_phi_;
ivo_david_michelle 31:d473eacfc271 122 double max_integral_theta_;
ivo_david_michelle 31:d473eacfc271 123
ivo_david_michelle 27:11116aa69f32 124
ivo_david_michelle 14:64b06476d943 125 public:
ivo_david_michelle 36:40b134328376 126 Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer* timer, Mutex *desired); // constructor
ivo_david_michelle 7:f3f94eadc5b5 127
ivo_david_michelle 34:eaea0ae92dfa 128 void controller();
ivo_david_michelle 8:326e7009ce0c 129 void readSensorValues();
ivo_david_michelle 14:64b06476d943 130 void readRc();
ivo_david_michelle 14:64b06476d943 131
ivo_david_michelle 11:5c54826d23a7 132 motors getPwm();
ivo_david_michelle 24:e220fbb70ded 133 state getState();
ivo_david_michelle 24:e220fbb70ded 134 Adafruit_LSM303_Accel_Unified getAccel();
ivo_david_michelle 24:e220fbb70ded 135 Adafruit_LSM303_Mag_Unified getMag();
ivo_david_michelle 24:e220fbb70ded 136 Adafruit_L3GD20_Unified getGyro();
ivo_david_michelle 24:e220fbb70ded 137 Adafruit_9DOF getIMU();
ivo_david_michelle 24:e220fbb70ded 138 offset* getOffset();
ivo_david_michelle 27:11116aa69f32 139 double getForce();
ivo_david_michelle 6:6f3ffd97d808 140 };
ivo_david_michelle 7:f3f94eadc5b5 141
ivo_david_michelle 6:6f3ffd97d808 142 #endif