Diff: source/rpr0521_driver.cpp

Revision:

1:145f11782373

Parent:

0:0bcc203c5c75

Child:

2:56d144605997

--- a/source/rpr0521_driver.cpp	Mon Sep 12 09:53:15 2016 +0000
+++ b/source/rpr0521_driver.cpp	Tue Sep 20 11:15:09 2016 +0000
@@ -23,30 +23,40 @@
 /* rpr0521 driver*/
 
 uint8_t rpr0521_readId(){
-  uint8_t id;
-  uint8_t read_bytes;
+    uint8_t id;
+    uint8_t read_bytes;
+
+    read_bytes = read_register(SAD, RPR0521_MANUFACT, &id, 1);
+    if ( read_bytes > 0 ){
+        uint8_t partid;
 
-  read_bytes = read_register(SAD, RPR0521_MANUFACT, &id, 1);
-  if ( read_bytes > 0 ){
-    DEBUG_printf("Manufacturer: %u\n\r", id);
-    return(id);
+        DEBUG_printf("Manufacturer: %u\n\r", id);
+        read_bytes = read_register(SAD, RPR0521_SYSTEM_CONTROL, &partid, 1);
+        if ( read_bytes > 0 ){
+            DEBUG_printf("Part ID: %u\n\r", (partid & 0b00111111) );
+        return(partid);
+        }
+        else{
+            DEBUG_print("Part ID read failed.\n\r");
+            return 255;
+        }
     }
-  else{
-    DEBUG_print("Manufacturer read failed.\n\r");
-    return 255;
+    else{
+        DEBUG_print("Manufacturer read failed.\n\r");
+        return 255;
     }
 }
 
 void rpr0521_wait_until_found(){
-  uint8_t id;
-
-  id = rpr0521_readId();
-  while (id == 255){
-    wait(100);
+    uint8_t id;
+    
     id = rpr0521_readId();
-    }
-  return;
-  }
+    while (id == 255){
+        wait(100);
+        id = rpr0521_readId();
+        }
+    return;
+}
 
 void rpr0521_soft_reset(){
     write_register(SAD, RPR0521_SYSTEM_CONTROL, RPR0521_SYSTEM_CONTROL_SW_RESET_START);
@@ -76,11 +86,13 @@
 /* input param: data16, pointer to 3*16bit memory 
    return: error, true/false */
 bool rpr0521_read_data(uint16_t* data16){
-    uint8_t data[6];
+    #define RPR0521_DATA_LEN 6
+
+    uint8_t data[RPR0521_DATA_LEN];
     uint8_t read_bytes;
 
-    read_bytes = read_register(SAD, RPR0521_PS_DATA_LSBS, &data[0], 6);
-    if (read_bytes == 6){
+    read_bytes = read_register(SAD, RPR0521_PS_DATA_LSBS, &data[0], RPR0521_DATA_LEN);
+    if (read_bytes == RPR0521_DATA_LEN){
         data16[0] = (data[0]) | (data[1] << 8); //ps_data
         data16[1] = (data[2]) | (data[3] << 8); //als_data0
         data16[2] = (data[4]) | (data[5] << 8); //als_data1