![](/media/cache/profiles/8ad6635f33710af6a535e8fb8486975e.50x50_q85.jpg)
NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
Revision 40:2ca410923691, committed 2014-02-14
- Comitter:
- maetugr
- Date:
- Fri Feb 14 14:17:32 2014 +0000
- Parent:
- 39:9fd3f4439978
- Commit message:
- now with MPU6050 before taking it too FlyBed2
Changed in this revision
--- a/Sensors/Acc/ADXL345.h Wed Aug 28 00:30:38 2013 +0000 +++ b/Sensors/Acc/ADXL345.h Fri Feb 14 14:17:32 2014 +0000 @@ -53,13 +53,15 @@ { public: ADXL345(PinName sda, PinName scl); // constructor, uses I2C_Sensor class - virtual void read(); // read all axis to array + float data[3]; // where the measured data is saved + void read(); // read all axis to array float offset[3]; // offset that's subtracted from every measurement void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving) private: - virtual void readraw(); + int raw[3]; + void readraw(); }; #endif
--- a/Sensors/Alt/BMP085.h Wed Aug 28 00:30:38 2013 +0000 +++ b/Sensors/Alt/BMP085.h Fri Feb 14 14:17:32 2014 +0000 @@ -12,8 +12,8 @@ { public: BMP085(PinName sda, PinName scl); - - //virtual void read(); + float data; // where the measured data is saved + //void read(); void calibrate(int s); @@ -21,7 +21,7 @@ private: // raw data and function to measure it - int raw[3]; + int raw; //void readraw(); // calibration parameters and their saving
--- a/Sensors/Comp/HMC5883.h Wed Aug 28 00:30:38 2013 +0000 +++ b/Sensors/Comp/HMC5883.h Fri Feb 14 14:17:32 2014 +0000 @@ -17,13 +17,14 @@ { public: HMC5883(PinName sda, PinName scl); - - virtual void read(); // read all axis from register to array data + float data[3]; // where the measured data is saved + void read(); // read all axis from register to array data void calibrate(int s); float get_angle(); private: - virtual void readraw(); // function to get raw data + int raw[3]; + void readraw(); // function to get raw data float scale[3]; // calibration parameters float offset[3];
--- a/Sensors/Gyro/L3G4200D.h Wed Aug 28 00:30:38 2013 +0000 +++ b/Sensors/Gyro/L3G4200D.h Fri Feb 14 14:17:32 2014 +0000 @@ -44,13 +44,15 @@ { public: L3G4200D(PinName sda, PinName scl); // constructor, uses I2C_Sensor class - virtual void read(); // read all axis from register to array data + float data[3]; // where the measured data is saved + void read(); // read all axis from register to array data float offset[3]; // offset that's subtracted from every measurement void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving) int readTemp(); // read temperature from sensor private: - virtual void readraw(); + int raw[3]; + void readraw(); }; #endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Gyro_Acc/MPU6050.cpp Fri Feb 14 14:17:32 2014 +0000 @@ -0,0 +1,82 @@ +#include "MPU6050.h" + +MPU6050::MPU6050(PinName sda, PinName scl) : I2C_Sensor(sda, scl, MPU6050_I2C_ADDRESS) +{ + // Turns on the MPU6050's gyro and initializes it + // register datasheet: http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf + writeRegister(MPU6050_RA_PWR_MGMT_1, 0x01); // wake up from sleep and chooses Gyro X-Axis as Clock source (stadard sleeping and with inacurate clock is 0x40) + writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18); // scales gyros range to +-2000dps + writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00); // scales accelerometers range to +-2g +} + +void MPU6050::read() +{ + readraw_gyro(); // read raw measurement data + readraw_acc(); + + for (int i = 0; i < 3; i++) + data_gyro[i] = (raw_gyro[i] - offset_gyro[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10) + + for (int i = 0; i < 3; i++) + data_acc[i] = raw_acc[i] - offset_acc[i]; // TODO: didn't care about units because IMU-algorithm just uses vector direction +} + +int MPU6050::readTemp() +{ + char buffer[2]; // 8-Bit pieces of temperature data + + readMultiRegister(MPU6050_RA_TEMP_OUT_H, buffer, 2); // read the sensors register for the temperature + return (short) (buffer[0] << 8 | buffer[1]); +} + +void MPU6050::readraw_gyro() +{ + char buffer[6]; // 8-Bit pieces of axis data + + readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7) + + raw_gyro[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers + raw_gyro[1] = (short) (buffer[2] << 8 | buffer[3]); + raw_gyro[2] = (short) (buffer[4] << 8 | buffer[5]); +} + +void MPU6050::readraw_acc() +{ + char buffer[6]; // 8-Bit pieces of axis data + + readMultiRegister(MPU6050_RA_ACCEL_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7) + + raw_acc[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers + raw_acc[1] = (short) (buffer[2] << 8 | buffer[3]); + raw_acc[2] = (short) (buffer[4] << 8 | buffer[5]); +} + +void MPU6050::calibrate(int times, float separation_time) +{ + // calibrate sensor with an average of count samples (result of calibration stored in offset[]) + // Calibrate Gyroscope ---------------------------------- + float calib_gyro[3] = {0,0,0}; // temporary array for the sum of calibration measurement + + for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time + readraw_gyro(); + for (int j = 0; j < 3; j++) + calib_gyro[j] += raw_gyro[j]; + wait(separation_time); + } + + for (int i = 0; i < 3; i++) + offset_gyro[i] = calib_gyro[i]/times; // take the average of the calibration measurements + + // Calibrate Accelerometer ------------------------------- + float calib_acc[3] = {0,0,0}; // temporary array for the sum of calibration measurement + + for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time + readraw_acc(); + for (int j = 0; j < 3; j++) + calib_acc[j] += raw_acc[j]; + wait(separation_time); + } + + for (int i = 0; i < 2; i++) + offset_acc[i] = calib_acc[i]/times; // take the average of the calibration measurements +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Gyro_Acc/MPU6050.h Fri Feb 14 14:17:32 2014 +0000 @@ -0,0 +1,144 @@ +// based on http://mbed.org/users/garfieldsg/code/MPU6050/ + +#ifndef MPU6050_H +#define MPU6050_H + +#include "mbed.h" +#include "I2C_Sensor.h" + +#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define MPU6050_I2C_ADDRESS 0xD0 // adresses above multiplied by 2 + +// register addresses +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +class MPU6050 : public I2C_Sensor +{ + public: + MPU6050(PinName sda, PinName scl); // constructor, uses I2C_Sensor class + float data_gyro[3]; // where the measured data is saved + float data_acc[3]; // where the measured data is saved + virtual void read(); // read all axis from register to array data + float offset_gyro[3]; // offset that's subtracted from every Gyroscope measurement + float offset_acc[3]; // offset that's subtracted from every Accelerometer measurement + void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving) + int readTemp(); // read temperature from sensor + + private: + int raw_gyro[3]; + int raw_acc[3]; + void readraw_gyro(); + void readraw_acc(); +}; + +#endif \ No newline at end of file
--- a/Sensors/I2C_Sensor.h Wed Aug 28 00:30:38 2013 +0000 +++ b/Sensors/I2C_Sensor.h Fri Feb 14 14:17:32 2014 +0000 @@ -11,10 +11,6 @@ public: I2C_Sensor(PinName sda, PinName scl, char address); - float data[3]; // where the measured data is saved - virtual void read() = 0; // read all axis from register to array data - //TODO: virtual void calibrate() = 0; // calibrate the sensor and if desired write calibration values to a file - protected: // Calibration void saveCalibrationValues(float values[], int size, char * filename); @@ -25,10 +21,6 @@ void writeRegister(char reg, char data); void readMultiRegister(char reg, char* output, int size); - // raw data and function to measure it - int raw[3]; - virtual void readraw() = 0; - private: I2C i2c_init; // original mbed I2C-library just to initialise the control registers MODI2C i2c; // I2C-Bus
--- a/main.cpp Wed Aug 28 00:30:38 2013 +0000 +++ b/main.cpp Fri Feb 14 14:17:32 2014 +0000 @@ -1,10 +1,11 @@ #include "mbed.h" // Standard Library #include "LED.h" // LEDs framework for blinking ;) #include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe) -#include "L3G4200D.h" // Gyro (Gyroscope) -#include "ADXL345.h" // Acc (Accelerometer) -#include "HMC5883.h" // Comp (Compass) -#include "BMP085_old.h" // Alt (Altitude sensor) +#include "MPU6050.h" // Combined Gyro and Acc +//#include "L3G4200D.h" // Gyro (Gyroscope) +//#include "ADXL345.h" // Acc (Accelerometer) +//#include "HMC5883.h" // Comp (Compass) +//#include "BMP085_old.h" // Alt (Altitude sensor) #include "RC_Channel.h" // RemoteControl Channels with PPM #include "Servo_PWM.h" // Motor PPM using PwmOut #include "PID.h" // PID Library (slim, self written) @@ -27,7 +28,7 @@ #define PITCH 1 #define YAW 2 -#define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC +#define PC_CONNECTED // decomment if you want to debug per USB/Bluetooth and your PC // Global variables bool armed = false; // this variable is for security (when false no motor rotates any more) @@ -54,13 +55,14 @@ // Initialisation of hardware (see includes for more info) LED LEDs; #ifdef PC_CONNECTED - //PC pc(USBTX, USBRX, 115200); // USB - PC pc(p9, p10, 115200); // Bluetooth + PC pc(USBTX, USBRX, 115200); // USB + //PC pc(p9, p10, 115200); // Bluetooth #endif -L3G4200D Gyro(p28, p27); -ADXL345 Acc(p28, p27); -HMC5883 Comp(p28, p27); -BMP085_old Alt(p28, p27); +MPU6050 Sensor(p28, p27); +//L3G4200D Gyro(p28, p27); +//ADXL345 Acc(p28, p27); +//HMC5883 Comp(p28, p27); +//BMP085_old Alt(p28, p27); RC_Channel RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)}; // no p19/p20 ! Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed! IMU_Filter IMU; // (don't write () after constructor for no arguments!) @@ -71,8 +73,9 @@ time_read_sensors = GlobalTimer.read(); // start time measure for sensors // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes - Gyro.read(); - Acc.read(); + Sensor.read(); + //Gyro.read(); + //Acc.read(); //Comp.read(); // TODO: not every loop every sensor? altitude not that important //Alt.Update(); // TODO needs very long to read because of waits @@ -84,7 +87,8 @@ dt = GlobalTimer.read() - time_for_dt; // time in us since last loop time_for_dt = GlobalTimer.read(); // set new time for next measurement - IMU.compute(dt, Gyro.data, Acc.data); + IMU.compute(dt, Sensor.data_gyro, Sensor.data_acc); + //IMU.compute(dt, Gyro.data, Acc.data); //pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors); if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100) @@ -147,7 +151,7 @@ for(int i=0;i<4;i++) // for security reason, set every motor to zero speed ESC[i] = 0; } - pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", + /*pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", armed, dt, dt_read_sensors, @@ -163,6 +167,24 @@ MIX.Motor_speed[0], MIX.Motor_speed[1], MIX.Motor_speed[2], + MIX.Motor_speed[3]);*/ + + pc.printf("%f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", + IMU.angle[ROLL], + IMU.angle[PITCH], + IMU.angle[YAW], + Sensor.data_gyro[0], + Sensor.data_gyro[1], + Sensor.data_gyro[2], + Sensor.data_acc[0], + Sensor.data_acc[1], + Sensor.data_acc[2], + controller_value[ROLL], + controller_value[PITCH], + controller_value[YAW], + MIX.Motor_speed[0], + MIX.Motor_speed[1], + MIX.Motor_speed[2], MIX.Motor_speed[3]); dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time for loop @@ -200,8 +222,9 @@ #endif LEDs.roll(2); - Gyro.calibrate(50, 0.02); - Acc.calibrate(50, 0.02); + Sensor.calibrate(50, 0.02); + //Gyro.calibrate(50, 0.02); + //Acc.calibrate(50, 0.02); // Start! GlobalTimer.start();