Diff: L3GD20_i2c/L3GD20_i2c.cpp

Revision:

0:3bd8aecadafc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20_i2c/L3GD20_i2c.cpp	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "L3GD20_i2c.hpp"
+
+l3gd20::l3gd20(I2C *_i, bool SA0):ADDR_WHO_AM_I(0x0f), ADDR_CTRL_REG1(0x20), ADDR_CTRL_REG2(0x21), ADDR_CTRL_REG3(0x22)
+                            , ADDR_CTRL_REG4(0x23), ADDR_CTRL_REG5(0x24), ADDR_OUT_TEMP(0x26), ADDR_STATUS_REG(0x27)
+                            , ADDR_OUT_X_L(0x28), ADDR_OUT_X_H(0x29), ADDR_OUT_Y_L(0x2A), ADDR_OUT_Y_H(0x2B), ADDR_OUT_Z_L(0x2C), ADDR_OUT_Z_H(0x2D)
+{
+    pre_time_ms = 0;
+    i = _i;
+    if(SA0) addr = 0xD6;
+    else    addr = 0xD4;
+    decom = 0.0875;
+    write_reg(ADDR_CTRL_REG1, 0x0F);
+    angleDeg[0] = angleDeg[1] = angleDeg[2] = 0;
+}
+
+char l3gd20::read_reg(char reg)
+{
+    char recv;
+    i->write(addr, &reg, 1, true);
+    i->read(addr | 0x01, &recv, 1);
+    return recv;
+}
+
+void l3gd20::write_reg(char reg, char data)
+{
+    char send[] = {reg, data};
+    i->write(addr, send, 2);
+}
+
+bool l3gd20::who_am_i()
+{
+    return read_reg(ADDR_WHO_AM_I) == 0xD4;
+}
+
+void l3gd20::set_range(int dps)
+{
+    switch(dps)
+    {
+        case 250:
+            decom = 0.00875;
+            write_reg(ADDR_CTRL_REG4, 0b00000000);
+            break;
+            
+        case 500:
+            decom = 0.0175;
+            write_reg(ADDR_CTRL_REG4, 0b00010000);
+            break;
+            
+        case 2000:
+            decom = 0.07;
+            write_reg(ADDR_CTRL_REG4, 0b00100000);
+            break;
+            
+    }
+}
+
+int16_t l3gd20::get_raw_omega(char se)
+{
+    char addrRegH, addrRegL;
+    switch(se)
+    {
+        case 'x': case 'X':
+            addrRegH = ADDR_OUT_X_H;
+            addrRegL = ADDR_OUT_X_L;
+            break;
+            
+        case 'y': case 'Y':
+            addrRegH = ADDR_OUT_Y_H;
+            addrRegL = ADDR_OUT_Y_L;
+            break;
+            
+        case 'z':case 'Z':
+            addrRegH = ADDR_OUT_Z_H;
+            addrRegL = ADDR_OUT_Z_L;
+            break;
+    }
+    int16_t rawOmegaH = read_reg(addrRegH);
+    int16_t rawOmegaL = read_reg(addrRegL);
+    
+    return  (rawOmegaH << 8) | rawOmegaL;
+}
+
+void l3gd20::apply_offset()
+{
+    int num = 100;
+    double accum[3] = {0};
+    for(int i = 0; i < num; i++)
+    {
+        accum[0] += get_raw_omega('X');
+        accum[1] += get_raw_omega('Y');
+        accum[2] += get_raw_omega('Z');
+        wait_ms(1);
+    }
+    for(int i = 0; i < 3; i++)
+    {
+        omegaOffset[i] = accum[i] * decom / (double)num;
+    }
+}
+
+void l3gd20::start()
+{
+    t.start();
+    stpFlg = false;
+}
+
+void l3gd20::reset()
+{
+    t.reset();
+    angleDeg[0] = angleDeg[1] = angleDeg[2] = 0;
+}
+
+void l3gd20::stop()
+{
+    t.stop();
+    t.reset();
+    stpFlg = true;
+}
+
+double l3gd20::trapezoid_integr(double data, double ex_data, double period){
+    return (data + ex_data) * period / 2.0;
+}
+
+void l3gd20::renew_angle()
+{
+    static double preOmega[3] = {0};
+    
+    if(stpFlg) return;
+    
+    double omega[] = 
+    {
+        get_raw_omega('X') * decom - omegaOffset[0],
+        get_raw_omega('Y') * decom - omegaOffset[1],
+        get_raw_omega('Z') * decom - omegaOffset[2]
+    };
+    
+    int dt_ms = t.read_ms();
+//    t.reset();
+    
+    for(int i = 0; i < 3; i++)
+    {
+        angleDeg[i] += trapezoid_integr(omega[i], preOmega[i], (dt_ms - pre_time_ms) / 1000.0);
+        preOmega[i] = omega[i];
+    }
+    pre_time_ms = dt_ms;
+    
+    if(t.read() > 1800)
+    {
+        t.reset();
+        pre_time_ms = 0;
+    }
+}
+
+void l3gd20::get_angle_deg(double *a)
+{
+    for(int i = 0; i < 3; i++)
+    {
+        a[i] = angleDeg[i];
+    }
+}
+
+void l3gd20::get_angle_rad(double *a)
+{
+    for(int i = 0; i < 3; i++)
+    {
+        a[i] = angleDeg[i] / 180.0 * 3.141593;
+    }
+}
\ No newline at end of file