David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.h@48:0346cdff12a7, 2016-05-06 (annotated)

Committer:
ivo_david_michelle
Date:
Fri May 06 01:44:29 2016 +0000
Revision:
48:0346cdff12a7
Parent:
46:4bcf2e679f96 
added initialization to average complementary filter

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