David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.h@42:d09dec5bb184, 2016-05-04 (annotated)

Committer:
ivo_david_michelle
Date:
Wed May 04 21:35:05 2016 +0000
Revision:
42:d09dec5bb184
Parent:
36:40b134328376
Child:
46:4bcf2e679f96
implemented min thrust, and biases

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