ROHMUSDC / Mbed 2 deprecated Nordic_UART_TEMPLATE_ROHM

First Revision of sample code for interfacing ROHM Multi-Sensor Shield board with Nordic Semiconductor's nRF51-DK Development Kit Host BTLE Board

Dependencies:   BLE_API mbed nRF51822 Nordic_UART_TEMPLATE_ROHM

Dependents:   Nordic_UART_TEMPLATE_ROHM

Fork of UART_TEMPLATE by daniel veilleux

Code Example for ROHM Multi-Sensor Shield on the Nordic Semiconductor nRF51-DK

This code was written to be used with the Nordic Semiconductor nRF51-DK.

This is the basic example code for interfacing ROHM's Multi-sensor Shield Board onto this board.

Additional information about the ROHM MultiSensor Shield Board can be found at the following link: https://github.com/ROHMUSDC/ROHM_SensorPlatform_Multi-Sensor-Shield

For code example for the ROHM SENSORSHLD1-EVK-101, please see the following link: https://developer.mbed.org/teams/ROHMUSDC/code/Nordic_UART_TEMPLATE_ROHM_SHLD1Update/

Operation

Ultimately, this code will initialize all the sensors on the Multi-sensor shield board and then poll the sensors. The sensor data will then be returned to the BTLE COM port link and will be view-able on any BTLE enabled phone that can connect to the Nordic UART Application.

Supported ROHM Sensor Devices

  • BDE0600G Temperature Sensor
  • BM1383GLV Pressure Sensor
  • BU52014 Hall Sensor
  • ML8511 UV Sensor
  • RPR-0521 ALS/PROX Sensor
  • BH1745NUC Color Sensor
  • KMX62 Accel/Mag Sensor
  • KX122 Accel Sensor
  • KXG03 Gyro/Accel Sensor

Sensor Applicable Code Sections

  • Added a Section in "Main" to act as initialization
  • Added to the "Periodic Callback" to read sensor data and return to Phone/Host

Questions/Feedback

Please feel free to let us know any questions/feedback/comments/concerns on the shield implementation by contacting the following e-mail:

Files at this revision

API Documentation at this revision

Revision 7:71046927a0e9, committed 2015-12-18

Comitter:
kbahar3
Date:
Fri Dec 18 00:19:01 2015 +0000
Parent:
6:6860e53dc7ae
Commit message:
Added new code for Gyro (KXG03)

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Mon Sep 28 19:00:02 2015 +0000
+++ b/main.cpp	Fri Dec 18 00:19:01 2015 +0000
@@ -43,14 +43,17 @@
  *  Contact Information: engineering@rohmsemiconductor.com
  */
 
+#define nRF52DevKit
+
 #define AnalogTemp          //BDE0600, Analog Temperature Sensor
 #define AnalogUV            //ML8511, Analog UV Sensor
 #define HallSensor          //BU52011, Hall Switch Sensor
 #define RPR0521             //RPR0521, ALS/PROX Sensor
 #define KMX62               //KMX61, Accel/Mag Sensor
-#define color               //BH1745, Color Sensor
-#define KX022               //KX022, Accelerometer Sensor
+#define Color               //BH1745, Color Sensor
+#define KX122               //KX122, Accelerometer Sensor
 #define Pressure            //BM1383, Barometric Pressure Sensor
+#define KXG03               //KXG03, Gyroscopic Sensor
 
 #include "mbed.h"
 #include "BLEDevice.h"
@@ -71,33 +74,42 @@
 
 // Global Variables
 BLEDevice   m_ble;
-Serial      m_serial_port(p9, p11);  // TX pin, RX pin
+Serial      m_serial_port(p9, p11);  // TX pin, RX pin Original
+//Serial      m_serial_port(p8, p10);  // TX pin, RX pin 
 DigitalOut  m_cmd_led(LED1);
 DigitalOut  m_error_led(LED2);
 UARTService *m_uart_service_ptr;
-DigitalIn   testButton(p20);
-InterruptIn sw4Press(p20);
-I2C         i2c(p30,p7);
+DigitalIn   testButton(p20);    //Original
+//DigitalIn   testButton(p19);
+InterruptIn sw4Press(p20);      //Original
+//InterruptIn sw4Press(p19);  
+I2C         i2c(p30,p7);  //Original DK Kit
+//I2C         i2c(p26,p27);
 bool        RepStart = true;
 bool        NoRepStart = false;
 int         i = 1;
 
 //Sensor Variables
 #ifdef AnalogTemp
-AnalogIn    BDE0600_Temp(p3);
+AnalogIn    BDE0600_Temp(p3);   //Original Dev Kit
+//AnalogIn    BDE0600_Temp(p28);
 uint16_t    BDE0600_Temp_value;
 float       BDE0600_output;
 #endif
 
 #ifdef AnalogUV
-AnalogIn    ML8511_UV(p5);
+AnalogIn    ML8511_UV(p5);    //Original Dev Kit
+//AnalogIn    ML8511_UV(p30);
 uint16_t    ML8511_UV_value;
 float       ML8511_output;
 #endif
 
 #ifdef HallSensor
-DigitalIn   Hall_GPIO0(p14);
-DigitalIn   Hall_GPIO1(p15);
+DigitalIn   Hall_GPIO0(p14);  //Original Dev Kit
+DigitalIn   Hall_GPIO1(p15);  //Original Dev Kit
+//DigitalIn   Hall_GPIO0(p13);
+//DigitalIn   Hall_GPIO1(p14);
+
 int         Hall_Return1;
 int         Hall_Return0;
 #endif
@@ -144,7 +156,7 @@
 float       MEMS_Mag_Conv_Zout = 0;
 #endif
 
-#ifdef color
+#ifdef Color
 int         BH1745_addr_w = 0x72;
 int         BH1745_addr_r = 0x73;
 
@@ -162,33 +174,33 @@
 
 #endif
 
-#ifdef KX022
-int         KX022_addr_w = 0x3C;
-int         KX022_addr_r = 0x3D;
+#ifdef KX122
+int         KX122_addr_w = 0x3C;
+int         KX122_addr_r = 0x3D;
 
-char        KX022_Accel_CNTL1[2] = {0x18, 0x41};
-char        KX022_Accel_ODCNTL[2] = {0x1B, 0x02};
-char        KX022_Accel_CNTL3[2] = {0x1A, 0xD8};
-char        KX022_Accel_TILT_TIMER[2] = {0x22, 0x01};
-char        KX022_Accel_CNTL2[2] = {0x18, 0xC1};
+char        KX122_Accel_CNTL1[2] = {0x18, 0x41};
+char        KX122_Accel_ODCNTL[2] = {0x1B, 0x02};
+char        KX122_Accel_CNTL3[2] = {0x1A, 0xD8};
+char        KX122_Accel_TILT_TIMER[2] = {0x22, 0x01};
+char        KX122_Accel_CNTL2[2] = {0x18, 0xC1};
 
-char        KX022_Content_ReadData[6];
-char        KX022_Addr_Accel_ReadData = 0x06;           
+char        KX122_Content_ReadData[6];
+char        KX122_Addr_Accel_ReadData = 0x06;           
 
-float       KX022_Accel_X;
-float       KX022_Accel_Y;                               
-float       KX022_Accel_Z;
+float       KX122_Accel_X;
+float       KX122_Accel_Y;                               
+float       KX122_Accel_Z;
 
-short int   KX022_Accel_X_RawOUT = 0;
-short int   KX022_Accel_Y_RawOUT = 0;
-short int   KX022_Accel_Z_RawOUT = 0;
+short int   KX122_Accel_X_RawOUT = 0;
+short int   KX122_Accel_Y_RawOUT = 0;
+short int   KX122_Accel_Z_RawOUT = 0;
 
-int         KX022_Accel_X_LB = 0;
-int         KX022_Accel_X_HB = 0;
-int         KX022_Accel_Y_LB = 0;
-int         KX022_Accel_Y_HB = 0;
-int         KX022_Accel_Z_LB = 0;
-int         KX022_Accel_Z_HB = 0;
+int         KX122_Accel_X_LB = 0;
+int         KX122_Accel_X_HB = 0;
+int         KX122_Accel_Y_LB = 0;
+int         KX122_Accel_Y_HB = 0;
+int         KX122_Accel_Z_LB = 0;
+int         KX122_Accel_Z_HB = 0;
 #endif
 
 #ifdef Pressure
@@ -216,6 +228,45 @@
 float       BM1383_Deci;
 #endif
 
+#ifdef KXG03
+int         j = 11;
+int         t = 1;
+short int   aveX = 0;
+short int   aveX2 = 0;
+short int   aveX3 = 0;
+short int   aveY = 0;
+short int   aveY2 = 0;
+short int   aveY3 = 0;
+short int   aveZ = 0;
+short int   aveZ2 = 0;
+short int   aveZ3 = 0;
+float       ave22;
+float       ave33;
+int         KXG03_addr_w = 0x9C;   //write 
+int         KXG03_addr_r = 0x9D;   //read 
+char        KXG03_STBY_REG[2] = {0x43, 0x00};
+char        KXG03_Content_ReadData[6];
+//char        KXG03_Content_Accel_ReadData[6];
+char        KXG03_Addr_ReadData = 0x02;
+//char        KXG03_Addr_Accel_ReadData = 0x08;
+float       KXG03_Gyro_XX;
+float       KXG03_Gyro_X;
+float       KXG03_Gyro_Y;                               
+float       KXG03_Gyro_Z;
+short int   KXG03_Gyro_X_RawOUT = 0;
+short int   KXG03_Gyro_Y_RawOUT = 0; 
+short int   KXG03_Gyro_Z_RawOUT = 0;
+short int   KXG03_Gyro_X_RawOUT2 = 0;
+short int   KXG03_Gyro_Y_RawOUT2 = 0; 
+short int   KXG03_Gyro_Z_RawOUT2 = 0;  
+float       KXG03_Accel_X;
+float       KXG03_Accel_Y;                               
+float       KXG03_Accel_Z;  
+short int   KXG03_Accel_X_RawOUT = 0;
+short int   KXG03_Accel_Y_RawOUT = 0;
+short int   KXG03_Accel_Z_RawOUT = 0;       
+#endif
+
 /**
  * This callback is used whenever a disconnection occurs.
  */
@@ -289,7 +340,7 @@
     uint32_t len = 0;
     
     if(i == 1) {
-        #ifdef color
+        #ifdef Color
         if (m_ble.getGapState().connected) {
             //Read color Portion from the IC
             i2c.write(BH1745_addr_w, &BH1745_Addr_color_ReadData, 1, RepStart);
@@ -492,39 +543,39 @@
         i++;
     }
     else if(i==7){
-        #ifdef KX022
+        #ifdef KX122
         if (m_ble.getGapState().connected) {
-            //Read KX022 Portion from the IC
-            i2c.write(KX022_addr_w, &KX022_Addr_Accel_ReadData, 1, RepStart);
-            i2c.read(KX022_addr_r, &KX022_Content_ReadData[0], 6, NoRepStart);
+            //Read KX122 Portion from the IC
+            i2c.write(KX122_addr_w, &KX122_Addr_Accel_ReadData, 1, RepStart);
+            i2c.read(KX122_addr_r, &KX122_Content_ReadData[0], 6, NoRepStart);
             
                 
             //reconfigure the data (taken from arduino code)
-            KX022_Accel_X_RawOUT = (KX022_Content_ReadData[1]<<8) | (KX022_Content_ReadData[0]);
-            KX022_Accel_Y_RawOUT = (KX022_Content_ReadData[3]<<8) | (KX022_Content_ReadData[2]);
-            KX022_Accel_Z_RawOUT = (KX022_Content_ReadData[5]<<8) | (KX022_Content_ReadData[4]);       
+            KX122_Accel_X_RawOUT = (KX122_Content_ReadData[1]<<8) | (KX122_Content_ReadData[0]);
+            KX122_Accel_Y_RawOUT = (KX122_Content_ReadData[3]<<8) | (KX122_Content_ReadData[2]);
+            KX122_Accel_Z_RawOUT = (KX122_Content_ReadData[5]<<8) | (KX122_Content_ReadData[4]);       
 
             //apply needed changes (taken from arduino code)
-            KX022_Accel_X = (float)KX022_Accel_X_RawOUT / 16384;
-            KX022_Accel_Y = (float)KX022_Accel_Y_RawOUT / 16384;
-            KX022_Accel_Z = (float)KX022_Accel_Z_RawOUT / 16384;
+            KX122_Accel_X = (float)KX122_Accel_X_RawOUT / 16384;
+            KX122_Accel_Y = (float)KX122_Accel_Y_RawOUT / 16384;
+            KX122_Accel_Z = (float)KX122_Accel_Z_RawOUT / 16384;
             
             
             
             //transmit the data
-            len = snprintf((char*) buf, MAX_REPLY_LEN, "KX022 Sensor:");
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "KX122 Sensor:");
             m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
             wait_ms(20);
             
-            len = snprintf((char*) buf, MAX_REPLY_LEN, "  ACCX= %0.2f g", KX022_Accel_X);
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  ACCX= %0.2f g", KX122_Accel_X);
             m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
             wait_ms(20);
             
-            len = snprintf((char*) buf, MAX_REPLY_LEN, "  ACCY= %0.2f g", KX022_Accel_Y);
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  ACCY= %0.2f g", KX122_Accel_Y);
             m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
             wait_ms(20);
             
-            len = snprintf((char*) buf, MAX_REPLY_LEN, "  ACCZ= %0.2f g", KX022_Accel_Z);
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  ACCZ= %0.2f g", KX122_Accel_Z);
             m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
             wait_ms(20);
             
@@ -558,12 +609,148 @@
             len = snprintf((char*) buf, MAX_REPLY_LEN, "  Pres= %0.2f hPa", BM1383_Pres_Conv_Out);
             m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
             wait_ms(20);
+        }        
+        #endif  
+        i++;
+    }
+    else if(i == 9){
+        #ifdef KXG03
+        if (m_ble.getGapState().connected) {
+        i2c.write(KXG03_addr_w, &KXG03_Addr_ReadData, 1, RepStart);
+        i2c.read(KXG03_addr_r, &KXG03_Content_ReadData[0], 6, NoRepStart);
+                        
+        if (t == 1){    
+            int j = 11;
+            while(--j) 
+            {
+                //Read KXG03 Gyro Portion from the IC
+                i2c.write(KXG03_addr_w, &KXG03_Addr_ReadData, 1, RepStart);
+                i2c.read(KXG03_addr_r, &KXG03_Content_ReadData[0], 6, NoRepStart);
+      
+                //Format Data
+                KXG03_Gyro_X_RawOUT = (KXG03_Content_ReadData[1]<<8) | (KXG03_Content_ReadData[0]);
+                KXG03_Gyro_Y_RawOUT = (KXG03_Content_ReadData[3]<<8) | (KXG03_Content_ReadData[2]);
+                KXG03_Gyro_Z_RawOUT = (KXG03_Content_ReadData[5]<<8) | (KXG03_Content_ReadData[4]);
+                aveX = KXG03_Gyro_X_RawOUT;
+                aveY = KXG03_Gyro_Y_RawOUT;
+                aveZ = KXG03_Gyro_Z_RawOUT;
+                aveX2 = aveX2 + aveX;
+                aveY2 = aveY2 + aveY;
+                aveZ2 = aveZ2 + aveZ;  
+             } 
+            aveX3 = aveX2 / 10;
+            aveY3 = aveY2 / 10;
+            aveZ3 = aveZ2 / 10; 
+            
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "Gyro Sensor:");
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);             
+
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "Calibration OK");
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);            
+           
+            //len = snprintf((char*) buf, MAX_REPLY_LEN, "  aveX2= %d", aveX2);
+            //m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            //wait_ms(20);    
+            
+            //len = snprintf((char*) buf, MAX_REPLY_LEN, "  aveX3= %d", aveX3);
+            //m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20); 
+                         
+            
+            //Read KXG03 Gyro Portion from the IC
+            i2c.write(KXG03_addr_w, &KXG03_Addr_ReadData, 1, RepStart);
+            i2c.read(KXG03_addr_r, &KXG03_Content_ReadData[0], 6, NoRepStart);                     
+                   
+            //reconfigure the data (taken from arduino code)
+            KXG03_Gyro_X_RawOUT = (KXG03_Content_ReadData[1]<<8) | (KXG03_Content_ReadData[0]);
+            KXG03_Gyro_Y_RawOUT = (KXG03_Content_ReadData[3]<<8) | (KXG03_Content_ReadData[2]);
+            KXG03_Gyro_Z_RawOUT = (KXG03_Content_ReadData[5]<<8) | (KXG03_Content_ReadData[4]);    
+            
+            KXG03_Gyro_X_RawOUT2 = KXG03_Gyro_X_RawOUT - aveX3;
+            KXG03_Gyro_Y_RawOUT2 = KXG03_Gyro_Y_RawOUT - aveY3;
+            KXG03_Gyro_Z_RawOUT2 = KXG03_Gyro_Z_RawOUT - aveZ3;
+            
+            /*
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  Y= %d", KXG03_Gyro_Y_RawOUT);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);
+            
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  aveY3= %d", aveY3);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);               
+            
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  Y= %d", KXG03_Gyro_Y_RawOUT2);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20); 
+            */                           
+            
+            //Scale Data
+            KXG03_Gyro_X = (float)KXG03_Gyro_X_RawOUT2 * 0.007813 + 0.000004;
+            KXG03_Gyro_Y = (float)KXG03_Gyro_Y_RawOUT2 * 0.007813 + 0.000004;
+            KXG03_Gyro_Z = (float)KXG03_Gyro_Z_RawOUT2 * 0.007813 + 0.000004;                                             
+
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  X= %0.2fdeg/s", KXG03_Gyro_X);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);     
+                
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  Y= %0.2fdeg/s", KXG03_Gyro_Y);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);            
+
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  Z= %0.2fdeg/s", KXG03_Gyro_Z);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);
+            
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "            ");
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);                           
+            
+            t = 0;
+            }
+        
+        else { 
+            //Read KXG03 Gyro Portion from the IC
+            i2c.write(KXG03_addr_w, &KXG03_Addr_ReadData, 1, RepStart);
+            i2c.read(KXG03_addr_r, &KXG03_Content_ReadData[0], 6, NoRepStart);                     
+                   
+            //reconfigure the data (taken from arduino code)
+            KXG03_Gyro_X_RawOUT = (KXG03_Content_ReadData[1]<<8) | (KXG03_Content_ReadData[0]);
+            KXG03_Gyro_Y_RawOUT = (KXG03_Content_ReadData[3]<<8) | (KXG03_Content_ReadData[2]);
+            KXG03_Gyro_Z_RawOUT = (KXG03_Content_ReadData[5]<<8) | (KXG03_Content_ReadData[4]);
+            
+            KXG03_Gyro_X_RawOUT2 = KXG03_Gyro_X_RawOUT - aveX3;
+            KXG03_Gyro_Y_RawOUT2 = KXG03_Gyro_Y_RawOUT - aveY3;
+            KXG03_Gyro_Z_RawOUT2 = KXG03_Gyro_Z_RawOUT - aveZ3;              
+            
+            //Scale Data
+            KXG03_Gyro_X = (float)KXG03_Gyro_X_RawOUT2 * 0.007813 + 0.000004;
+            KXG03_Gyro_Y = (float)KXG03_Gyro_Y_RawOUT2 * 0.007813 + 0.000004;
+            KXG03_Gyro_Z = (float)KXG03_Gyro_Z_RawOUT2 * 0.007813 + 0.000004;                                   
+            
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "Gyro Sensor:");
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20); 
+
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  X= %0.2fdeg/s", KXG03_Gyro_X);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);     
+                
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  Y= %0.2fdeg/s", KXG03_Gyro_Y);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);            
+
+            len = snprintf((char*) buf, MAX_REPLY_LEN, "  Z= %0.2fdeg/s", KXG03_Gyro_Z);
+            m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
+            wait_ms(20);
             
             len = snprintf((char*) buf, MAX_REPLY_LEN, "            ");
             m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
             wait_ms(20);
-        }        
-        #endif  
+            }
+        }
+        #endif
         i=1;
     }
 }
@@ -617,7 +804,7 @@
     i2c.write(KMX62_addr_w, &KMX62_CNTL2[0], 2, false);
     #endif
 
-    #ifdef color
+    #ifdef Color
     // 1. CNTL2 (0x3A), write (0x5F): 4g, Max RES, EN temp mag and accel
     i2c.write(BH1745_addr_w, &BH1745_persistence[0], 2, false);
     i2c.write(BH1745_addr_w, &BH1745_mode1[0], 2, false);
@@ -625,12 +812,12 @@
     i2c.write(BH1745_addr_w, &BH1745_mode3[0], 2, false);
     #endif
 
-    #ifdef KX022
-    i2c.write(KX022_addr_w, &KX022_Accel_CNTL1[0], 2, false);
-    i2c.write(KX022_addr_w, &KX022_Accel_ODCNTL[0], 2, false);
-    i2c.write(KX022_addr_w, &KX022_Accel_CNTL3[0], 2, false);
-    i2c.write(KX022_addr_w, &KX022_Accel_TILT_TIMER[0], 2, false);
-    i2c.write(KX022_addr_w, &KX022_Accel_CNTL2[0], 2, false);
+    #ifdef KX122
+    i2c.write(KX122_addr_w, &KX122_Accel_CNTL1[0], 2, false);
+    i2c.write(KX122_addr_w, &KX122_Accel_ODCNTL[0], 2, false);
+    i2c.write(KX122_addr_w, &KX122_Accel_CNTL3[0], 2, false);
+    i2c.write(KX122_addr_w, &KX122_Accel_TILT_TIMER[0], 2, false);
+    i2c.write(KX122_addr_w, &KX122_Accel_CNTL2[0], 2, false);
     #endif
     
     #ifdef Pressure
@@ -638,6 +825,10 @@
     i2c.write(Press_addr_w, &SLEEP[0], 2, false);
     i2c.write(Press_addr_w, &Mode_Control[0], 2, false);
     #endif
+    
+    #ifdef KXG03
+    i2c.write(KXG03_addr_w, &KXG03_STBY_REG[0], 2, false);        
+    #endif    
 
     //Start BTLE Initialization Section
     m_ble.init();