Prosper Van / Sensorv2

Sensor library for quadcopter

Dependencies:   MotionDriverv512

Sensor.cpp@4:f817736140b6, 2014-11-02 (annotated)

Committer:
oprospero
Date:
Sun Nov 02 19:17:54 2014 +0000
Revision:
4:f817736140b6
Parent:
3:e658293d4b83 
added reset functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oprospero 0:5c097bb1300f 1
oprospero 0:5c097bb1300f 2 #include "Sensor.h"
oprospero 4:f817736140b6 3 //#define _DEBUG_S
oprospero 4:f817736140b6 4 #ifdef _DEBUG_S
oprospero 0:5c097bb1300f 5 #include "mbed.h"
oprospero 0:5c097bb1300f 6 Serial p(USBTX, USBRX);
oprospero 0:5c097bb1300f 7 Timer td;
oprospero 0:5c097bb1300f 8 int printCounter = 0;
oprospero 0:5c097bb1300f 9 int tIntStatus = 0;
oprospero 0:5c097bb1300f 10 int tgetFIFOCount = 0;
oprospero 0:5c097bb1300f 11 int tgetFIFOCheck = 0;
oprospero 0:5c097bb1300f 12 int tgetFIFOreset = 0;
oprospero 0:5c097bb1300f 13 int tgetFIFOBytes = 0;
oprospero 0:5c097bb1300f 14 int teulars = 0;
oprospero 0:5c097bb1300f 15 #define NL "\n\r"
oprospero 0:5c097bb1300f 16 #define PRINT(x) p.printf(x) //Serial.print(x)
oprospero 0:5c097bb1300f 17 #define PRINTF p.printf //Serial.print(x, y)
oprospero 0:5c097bb1300f 18 #define PRINTLN(x) PRINT(x);PRINT(NL)
oprospero 0:5c097bb1300f 19 #define START td.start();
oprospero 0:5c097bb1300f 20 #define STOP td.stop()
oprospero 0:5c097bb1300f 21 #define RESET td.reset()
oprospero 0:5c097bb1300f 22 #define READ td.read_us()
oprospero 0:5c097bb1300f 23 #define GET(x) x = READ
oprospero 0:5c097bb1300f 24 #else
oprospero 0:5c097bb1300f 25 #define PRINT(x)
oprospero 0:5c097bb1300f 26 #define PRINTF(x, y)
oprospero 0:5c097bb1300f 27 #define PRINTLN(x)
oprospero 0:5c097bb1300f 28 #define START
oprospero 0:5c097bb1300f 29 #define STOP
oprospero 0:5c097bb1300f 30 #define RESET
oprospero 0:5c097bb1300f 31 #define READ
oprospero 0:5c097bb1300f 32 #define GET(x)
oprospero 0:5c097bb1300f 33 #endif
oprospero 0:5c097bb1300f 34
oprospero 0:5c097bb1300f 35
oprospero 0:5c097bb1300f 36 Sensor::Sensor() : dmp()
oprospero 0:5c097bb1300f 37 {
oprospero 2:dae13ba06097 38 gyro_orientation[0] = 1;
oprospero 0:5c097bb1300f 39 gyro_orientation[1] = 0;
oprospero 0:5c097bb1300f 40 gyro_orientation[2] = 0;
oprospero 0:5c097bb1300f 41 gyro_orientation[3] = 0;
oprospero 2:dae13ba06097 42 gyro_orientation[4] = 1;
oprospero 0:5c097bb1300f 43 gyro_orientation[5] = 0;
oprospero 0:5c097bb1300f 44 gyro_orientation[6] = 0;
oprospero 0:5c097bb1300f 45 gyro_orientation[7] = 0;
oprospero 0:5c097bb1300f 46 gyro_orientation[8] = 1;
oprospero 0:5c097bb1300f 47 //InvMpu mpu;
oprospero 0:5c097bb1300f 48 }
oprospero 0:5c097bb1300f 49
oprospero 0:5c097bb1300f 50 int Sensor::initialize()
oprospero 0:5c097bb1300f 51 {
oprospero 0:5c097bb1300f 52 int result = 0;
oprospero 1:fe68e0f86ecf 53 result = mpu_initialize();
oprospero 4:f817736140b6 54 if (result != 0) return result;
oprospero 4:f817736140b6 55 wait_ms(20);
oprospero 2:dae13ba06097 56 result = dmp_initialize();
oprospero 4:f817736140b6 57 if (result != 0) return result;
oprospero 2:dae13ba06097 58 run_self_test();
oprospero 1:fe68e0f86ecf 59 return 0;
oprospero 0:5c097bb1300f 60 }
oprospero 0:5c097bb1300f 61
oprospero 0:5c097bb1300f 62 int Sensor::mpu_initialize()
oprospero 0:5c097bb1300f 63 {
oprospero 0:5c097bb1300f 64 unsigned char accel_fsr;
oprospero 0:5c097bb1300f 65 unsigned short gyro_rate, gyro_fsr;
oprospero 4:f817736140b6 66 // long accel_bias[3], gyro_bias[3];
oprospero 1:fe68e0f86ecf 67 if( !dmp.testConnection() )
oprospero 1:fe68e0f86ecf 68 return -1;
oprospero 0:5c097bb1300f 69 /* Get/set hardware configuration. Start gyro. */
oprospero 0:5c097bb1300f 70 /* Wake up all sensors. */
oprospero 1:fe68e0f86ecf 71 if( dmp.mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
oprospero 1:fe68e0f86ecf 72 return -3;
oprospero 0:5c097bb1300f 73 /* Push both gyro and accel data into the FIFO. */
oprospero 1:fe68e0f86ecf 74 if( dmp.mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
oprospero 1:fe68e0f86ecf 75 return -4;
oprospero 1:fe68e0f86ecf 76 if( dmp.mpu_set_sample_rate(SAMPLE_RATE) )
oprospero 1:fe68e0f86ecf 77 return -5;
oprospero 0:5c097bb1300f 78 /* Read back configuration in case it was set improperly. */
oprospero 0:5c097bb1300f 79 dmp.mpu_get_sample_rate(&gyro_rate);
oprospero 0:5c097bb1300f 80 dmp.mpu_get_gyro_fsr(&gyro_fsr);
oprospero 0:5c097bb1300f 81 dmp.mpu_get_accel_fsr(&accel_fsr);
oprospero 0:5c097bb1300f 82
oprospero 4:f817736140b6 83 // if( !(gyro_rate == SAMPLE_RATE && gyro_fsr == 2000 && accel_fsr == 2) )
oprospero 4:f817736140b6 84 if( !(gyro_rate == SAMPLE_RATE) )
oprospero 1:fe68e0f86ecf 85 return -6;
oprospero 2:dae13ba06097 86
oprospero 1:fe68e0f86ecf 87 return 0;
oprospero 0:5c097bb1300f 88 }
oprospero 0:5c097bb1300f 89
oprospero 0:5c097bb1300f 90 int Sensor::dmp_initialize()
oprospero 0:5c097bb1300f 91 {
oprospero 0:5c097bb1300f 92 // Instruction via MotionDriver_Tutorial
oprospero 1:fe68e0f86ecf 93 if( dmp.dmp_load_motion_driver_firmware() )
oprospero 1:fe68e0f86ecf 94 return -8;
oprospero 0:5c097bb1300f 95 unsigned short temp = dmp.inv_orientation_matrix_to_scalar(gyro_orientation);
oprospero 1:fe68e0f86ecf 96 if( dmp.dmp_set_orientation(temp) )
oprospero 1:fe68e0f86ecf 97 return -9;
oprospero 0:5c097bb1300f 98 // unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_RAW_ACCEL ;
oprospero 0:5c097bb1300f 99 unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL ;
oprospero 1:fe68e0f86ecf 100 // unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO;
oprospero 1:fe68e0f86ecf 101 if( dmp.dmp_enable_feature(mask) )
oprospero 1:fe68e0f86ecf 102 return -10;
oprospero 4:f817736140b6 103 // if( dmp.mpu_set_sample_rate(200) )
oprospero 4:f817736140b6 104 // return -11;
oprospero 1:fe68e0f86ecf 105 if( dmp.dmp_set_fifo_rate(100) )
oprospero 1:fe68e0f86ecf 106 return -12;
oprospero 1:fe68e0f86ecf 107 if( dmp.mpu_set_dmp_state(1) )
oprospero 1:fe68e0f86ecf 108 return -13;
oprospero 2:dae13ba06097 109 // if( dmp.mpu_set_lpf(188) )
oprospero 2:dae13ba06097 110 // return -14;
oprospero 1:fe68e0f86ecf 111 return 0;
oprospero 0:5c097bb1300f 112 }
oprospero 0:5c097bb1300f 113
oprospero 3:e658293d4b83 114 void Sensor::reset()
oprospero 3:e658293d4b83 115 {
oprospero 3:e658293d4b83 116 dmp.reset();
oprospero 3:e658293d4b83 117 }
oprospero 3:e658293d4b83 118
oprospero 0:5c097bb1300f 119
oprospero 4:f817736140b6 120 int Sensor::run_self_test()
oprospero 0:5c097bb1300f 121 {
oprospero 0:5c097bb1300f 122 int result;
oprospero 0:5c097bb1300f 123 long gyro_cal[3], accel_cal[3];
oprospero 0:5c097bb1300f 124 unsigned char i = 0;
oprospero 0:5c097bb1300f 125
oprospero 0:5c097bb1300f 126 result = dmp.mpu_run_self_test(gyro_cal, accel_cal);
oprospero 0:5c097bb1300f 127 if (result == 0x7) {
oprospero 0:5c097bb1300f 128 /* Test passed. We can trust the gyro data here, so let's push it down
oprospero 0:5c097bb1300f 129 * to the DMP.
oprospero 0:5c097bb1300f 130 */
oprospero 0:5c097bb1300f 131 for(i = 0; i<3; i++) {
oprospero 0:5c097bb1300f 132 gyro_cal[i] = (long)(gyro_cal[i] * 32.8f); //convert to +-1000dps
oprospero 0:5c097bb1300f 133 accel_cal[i] *= 4096.f; //convert to +-8G
oprospero 0:5c097bb1300f 134 accel_cal[i] = accel[i] >> 16;
oprospero 0:5c097bb1300f 135 gyro_cal[i] = (long)(gyro_cal[i] >> 16);
oprospero 0:5c097bb1300f 136 }
oprospero 0:5c097bb1300f 137 dmp.mpu_set_gyro_bias_reg(gyro_cal);
oprospero 0:5c097bb1300f 138 dmp.mpu_set_accel_bias_6050_reg(accel_cal);
oprospero 4:f817736140b6 139 return 0;
oprospero 0:5c097bb1300f 140 }
oprospero 4:f817736140b6 141 return -1;
oprospero 0:5c097bb1300f 142 }
oprospero 0:5c097bb1300f 143
oprospero 0:5c097bb1300f 144 int Sensor::updateAngles()
oprospero 0:5c097bb1300f 145 {
oprospero 2:dae13ba06097 146 int result = false;
oprospero 0:5c097bb1300f 147 long quat[4];
oprospero 0:5c097bb1300f 148 unsigned char more;
oprospero 0:5c097bb1300f 149 short sensors;
oprospero 0:5c097bb1300f 150 result = dmp.dmp_read_fifo(gyro, accel, quat, NULL, &sensors, &more);
oprospero 3:e658293d4b83 151
oprospero 0:5c097bb1300f 152 if (result == 0)
oprospero 0:5c097bb1300f 153 {
oprospero 0:5c097bb1300f 154 q.set(quat);
oprospero 0:5c097bb1300f 155 dmp.dmpGetGravity(&gravity,&q);
oprospero 0:5c097bb1300f 156 dmp.dmpGetYawPitchRoll(ypr,&q,&gravity);
oprospero 0:5c097bb1300f 157 }
oprospero 0:5c097bb1300f 158 // PRINTF("More: %d\n\r",more);
oprospero 3:e658293d4b83 159 if (more > 2) dmp.mpu_reset_fifo();
oprospero 0:5c097bb1300f 160 return more;
oprospero 0:5c097bb1300f 161 }
oprospero 0:5c097bb1300f 162
oprospero 0:5c097bb1300f 163 float Sensor::getPitch()
oprospero 0:5c097bb1300f 164 {
oprospero 0:5c097bb1300f 165 return ypr[1] * RAD2DEG;
oprospero 0:5c097bb1300f 166 }
oprospero 0:5c097bb1300f 167 float Sensor::getRoll()
oprospero 0:5c097bb1300f 168 {
oprospero 0:5c097bb1300f 169 return ypr[2] * RAD2DEG;
oprospero 0:5c097bb1300f 170 }
oprospero 0:5c097bb1300f 171 float Sensor::getYaw()
oprospero 0:5c097bb1300f 172 {
oprospero 0:5c097bb1300f 173 return ypr[0] * RAD2DEG;
oprospero 0:5c097bb1300f 174 }
oprospero 0:5c097bb1300f 175
oprospero 0:5c097bb1300f 176 float Sensor::getGyroPitch()
oprospero 0:5c097bb1300f 177 {
oprospero 2:dae13ba06097 178 return gyro[0] * GYRO_GAIN;
oprospero 0:5c097bb1300f 179 }
oprospero 0:5c097bb1300f 180 float Sensor::getGyroRoll()
oprospero 0:5c097bb1300f 181 {
oprospero 2:dae13ba06097 182 return gyro[1] * GYRO_GAIN;
oprospero 0:5c097bb1300f 183 }
oprospero 0:5c097bb1300f 184 float Sensor::getGyroYaw()
oprospero 0:5c097bb1300f 185 {
oprospero 2:dae13ba06097 186 return gyro[2] * GYRO_GAIN;
oprospero 0:5c097bb1300f 187 }