Diff: Sensor.cpp

Revision:

0:5c097bb1300f

Child:

1:fe68e0f86ecf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensor.cpp	Tue Jun 03 07:57:32 2014 +0000
@@ -0,0 +1,165 @@
+
+#include "Sensor.h"
+#ifdef DEBUG_S
+    #include "mbed.h"
+    Serial p(USBTX, USBRX);
+    Timer td;
+    int printCounter = 0;
+    int tIntStatus = 0;
+    int tgetFIFOCount = 0;
+    int tgetFIFOCheck = 0;
+    int tgetFIFOreset = 0;
+    int tgetFIFOBytes = 0;
+    int teulars = 0;
+    #define NL "\n\r"
+    #define PRINT(x) p.printf(x)   //Serial.print(x)
+    #define PRINTF p.printf   //Serial.print(x, y)
+    #define PRINTLN(x) PRINT(x);PRINT(NL)
+    #define START td.start(); 
+    #define STOP td.stop()
+    #define RESET td.reset()
+    #define READ td.read_us()
+    #define GET(x) x = READ
+#else
+    #define PRINT(x)
+    #define PRINTF(x, y)
+    #define PRINTLN(x)
+    #define START 
+    #define STOP  
+    #define RESET 
+    #define READ 
+    #define GET(x)
+#endif
+
+
+Sensor::Sensor() : dmp()
+{
+    gyro_orientation[0] = -1;
+    gyro_orientation[1] = 0;
+    gyro_orientation[2] = 0;
+    gyro_orientation[3] = 0;
+    gyro_orientation[4] = -1;
+    gyro_orientation[5] = 0;
+    gyro_orientation[6] = 0;
+    gyro_orientation[7] = 0;
+    gyro_orientation[8] = 1;
+//InvMpu mpu;
+}
+
+int Sensor::initialize()
+{
+    int result = 0;
+    result -= mpu_initialize(); 
+    result -= dmp_initialize();
+    run_self_test();
+    return result;  
+}
+
+int Sensor::mpu_initialize()
+{    
+    int result = 0;
+    unsigned char accel_fsr;
+    unsigned short gyro_rate, gyro_fsr;
+    long accel_bias[3], gyro_bias[3];
+    result -= dmp.testConnection() ;
+    result -= dmp.mpu_init();
+    /* Get/set hardware configuration. Start gyro. */
+    /* Wake up all sensors. */
+    result -= dmp.mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
+    /* Push both gyro and accel data into the FIFO. */
+    result -= dmp.mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
+    result -= dmp.mpu_set_sample_rate(SAMPLE_RATE);
+    /* Read back configuration in case it was set improperly. */
+    dmp.mpu_get_sample_rate(&gyro_rate);
+    dmp.mpu_get_gyro_fsr(&gyro_fsr);
+    dmp.mpu_get_accel_fsr(&accel_fsr);
+    
+    if( !(gyro_rate == SAMPLE_RATE && gyro_fsr == 2000 && accel_fsr == 2) )
+        result -= 1;
+    
+//    dmp.mpu_read_6500_gyro_bias(accel_bias);
+    if (dmp.mpu_run_self_test(gyro_bias,accel_bias) != 0x07);
+        result -= 1;
+    return result;
+}
+
+int Sensor::dmp_initialize()
+{   
+    int result = 0;
+    // Instruction via MotionDriver_Tutorial
+    result -= dmp.dmp_load_motion_driver_firmware();
+    unsigned short temp = dmp.inv_orientation_matrix_to_scalar(gyro_orientation);
+    result -= dmp.dmp_set_orientation(temp);
+//    unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO |  DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_RAW_ACCEL ;
+    unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO |  DMP_FEATURE_GYRO_CAL ;
+    result -= dmp.dmp_enable_feature(mask);
+    result -= dmp.dmp_set_fifo_rate(100); 
+    result -= dmp.mpu_set_dmp_state(1);
+    return result;
+}
+
+
+void Sensor::run_self_test()
+{
+    int result;
+    long gyro_cal[3], accel_cal[3];
+    unsigned char i = 0;
+
+    result = dmp.mpu_run_self_test(gyro_cal, accel_cal);
+    if (result == 0x7) {
+        /* Test passed. We can trust the gyro data here, so let's push it down
+         * to the DMP.
+         */
+        for(i = 0; i<3; i++) {
+            gyro_cal[i] = (long)(gyro_cal[i] * 32.8f); //convert to +-1000dps
+            accel_cal[i] *= 4096.f; //convert to +-8G
+            accel_cal[i] = accel[i] >> 16;
+            gyro_cal[i] = (long)(gyro_cal[i] >> 16);
+        }
+        dmp.mpu_set_gyro_bias_reg(gyro_cal);
+        dmp.mpu_set_accel_bias_6050_reg(accel_cal);
+    }
+}
+
+int Sensor::updateAngles()
+{
+    bool result = false;
+    long quat[4];
+    unsigned char more;
+    short sensors;
+    result = dmp.dmp_read_fifo(gyro, accel, quat, NULL, &sensors, &more);
+    if (result == 0)
+    {
+        q.set(quat);
+        dmp.dmpGetGravity(&gravity,&q);
+        dmp.dmpGetYawPitchRoll(ypr,&q,&gravity);
+    }
+//    PRINTF("More: %d\n\r",more);
+    return more;
+}
+
+float Sensor::getPitch()
+{
+    return ypr[1] * RAD2DEG;
+}
+float Sensor::getRoll()
+{
+    return ypr[2] * RAD2DEG;
+}
+float Sensor::getYaw()
+{
+    return ypr[0] * RAD2DEG;
+}
+
+float Sensor::getGyroPitch()
+{
+    return gyro[1] * GYRO_GAIN;
+}
+float Sensor::getGyroRoll()
+{
+    return gyro[2] * GYRO_GAIN;   
+}
+float Sensor::getGyroYaw()
+{
+    return gyro[0] * GYRO_GAIN;   
+}
\ No newline at end of file