Revision 3:9ed8bc1d0da3, committed 2017-06-13

Comitter:

ChangYuHsuan

Date:

Tue Jun 13 03:51:56 2017 +0000

Parent:

2:e8c2301f7523

Commit message:

F446RE test;

Changed in this revision

--- a/LSM9DS1.cpp	Wed Feb 03 18:24:29 2016 +0000
+++ b/LSM9DS1.cpp	Tue Jun 13 03:51:56 2017 +0000
@@ -42,6 +42,10 @@
     :i2c(sda, scl)
 {
     init(IMU_MODE_I2C, xgAddr, mAddr); // dont know about 0xD6 or 0x3B
+    
+    // Unit transformation
+    deg2rad = PI/180.0;
+    rad2deg = 180.0/PI;
 }
 /*
 LSM9DS1::LSM9DS1()
@@ -539,6 +543,26 @@
     return value;
 }
 
+// =========================================================================== //
+
+void LSM9DS1::readAccelFloatVector(vector<float> &v_out)
+{
+    readAccel();
+    
+    v_out[X_AXIS] = calcAccel(ax)*G;
+    v_out[Y_AXIS] = calcAccel(ay)*G;
+    v_out[Z_AXIS] = calcAccel(az)*G;
+}
+
+float LSM9DS1::readAccelFloat(lsm9ds1_axis axis)
+{
+    return calcAccel(readAccel(axis))*G;
+}
+// =========================================================================== //
+
+
+
+
 void LSM9DS1::readMag()
 {
     uint8_t temp[6]; // We'll read six bytes from the mag into temp 
@@ -592,6 +616,38 @@
     return value;
 }
 
+// =========================================================================== //
+
+void LSM9DS1::readGyroFloatVectorRad(vector<float> &v_out)
+{
+    readGyro();
+    
+    v_out[X_AXIS] = calcGyro(gx)*deg2rad;
+    v_out[Y_AXIS] = calcGyro(gy)*deg2rad;
+    v_out[Z_AXIS] = calcGyro(gz)*deg2rad;
+}
+
+void LSM9DS1::readGyroFloatVectorDeg(vector<float> &v_out)
+{
+    readGyro();
+    
+    v_out[X_AXIS] = calcGyro(gx);
+    v_out[Y_AXIS] = calcGyro(gy);
+    v_out[Z_AXIS] = calcGyro(gz);
+}
+
+float LSM9DS1::readGyroFloatRad(lsm9ds1_axis axis)
+{
+    return calcGyro(readGyro(axis))*deg2rad;    
+}
+
+float LSM9DS1::readGyroFloatDeg(lsm9ds1_axis axis)
+{
+    return calcGyro(readGyro(axis));    
+}
+
+// =========================================================================== //
+
 float LSM9DS1::calcGyro(int16_t gyro)
 {
     // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):

--- a/LSM9DS1.h	Wed Feb 03 18:24:29 2016 +0000
+++ b/LSM9DS1.h	Tue Jun 13 03:51:56 2017 +0000
@@ -33,6 +33,11 @@
 #include <stdint.h>
 #include "LSM9DS1_Registers.h"
 #include "LSM9DS1_Types.h"
+#include <vector>
+using std::vector;
+
+#define PI 3.1415926
+#define G  9.81
 
 #define LSM9DS1_AG_ADDR(sa0)    ((sa0) == 0 ? 0x6A : 0x6B)
 #define LSM9DS1_M_ADDR(sa1)     ((sa1) == 0 ? 0x1C : 0x1E)
@@ -61,6 +66,10 @@
     float gBias[3], aBias[3], mBias[3];
     int16_t gBiasRaw[3], aBiasRaw[3], mBiasRaw[3];
     
+    // Unit transformation
+    float deg2rad; // = 3.1415926/180.0;
+    float rad2deg; // = 180.0/3.1415926;
+    
     // LSM9DS1 -- LSM9DS1 class constructor
     // The constructor will set up a handful of private variables, and set the
     // communication mode as well.
@@ -136,6 +145,40 @@
     */
     int16_t readGyro(lsm9ds1_axis axis);
     
+    /** readGyroFloatVectorRad() -- Read the gyroscope output registers.
+    * This function will read all six gyroscope output registers and 
+    * transform the data into rad/s^2 unit.
+    * The readings are stored in the class' gx, gy, and gz variables. Read
+    * those _after_ calling readGyro().
+    */
+    void readGyroFloatVectorRad(vector<float> &v_out);
+    
+    /** readGyroFloatVectorDeg() -- Read the gyroscope output registers.
+    * This function will read all six gyroscope output registers and 
+    * transform the data into rad/s^2 unit.
+    * The readings are stored in the class' gx, gy, and gz variables. Read
+    * those _after_ calling readGyro().
+    */
+    void readGyroFloatVectorDeg(vector<float> &v_out);
+    
+    /** float readGyroFloatRad(axis) -- Read a specific axis of the gyroscope in rad/s^2 unit.
+    * [axis] can be any of X_AXIS, Y_AXIS, or Z_AXIS.
+    * Input:
+    *  - axis: can be either X_AXIS, Y_AXIS, or Z_AXIS.
+    * Output:
+    *  A float with sensor data on requested axis.
+    */
+    float readGyroFloatRad(lsm9ds1_axis axis);
+    
+    /** float readGyroFloatDeg(axis) -- Read a specific axis of the gyroscope in rad/s^2 unit.
+    * [axis] can be any of X_AXIS, Y_AXIS, or Z_AXIS.
+    * Input:
+    *  - axis: can be either X_AXIS, Y_AXIS, or Z_AXIS.
+    * Output:
+    *  A float with sensor data on requested axis.
+    */
+    float readGyroFloatDeg(lsm9ds1_axis axis);
+    
     /** readAccel() -- Read the accelerometer output registers.
     * This function will read all six accelerometer output registers.
     * The readings are stored in the class' ax, ay, and az variables. Read
@@ -152,13 +195,31 @@
     */
     int16_t readAccel(lsm9ds1_axis axis);
     
+    /** readAccel() -- Read the accelerometer output registers.
+    * This function will read all six accelerometer output registers and 
+    * transform the data into m/s^2 unit.
+    * The readings are stored in the class' ax, ay, and az variables. Read
+    * those _after_ calling readAccel().
+    */
+    void readAccelFloatVector(vector<float> &v_out);
+    
+    /** float readAccelFloat(axis) -- Read a specific axis of the accelerometer.
+    * [axis] can be any of X_AXIS, Y_AXIS, or Z_AXIS.
+    * Input:
+    *  - axis: can be either X_AXIS, Y_AXIS, or Z_AXIS.
+    * Output:
+    *  A float with sensor data on requested axis.
+    */
+    float readAccelFloat(lsm9ds1_axis axis);
+    
+    
     /** readMag() -- Read the magnetometer output registers.
     * This function will read all six magnetometer output registers.
     * The readings are stored in the class' mx, my, and mz variables. Read
     * those _after_ calling readMag().
     */
     void readMag();
-    
+        
     /** int16_t readMag(axis) -- Read a specific axis of the magnetometer.
     * [axis] can be any of X_AXIS, Y_AXIS, or Z_AXIS.
     * Input:

--- a/main.cpp	Wed Feb 03 18:24:29 2016 +0000
+++ b/main.cpp	Tue Jun 13 03:51:56 2017 +0000
@@ -1,29 +1,31 @@
 #include "LSM9DS1.h"
+#include <vector>
+using std::vector;
 
 DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
 
 int main() {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 lol(p9, p10, 0xD6, 0x3C);
+    //LSM9DS1 lol(D14, D15, 0x6B, 0x1E);
+    LSM9DS1 lol(D14, D15, 0xD6, 0x3C);
+    
+    vector<float> gyro(3, 0.0);
+    vector<float> accel(3, 0.0);
+    
     lol.begin();
     if (!lol.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     lol.calibrate();
+    
     while(1) {
-        lol.readTemp();
-        lol.readMag();
-        lol.readGyro();
         
-        //pc.printf("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
-        //pc.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        myled = 1;
-        wait(2);
-        myled = 0;
-        wait(2);
+        lol.readGyroFloatVectorDeg(gyro);
+        lol.readAccelFloatVector(accel);
+        
+        
+        pc.printf("gyro: %.3f %.3f %.3f\n\r", gyro[X_AXIS], gyro[Y_AXIS], gyro[Z_AXIS]);
+        //pc.printf("accel: %f %f %f\n\n\r", accel[X_AXIS], accel[Y_AXIS], accel[Z_AXIS]);
+        
     }
 }