Leo Veenstra / Mbed 2 deprecated Quad

Quadcopter control - WIP

Dependencies:   ADXL345_I2C2 ITG32003 USBDevice mbed

main.cpp@1:939b3a2453a6, 2013-10-22 (annotated)

Committer:
SED9008
Date:
Tue Oct 22 21:30:58 2013 +0000
Revision:
1:939b3a2453a6
Parent:
0:b888fb67c9cf 
qwe

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SED9008 0:b888fb67c9cf 1 #include "mbed.h"
SED9008 0:b888fb67c9cf 2 #include "ADXL345_I2C.h"
SED9008 0:b888fb67c9cf 3 #include "ITG3200.h"
SED9008 0:b888fb67c9cf 4
SED9008 0:b888fb67c9cf 5 #define PI 3.14159265358979f
SED9008 0:b888fb67c9cf 6 #define FILTER_FREQ 800
SED9008 0:b888fb67c9cf 7 #define FILTER_SPLIT 0.95
SED9008 0:b888fb67c9cf 8 #define ANGLE_SAMPLES 8
SED9008 0:b888fb67c9cf 9
SED9008 0:b888fb67c9cf 10 //Pin Defines
SED9008 0:b888fb67c9cf 11 DigitalOut led(LED1);
SED9008 0:b888fb67c9cf 12 Serial pc(USBTX,USBRX);
SED9008 0:b888fb67c9cf 13 PwmOut m0(p21);
SED9008 0:b888fb67c9cf 14 PwmOut m1(p22);
SED9008 0:b888fb67c9cf 15 PwmOut m2(p23);
SED9008 0:b888fb67c9cf 16 PwmOut m3(p24);
SED9008 0:b888fb67c9cf 17 ADXL345_I2C accelerometer(p9, p10);
SED9008 0:b888fb67c9cf 18 ITG3200 gyroscope(p9, p10);
SED9008 0:b888fb67c9cf 19
SED9008 0:b888fb67c9cf 20 Ticker filter_tick;
SED9008 0:b888fb67c9cf 21 Ticker debug_tick;
SED9008 0:b888fb67c9cf 22
SED9008 0:b888fb67c9cf 23 //Prototypes
SED9008 0:b888fb67c9cf 24 void accelSetup(void);
SED9008 0:b888fb67c9cf 25 void accelSample(void);
SED9008 0:b888fb67c9cf 26 void accelCal(int samples);
SED9008 0:b888fb67c9cf 27 void accelAngle(void);
SED9008 0:b888fb67c9cf 28 void gyroSetup(void);
SED9008 0:b888fb67c9cf 29 void gyroSample(void);
SED9008 0:b888fb67c9cf 30 void gyroCal(int samples);
SED9008 0:b888fb67c9cf 31 void angleIni(void);
SED9008 0:b888fb67c9cf 32
SED9008 0:b888fb67c9cf 33 //Vars
SED9008 0:b888fb67c9cf 34 float accel[3];
SED9008 0:b888fb67c9cf 35 float accel_offset[3];
SED9008 0:b888fb67c9cf 36 float gyro[3];
SED9008 0:b888fb67c9cf 37 float gyro_offset[3];
SED9008 0:b888fb67c9cf 38 float angle[2];
SED9008 0:b888fb67c9cf 39 float angle_buf[2];
SED9008 0:b888fb67c9cf 40 float angle_sam[2][ANGLE_SAMPLES];
SED9008 0:b888fb67c9cf 41 float last_sample[2];
SED9008 0:b888fb67c9cf 42
SED9008 0:b888fb67c9cf 43 int buf_cnt = 0;
SED9008 0:b888fb67c9cf 44
SED9008 0:b888fb67c9cf 45 void filter(void){
SED9008 0:b888fb67c9cf 46 gyroSample();
SED9008 0:b888fb67c9cf 47 accelSample();
SED9008 0:b888fb67c9cf 48 accelAngle();
SED9008 0:b888fb67c9cf 49 for(int h=0;h<2;h++){
SED9008 0:b888fb67c9cf 50 float tmpf = (float)FILTER_SPLIT*(angle[h]+(gyro[h]/(float)FILTER_FREQ))+(1-(float)FILTER_SPLIT)*accel[h];
SED9008 0:b888fb67c9cf 51 angle_buf[h] += tmpf - angle_sam[h][buf_cnt];
SED9008 0:b888fb67c9cf 52 angle_sam[h][buf_cnt] = tmpf;
SED9008 0:b888fb67c9cf 53 angle[h] = angle_buf[h]/ANGLE_SAMPLES;
SED9008 0:b888fb67c9cf 54 }
SED9008 0:b888fb67c9cf 55 if(buf_cnt++ >= ANGLE_SAMPLES-1) buf_cnt = 0;
SED9008 0:b888fb67c9cf 56 led = !led;
SED9008 0:b888fb67c9cf 57 }
SED9008 0:b888fb67c9cf 58
SED9008 0:b888fb67c9cf 59 void debug(void){
SED9008 0:b888fb67c9cf 60 pc.printf("x:%6.3f y:%6.3f\n\r",angle[0],angle[1]);
SED9008 0:b888fb67c9cf 61 }
SED9008 0:b888fb67c9cf 62
SED9008 0:b888fb67c9cf 63 int main() {
SED9008 0:b888fb67c9cf 64 accelSetup();
SED9008 0:b888fb67c9cf 65 gyroSetup();
SED9008 0:b888fb67c9cf 66
SED9008 0:b888fb67c9cf 67 gyroCal(100);
SED9008 0:b888fb67c9cf 68
SED9008 0:b888fb67c9cf 69 pc.printf("Calibrated\n\r");
SED9008 0:b888fb67c9cf 70 while(1) if(pc.readable()) break;
SED9008 0:b888fb67c9cf 71
SED9008 0:b888fb67c9cf 72 angleIni();
SED9008 0:b888fb67c9cf 73
SED9008 0:b888fb67c9cf 74 pc.printf("x:%5.3f y:%5.3f\n\r",angle[0],angle[1]);
SED9008 0:b888fb67c9cf 75 wait(0.5);
SED9008 0:b888fb67c9cf 76 pc.printf("%d - %d\n\r",(int)(1000000/FILTER_FREQ),ANGLE_SAMPLES);
SED9008 0:b888fb67c9cf 77 wait(0.5);
SED9008 0:b888fb67c9cf 78
SED9008 0:b888fb67c9cf 79 debug_tick.attach(&debug, 0.1);
SED9008 0:b888fb67c9cf 80 filter_tick.attach_us(&filter, (int)(1000000/FILTER_FREQ));
SED9008 0:b888fb67c9cf 81
SED9008 0:b888fb67c9cf 82 while(1) {
SED9008 0:b888fb67c9cf 83 // pc.printf("x:%5.3f y:%5.3f \n\r",angle[0],angle[1]);
SED9008 0:b888fb67c9cf 84 // wait_us(1000);
SED9008 0:b888fb67c9cf 85 }
SED9008 0:b888fb67c9cf 86 }
SED9008 0:b888fb67c9cf 87
SED9008 0:b888fb67c9cf 88 void accelSetup(void) {
SED9008 0:b888fb67c9cf 89 accelerometer.setPowerControl(0x00); //Go into standby mode to configure the device.
SED9008 0:b888fb67c9cf 90 accelerometer.setDataFormatControl(0x0B); //Full resolution, +/-16g, 4mg/LSB.
SED9008 0:b888fb67c9cf 91 accelerometer.setDataRate(ADXL345_200HZ); //200Hz data rate.
SED9008 0:b888fb67c9cf 92 accelerometer.setPowerControl(0x08); //Measurement mode.
SED9008 0:b888fb67c9cf 93 wait_ms(22); //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
SED9008 0:b888fb67c9cf 94 }
SED9008 0:b888fb67c9cf 95
SED9008 0:b888fb67c9cf 96 void accelSample(void){
SED9008 0:b888fb67c9cf 97 int readings[3];
SED9008 0:b888fb67c9cf 98 accelerometer.getOutput(readings);
SED9008 0:b888fb67c9cf 99 for(int h=0;h<3;h++) accel[h] = ((float)(int16_t)readings[h]/256)-accel_offset[h];
SED9008 0:b888fb67c9cf 100 }
SED9008 0:b888fb67c9cf 101
SED9008 0:b888fb67c9cf 102 void accelCal(int samples){
SED9008 0:b888fb67c9cf 103 float temp[3] = {0,0,0};
SED9008 0:b888fb67c9cf 104 for(int h=0;h<=samples;h++){
SED9008 0:b888fb67c9cf 105 accelSample();
SED9008 0:b888fb67c9cf 106 for(int j=0;j<=2;j++) temp[j] += accel[j];
SED9008 0:b888fb67c9cf 107 }
SED9008 0:b888fb67c9cf 108 for(int h=0;h<=2;h++) accel_offset[h] = temp[h]/samples;
SED9008 0:b888fb67c9cf 109 accel_offset[2] -= 1;
SED9008 0:b888fb67c9cf 110 }
SED9008 0:b888fb67c9cf 111
SED9008 0:b888fb67c9cf 112 void accelAngle(void){
SED9008 0:b888fb67c9cf 113 accel[0] = atan2(accel[2],accel[0])*(180/(float)PI)-90;
SED9008 0:b888fb67c9cf 114 accel[1] = atan2(accel[2],accel[1])*(180/(float)PI)-90;
SED9008 0:b888fb67c9cf 115 }
SED9008 0:b888fb67c9cf 116
SED9008 0:b888fb67c9cf 117 void gyroSetup(void){
SED9008 0:b888fb67c9cf 118 gyroscope.setLpBandwidth(LPFBW_42HZ); //Set highest bandwidth.
SED9008 0:b888fb67c9cf 119 wait_ms(100);
SED9008 0:b888fb67c9cf 120 }
SED9008 0:b888fb67c9cf 121
SED9008 0:b888fb67c9cf 122 void gyroSample(void){
SED9008 0:b888fb67c9cf 123 gyro[0] = (gyroscope.getGyroX()-gyro_offset[0])/14.375;
SED9008 0:b888fb67c9cf 124 gyro[1] = (gyroscope.getGyroY()-gyro_offset[1])/14.375;
SED9008 0:b888fb67c9cf 125 gyro[2] = (gyroscope.getGyroZ()-gyro_offset[2])/14.375;
SED9008 0:b888fb67c9cf 126 }
SED9008 0:b888fb67c9cf 127
SED9008 0:b888fb67c9cf 128 void gyroCal(int samples){
SED9008 0:b888fb67c9cf 129 int temp[3] = {0,0,0};
SED9008 0:b888fb67c9cf 130 for(int h=0;h<=samples;h++){
SED9008 0:b888fb67c9cf 131 temp[0] += gyroscope.getGyroX();
SED9008 0:b888fb67c9cf 132 temp[1] += gyroscope.getGyroY();
SED9008 0:b888fb67c9cf 133 temp[2] += gyroscope.getGyroZ();
SED9008 0:b888fb67c9cf 134 }
SED9008 0:b888fb67c9cf 135 for(int h=0;h<=2;h++) gyro_offset[h] = temp[h]/samples;
SED9008 0:b888fb67c9cf 136 }
SED9008 0:b888fb67c9cf 137
SED9008 0:b888fb67c9cf 138 void angleIni(void){
SED9008 0:b888fb67c9cf 139 for(int h=0;h<2;h++) angle_buf[h] = 0;
SED9008 0:b888fb67c9cf 140 for(int h=0;h<ANGLE_SAMPLES;h++){
SED9008 0:b888fb67c9cf 141 accelSample();
SED9008 0:b888fb67c9cf 142 accelAngle();
SED9008 0:b888fb67c9cf 143 for(int j=0;j<2;j++){
SED9008 0:b888fb67c9cf 144 angle_buf[j] += accel[j];
SED9008 0:b888fb67c9cf 145 angle_sam[j][h] = accel[j];
SED9008 0:b888fb67c9cf 146 }
SED9008 0:b888fb67c9cf 147 }
SED9008 0:b888fb67c9cf 148 for(int h=0;h<2;h++) angle[h] = angle_buf[h]/ANGLE_SAMPLES;
SED9008 0:b888fb67c9cf 149 }