David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.h@34:eaea0ae92dfa, 2016-04-24 (annotated)

Committer:
ivo_david_michelle
Date:
Sun Apr 24 17:15:20 2016 +0000
Revision:
34:eaea0ae92dfa
Parent:
32:e12b01c94b4a
Child:
35:35997980a8ba
testing with kalman

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