Diff: angle.h

Revision:

0:15e41c824e3b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angle.h	Sun Nov 30 11:06:13 2014 +0000
@@ -0,0 +1,228 @@
+#ifndef _angle_H
+#define _angle_H
+#include "mbed.h"
+
+#define acc_i2c  0x53<<1
+#define acc_i2c_write  0x53<<1
+#define acc_i2c_read  0x53<<1|1
+#define ADXL345_DEVID_REG          0x00
+#define ADXL345_THRESH_TAP_REG     0x1D
+#define ADXL345_OFSX_REG           0x1E
+#define ADXL345_OFSY_REG           0x1F
+#define ADXL345_OFSZ_REG           0x20
+#define ADXL345_DUR_REG            0x21
+#define ADXL345_LATENT_REG         0x22
+#define ADXL345_WINDOW_REG         0x23
+#define ADXL345_THRESH_ACT_REG     0x24
+#define ADXL345_THRESH_INACT_REG   0x25
+#define ADXL345_TIME_INACT_REG     0x26
+#define ADXL345_ACT_INACT_CTL_REG  0x27
+#define ADXL345_THRESH_FF_REG      0x28
+#define ADXL345_TIME_FF_REG        0x29
+#define ADXL345_TAP_AXES_REG       0x2A
+#define ADXL345_ACT_TAP_STATUS_REG 0x2B
+#define ADXL345_BW_RATE_REG        0x2C
+#define ADXL345_POWER_CTL_REG      0x2D
+#define ADXL345_INT_ENABLE_REG     0x2E
+#define ADXL345_INT_MAP_REG        0x2F
+#define ADXL345_INT_SOURCE_REG     0x30
+#define ADXL345_DATA_FORMAT_REG    0x31
+#define ADXL345_DATAX0_REG         0x32
+#define ADXL345_DATAX1_REG         0x33
+#define ADXL345_DATAY0_REG         0x34
+#define ADXL345_DATAY1_REG         0x35
+#define ADXL345_DATAZ0_REG         0x36
+#define ADXL345_DATAZ1_REG         0x37
+#define ADXL345_FIFO_CTL           0x38
+#define ADXL345_FIFO_STATUS        0x39
+
+#define ADXL345_3200HZ      0x0F
+#define ADXL345_1600HZ      0x0E
+#define ADXL345_800HZ       0x0D
+#define ADXL345_400HZ       0x0C
+#define ADXL345_200HZ       0x0B
+#define ADXL345_100HZ       0x0A
+#define ADXL345_50HZ        0x09
+#define ADXL345_25HZ        0x08
+#define ADXL345_12HZ5       0x07
+#define ADXL345_6HZ25       0x06
+
+#define ADXL345_X           0x00
+#define ADXL345_Y           0x01
+#define ADXL345_Z           0x02
+
+#define MeasurementMode     0x08
+
+#define GYR_ADDRESS 0xD4
+#define L3GD20_WHO_AM_I      0x0F
+
+#define L3GD20_CTRL_REG1     0x20
+#define L3GD20_CTRL_REG2     0x21
+#define L3GD20_CTRL_REG3     0x22
+#define L3GD20_CTRL_REG4     0x23
+#define L3GD20_CTRL_REG5     0x24
+#define L3GD20_REFERENCE     0x25
+#define L3GD20_OUT_TEMP      0x26
+#define L3GD20_STATUS_REG    0x27
+
+#define L3GD20_OUT_X_L       0x28
+#define L3GD20_OUT_X_H       0x29
+#define L3GD20_OUT_Y_L       0x2A
+#define L3GD20_OUT_Y_H       0x2B
+#define L3GD20_OUT_Z_L       0x2C
+#define L3GD20_OUT_Z_H       0x2D
+
+#define L3GD20_FIFO_CTRL_REG 0x2E
+#define L3GD20_FIFO_SRC_REG  0x2F
+
+#define L3GD20_INT1_CFG      0x30
+#define L3GD20_INT1_SRC      0x31
+#define L3GD20_INT1_THS_XH   0x32
+#define L3GD20_INT1_THS_XL   0x33
+#define L3GD20_INT1_THS_YH   0x34
+#define L3GD20_INT1_THS_YL   0x35
+#define L3GD20_INT1_THS_ZH   0x36
+#define L3GD20_INT1_THS_ZL   0x37
+#define L3GD20_INT1_DURATION 0x38
+class kalman;
+class ANGLE{
+    private:
+    class kalman
+            {
+                public:
+                    kalman(void);
+                    double getAngle(double newAngle, double newRate, double dt);
+                    
+                    void setAngle(double newAngle);
+                    void setQangle(double newQ_angle);
+                    void setQgyroBias(double newQ_gyroBias);
+                    void setRangle(double newR_angle);
+                    
+                    double getRate(void);
+                    double getQangle(void);
+                    double getQbias(void);
+                    double getRangle(void);
+                    
+                
+                private:
+                    double P[2][2];         //error covariance matrix
+                    double K[2];            //kalman gain
+                    double y;               //angle difference
+                    double S;               //estimation error
+                
+                    double rate;            //rate in deg/s
+                    double angle;           //kalman angle
+                    double bias;            //kalman gyro bias
+                
+                    double Q_angle;         //process noise variance for the angle of the accelerometer
+                    double Q_gyroBias;      //process noise variance for the gyroscope bias
+                    double R_angle;         //measurement noise variance 
+            };
+        I2C i2c_;
+        //ACC ADXL345
+        int x_acc,y_acc,z_acc,sampleNum;
+        double x_offset,y_offset,z_offset;
+        float gx,gy,gz,xnoise,ynoise,znoise;
+        //GYALO L3GD20
+        double Rate;
+        double  sampleTime;       
+        float noise[3];
+        short offset[3];short rate[3],prev_rate[3];
+        double angle[3],offset_angle[3],Synthesis_angle[3],kalman_angle[3],comp_angle[3];
+        double t[3];
+        short tempDATA_ACC[3],tempDATA_ANGLE[3];
+        //KALMAN
+            kalman kalma[3];
+        
+        char data_single_get(char reg);
+        int data_single_put(char reg,char data);
+        void data_multi_get(char reg,char* data, int size);
+        int data_multi_put(char reg, char* data, int size);
+        bool write_reg(int addr_i2c,int addr_reg, char v);
+        bool read_reg(int addr_i2c,int addr_reg, char *v);
+        bool recv(char sad, char sub, char *buf, int length);
+    public:
+        ANGLE(PinName sda, PinName scl);
+        void ADXL_setup();
+        void ADXL_setnum(int Num=500,float time=0.001,double rate=0.00390635);//set data member
+        void Synthesis(double* X,double* Y);
+        void getaxis_acc(short* DATA_ACC);
+        char getdeviceID(){return data_single_get(ADXL345_DEVID_REG);}
+        void getangle_acc(double* DATA_ANGLE);
+        void get_angle_rate(double *x,double *y,double *z);
+        void get_rate(short* RATE);
+        void set_angle();//set always [time]
+        void set_angle(double ANG_x,double ANG_y,double ANG_z);
+        void get_angle(double *x,double *y,double *z);
+        void get_Synthesis_angle(double* X,double* Y);
+        void get_Kalman_angle(double* X,double* Y);
+        void get_Comp_angle(double* X,double* Y);
+        void set_noise();
+        void set_offset();
+        void set_angleoffset();
+
+        
+        double getRate(void);
+        double getQangle(void);
+        double getQbias(void);
+        double getRangle(void);
+
+
+        int setPowerMode(char mode);
+        
+        char getPowerControl(){return data_single_get(ADXL345_POWER_CTL_REG);}
+        int setPowerControl(char control){return data_single_put(ADXL345_POWER_CTL_REG, control);}
+        
+        char getDataFormatControl(void){return data_single_get(ADXL345_DATA_FORMAT_REG);}
+        int setDataFormatControl(char control){return data_single_put(ADXL345_DATA_FORMAT_REG, control);}
+        
+        int setDataRate(char rate);
+        
+        char getOffset(char axis);
+        int setOffset(char axis, char offset);
+        
+        char getFifoControl(void){return data_single_get(ADXL345_FIFO_CTL);}
+        int setFifoControl(char settings){return data_single_put(ADXL345_FIFO_STATUS, settings);}
+        char getFifoStatus(void){return data_single_get(ADXL345_FIFO_STATUS);}
+        
+        char getTapThreshold(void){return data_single_get(ADXL345_THRESH_TAP_REG);}
+        int setTapThreshold(char threshold){return data_single_put(ADXL345_THRESH_TAP_REG, threshold);}
+        float getTapDuration(void){return (float)data_single_get(ADXL345_DUR_REG)*625;}
+        int setTapDuration(short int duration_us);
+        float getTapLatency(void){return (float)data_single_get(ADXL345_LATENT_REG)*1.25;}
+        int setTapLatency(short int latency_ms);
+        
+        float getWindowTime(void){return (float)data_single_get(ADXL345_WINDOW_REG)*1.25;}
+        int setWindowTime(short int window_ms);
+        
+        char getActivityThreshold(void){return data_single_get(ADXL345_THRESH_ACT_REG);}
+        int setActivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_ACT_REG, threshold);}
+        
+        char getInactivityThreshold(void){return data_single_get(ADXL345_THRESH_INACT_REG);}
+        int setInactivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_INACT_REG, threshold);}
+        char getTimeInactivity(void){return data_single_get(ADXL345_TIME_INACT_REG);}
+        int setTimeInactivity(char timeInactivity){return data_single_put(ADXL345_TIME_INACT_REG, timeInactivity);}
+        
+        char getActivityInactivityControl(void){return data_single_get(ADXL345_ACT_INACT_CTL_REG);}
+        int setActivityInactivityControl(char settings){return data_single_put(ADXL345_ACT_INACT_CTL_REG, settings);}
+        
+        char getFreefallThreshold(void){return data_single_get(ADXL345_THRESH_FF_REG);}
+        int setFreefallThreshold(char threshold){return data_single_put(ADXL345_THRESH_FF_REG, threshold);}
+        char getFreefallTime(void){return data_single_get(ADXL345_TIME_FF_REG)*5;}
+        int setFreefallTime(short int freefallTime_ms);
+        
+        char getTapAxisControl(void){return data_single_get(ADXL345_TAP_AXES_REG);}
+        int setTapAxisControl(char settings){return data_single_put(ADXL345_TAP_AXES_REG, settings);}
+        char getTapSource(void){return data_single_get(ADXL345_ACT_TAP_STATUS_REG);}
+        
+        char getInterruptEnableControl(void){return data_single_get(ADXL345_INT_ENABLE_REG);}
+        int setInterruptEnableControl(char settings){return data_single_put(ADXL345_INT_ENABLE_REG, settings);};
+        char getInterruptMappingControl(void){return data_single_get(ADXL345_INT_MAP_REG);};
+        int setInterruptMappingControl(char settings){return data_single_put(ADXL345_INT_MAP_REG, settings);};
+        char getInterruptSource(void){return data_single_get(ADXL345_INT_SOURCE_REG);}
+        bool read(float *gx, float *gy, float *gz);
+        bool read(short *axis);
+
+};
+
+#endif
\ No newline at end of file