Joseph Roberts / Mbed 2 deprecated QuadCopter

Quadcopter software. Flying - Contact me for more details or a copy of the PC ground station code

Dependencies:   ConfigFile PID PPM MODSERIAL mbed-rtos mbed MaxbotixDriver TinyGPS filter

hardware.h@3:82665e39f1ea, 2014-09-18 (annotated)

Committer:
joe4465
Date:
Thu Sep 18 08:45:46 2014 +0000
Revision:
3:82665e39f1ea
Parent:
2:b3b771c8f7d1
Child:
6:4c207e7b1203
First revision of quadcopter software

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 0:0010a5abcc31 1 #include "mbed.h"
joe4465 0:0010a5abcc31 2 #include "rtos.h"
joe4465 0:0010a5abcc31 3 #include "FreeIMU.h"
joe4465 0:0010a5abcc31 4 #include "PID.h"
joe4465 0:0010a5abcc31 5 #include "ConfigFile.h"
joe4465 0:0010a5abcc31 6 #include "beep.h"
joe4465 3:82665e39f1ea 7 #include "PPM.h"
joe4465 0:0010a5abcc31 8 #include <sstream>
joe4465 0:0010a5abcc31 9
joe4465 0:0010a5abcc31 10 #ifndef HARDWARE_H
joe4465 0:0010a5abcc31 11 #define HARDWARE_H
joe4465 0:0010a5abcc31 12
joe4465 0:0010a5abcc31 13 //Constants
joe4465 3:82665e39f1ea 14 #define IMU_YAW_ANGLE_MAX 180
joe4465 3:82665e39f1ea 15 #define IMU_YAW_ANGLE_MIN -180
joe4465 3:82665e39f1ea 16 #define IMU_ROLL_ANGLE_MAX 90
joe4465 3:82665e39f1ea 17 #define IMU_ROLL_ANGLE_MIN -90
joe4465 3:82665e39f1ea 18 #define IMU_PITCH_ANGLE_MAX 90
joe4465 3:82665e39f1ea 19 #define IMU_PITCH_ANGLE_MIN -90
joe4465 3:82665e39f1ea 20 #define IMU_YAW_RATE_MAX 360
joe4465 3:82665e39f1ea 21 #define IMU_YAW_RATE_MIN -360
joe4465 3:82665e39f1ea 22 #define IMU_ROLL_RATE_MAX 360
joe4465 3:82665e39f1ea 23 #define IMU_ROLL_RATE_MIN -360
joe4465 3:82665e39f1ea 24 #define IMU_PITCH_RATE_MAX 360
joe4465 3:82665e39f1ea 25 #define IMU_PITCH_RATE_MIN -360
joe4465 3:82665e39f1ea 26
joe4465 3:82665e39f1ea 27 #define RC_CHANNELS 8
joe4465 3:82665e39f1ea 28 #define RC_THROTTLE_CHANNEL 3
joe4465 3:82665e39f1ea 29 #define RC_IN_MAX 1900
joe4465 3:82665e39f1ea 30 #define RC_IN_MIN 1000
joe4465 3:82665e39f1ea 31 #define RC_OUT_MAX 1
joe4465 3:82665e39f1ea 32 #define RC_OUT_MIN 0
joe4465 3:82665e39f1ea 33 #define RC_YAW_RATE_MAX 180
joe4465 3:82665e39f1ea 34 #define RC_YAW_RATE_MIN -180
joe4465 3:82665e39f1ea 35 #define RC_ROLL_RATE_MAX 180
joe4465 3:82665e39f1ea 36 #define RC_ROLL_RATE_MIN -180
joe4465 3:82665e39f1ea 37 #define RC_PITCH_RATE_MAX 180
joe4465 3:82665e39f1ea 38 #define RC_PITCH_RATE_MIN -180
joe4465 3:82665e39f1ea 39 #define RC_ROLL_ANGLE_MAX 45
joe4465 3:82665e39f1ea 40 #define RC_ROLL_ANGLE_MIN -45
joe4465 3:82665e39f1ea 41 #define RC_PITCH_ANGLE_MAX 45
joe4465 3:82665e39f1ea 42 #define RC_PITCH_ANGLE_MIN -45
joe4465 3:82665e39f1ea 43 #define RC_THRUST_MAX 1
joe4465 3:82665e39f1ea 44 #define RC_THRUST_MIN 0
joe4465 3:82665e39f1ea 45
joe4465 3:82665e39f1ea 46 #define MOTORS_OFF 0
joe4465 3:82665e39f1ea 47 #define MOTORS_ARMED 1000
joe4465 3:82665e39f1ea 48 #define MOTORS_MIN 1060
joe4465 3:82665e39f1ea 49 #define MOTORS_MAX 1860
joe4465 3:82665e39f1ea 50
joe4465 3:82665e39f1ea 51 #define RATE_PID_CONTROLLER_OUTPUT_MAX 100
joe4465 3:82665e39f1ea 52 #define RATE_PID_CONTROLLER_OUTPUT_MIN -100
joe4465 3:82665e39f1ea 53
joe4465 3:82665e39f1ea 54 #define FLIGHT_CONTROLLER_FREQUENCY 500
joe4465 3:82665e39f1ea 55 #define MOTOR_PWM_FREQUENCY 500
joe4465 3:82665e39f1ea 56 #define RC_COMMANDS_FREQUENCY 50
joe4465 3:82665e39f1ea 57 #define SERIAL_MONITOR_FREQUENCY 10
joe4465 3:82665e39f1ea 58 #define PING_FREQUENCY 10
joe4465 3:82665e39f1ea 59 #define PIXY_CAM_FREQUENCY 10
joe4465 3:82665e39f1ea 60 #define STATUS_LIGHTS_FREQUENCY 10
joe4465 3:82665e39f1ea 61
joe4465 3:82665e39f1ea 62 //Shared Functions
joe4465 3:82665e39f1ea 63 void ZeroPitchRoll();
joe4465 3:82665e39f1ea 64 void Arm();
joe4465 3:82665e39f1ea 65 void Disarm();
joe4465 3:82665e39f1ea 66 float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax);
joe4465 0:0010a5abcc31 67
joe4465 0:0010a5abcc31 68 //Shared Variables
joe4465 3:82665e39f1ea 69 float _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD;
joe4465 3:82665e39f1ea 70 float _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD;
joe4465 3:82665e39f1ea 71 float _zeroValues[3] = {0,0,0}; //Yaw, pitch, roll
joe4465 3:82665e39f1ea 72 float _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
joe4465 3:82665e39f1ea 73 //float _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
joe4465 3:82665e39f1ea 74 bool _armed = false;
joe4465 3:82665e39f1ea 75 bool _rate = false;
joe4465 3:82665e39f1ea 76 bool _stab = true;
joe4465 3:82665e39f1ea 77 bool _initialised = true;
joe4465 3:82665e39f1ea 78 //bool _gpsConnected = false;
joe4465 3:82665e39f1ea 79 float _motorPower [4] = {0,0,0,0};
joe4465 3:82665e39f1ea 80 float _gyroRate[3] ={}; // Yaw, Pitch, Roll
joe4465 3:82665e39f1ea 81 float _ypr[3] = {0,0,0}; // Yaw, pitch, roll
joe4465 3:82665e39f1ea 82 float _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
joe4465 3:82665e39f1ea 83 float _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
joe4465 3:82665e39f1ea 84
joe4465 3:82665e39f1ea 85 //PID controllers
joe4465 3:82665e39f1ea 86 PID *_yawRatePIDController;
joe4465 3:82665e39f1ea 87 PID *_pitchRatePIDController;
joe4465 3:82665e39f1ea 88 PID *_rollRatePIDController;
joe4465 3:82665e39f1ea 89 PID *_yawStabPIDController;
joe4465 3:82665e39f1ea 90 PID *_pitchStabPIDController;
joe4465 3:82665e39f1ea 91 PID *_rollStabPIDController;
joe4465 0:0010a5abcc31 92
joe4465 3:82665e39f1ea 93 //Threads
joe4465 3:82665e39f1ea 94 Thread *_statusThread;
joe4465 3:82665e39f1ea 95 Thread *_serialPortMonitorThread;
joe4465 3:82665e39f1ea 96 Thread *_flightControllerThread;
joe4465 3:82665e39f1ea 97 Thread *_rcCommandMonitorThread;
joe4465 0:0010a5abcc31 98
joe4465 0:0010a5abcc31 99 //Config file
joe4465 3:82665e39f1ea 100 LocalFileSystem local("local");
joe4465 3:82665e39f1ea 101 ConfigFile _configFile;
joe4465 0:0010a5abcc31 102
joe4465 0:0010a5abcc31 103 //HARDWARE////////////////////////////////////////////////////////////////////////////////////
joe4465 0:0010a5abcc31 104 // M1 M2
joe4465 0:0010a5abcc31 105 // \ /
joe4465 0:0010a5abcc31 106 // \/
joe4465 0:0010a5abcc31 107 // /\
joe4465 0:0010a5abcc31 108 // / \
joe4465 0:0010a5abcc31 109 // M3 M4
joe4465 0:0010a5abcc31 110
joe4465 0:0010a5abcc31 111 //Motors
joe4465 3:82665e39f1ea 112 PwmOut _motor1(p22);
joe4465 3:82665e39f1ea 113 PwmOut _motor2(p23);
joe4465 3:82665e39f1ea 114 PwmOut _motor3(p24);
joe4465 3:82665e39f1ea 115 PwmOut _motor4(p25);
joe4465 0:0010a5abcc31 116
joe4465 0:0010a5abcc31 117 //USB serial
joe4465 3:82665e39f1ea 118 Serial _wiredSerial(USBTX, USBRX);
joe4465 0:0010a5abcc31 119
joe4465 0:0010a5abcc31 120 //Wireless Serial
joe4465 3:82665e39f1ea 121 Serial _wirelessSerial(p9, p10);
joe4465 0:0010a5abcc31 122
joe4465 3:82665e39f1ea 123 //GPS Serial
joe4465 3:82665e39f1ea 124 //Serial _gpsSerial(p28, p27);
joe4465 3:82665e39f1ea 125
joe4465 3:82665e39f1ea 126 //PPM in
joe4465 3:82665e39f1ea 127 PPM *_ppm;
joe4465 3:82665e39f1ea 128 InterruptIn *_interruptPin = new InterruptIn(p5);
joe4465 0:0010a5abcc31 129
joe4465 0:0010a5abcc31 130 //Battery monitor
joe4465 3:82665e39f1ea 131 //DigitalIn _batteryMonitor(p8);
joe4465 0:0010a5abcc31 132
joe4465 0:0010a5abcc31 133 //Onboard LED's
joe4465 3:82665e39f1ea 134 DigitalOut _led1(LED1);
joe4465 3:82665e39f1ea 135 DigitalOut _led2(LED2);
joe4465 3:82665e39f1ea 136 DigitalOut _led3(LED3);
joe4465 3:82665e39f1ea 137 DigitalOut _led4(LED4);
joe4465 0:0010a5abcc31 138
joe4465 3:82665e39f1ea 139 //Outputs
joe4465 3:82665e39f1ea 140 DigitalOut _output1(p11);
joe4465 3:82665e39f1ea 141 DigitalOut _output2(p12);
joe4465 3:82665e39f1ea 142 DigitalOut _output3(p5);
joe4465 3:82665e39f1ea 143 DigitalOut _output4(p6);
joe4465 0:0010a5abcc31 144
joe4465 0:0010a5abcc31 145 //Buzzer
joe4465 3:82665e39f1ea 146 Beep _buzzer(p26);
joe4465 0:0010a5abcc31 147
joe4465 0:0010a5abcc31 148 //IMU
joe4465 3:82665e39f1ea 149 FreeIMU _freeIMU;
joe4465 3:82665e39f1ea 150
joe4465 3:82665e39f1ea 151 //Functions///////////////////////////////////////////////////////////////////////////////////////////////
joe4465 3:82665e39f1ea 152 //Zero gyro, zero yaw and arm
joe4465 3:82665e39f1ea 153 void Arm()
joe4465 3:82665e39f1ea 154 {
joe4465 3:82665e39f1ea 155 //Zero gyro
joe4465 3:82665e39f1ea 156 _freeIMU.zeroGyro();
joe4465 3:82665e39f1ea 157
joe4465 3:82665e39f1ea 158 //Set armed to true
joe4465 3:82665e39f1ea 159 _armed = true;
joe4465 3:82665e39f1ea 160 }
joe4465 3:82665e39f1ea 161
joe4465 3:82665e39f1ea 162 //Set all RC to 0 and disarm
joe4465 3:82665e39f1ea 163 void Disarm()
joe4465 3:82665e39f1ea 164 {
joe4465 3:82665e39f1ea 165 //Set all RC to 0
joe4465 3:82665e39f1ea 166 _rcMappedCommands[0] = 0;
joe4465 3:82665e39f1ea 167 _rcMappedCommands[1] = 0;
joe4465 3:82665e39f1ea 168 _rcMappedCommands[2] = 0;
joe4465 3:82665e39f1ea 169 _rcMappedCommands[3] = 0;
joe4465 3:82665e39f1ea 170
joe4465 3:82665e39f1ea 171 //Set armed to false
joe4465 3:82665e39f1ea 172 _armed = false;
joe4465 3:82665e39f1ea 173 }
joe4465 3:82665e39f1ea 174
joe4465 3:82665e39f1ea 175 //Zero pitch and roll
joe4465 3:82665e39f1ea 176 void ZeroPitchRoll()
joe4465 3:82665e39f1ea 177 {
joe4465 3:82665e39f1ea 178 //Zero pitch and roll
joe4465 3:82665e39f1ea 179 float totalPitch = 0;
joe4465 3:82665e39f1ea 180 float totalRoll = 0;
joe4465 3:82665e39f1ea 181 float ypr[3] = {0,0,0}; // Yaw, pitch, roll
joe4465 3:82665e39f1ea 182 for(int i = 0; i < 500; i++)
joe4465 3:82665e39f1ea 183 {
joe4465 3:82665e39f1ea 184 _freeIMU.getYawPitchRoll(ypr);
joe4465 3:82665e39f1ea 185 totalPitch += ypr[1];
joe4465 3:82665e39f1ea 186 totalRoll += ypr[2];
joe4465 3:82665e39f1ea 187 }
joe4465 3:82665e39f1ea 188
joe4465 3:82665e39f1ea 189 _zeroValues[1] = totalPitch/500;
joe4465 3:82665e39f1ea 190 _zeroValues[2] = totalRoll/500;
joe4465 3:82665e39f1ea 191 printf("Pitch %f\r\n", _zeroValues[1]);
joe4465 3:82665e39f1ea 192 printf("Roll %f\r\n", _zeroValues[2]);
joe4465 3:82665e39f1ea 193 }
joe4465 3:82665e39f1ea 194
joe4465 3:82665e39f1ea 195 float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax)
joe4465 3:82665e39f1ea 196 {
joe4465 3:82665e39f1ea 197 return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
joe4465 3:82665e39f1ea 198 }
joe4465 0:0010a5abcc31 199
joe4465 0:0010a5abcc31 200 #endif