Added synchronization around I2C and SPI write/read register calls.

This addresses Issue #29.
kauailabs · Jan 1, 2017 · 3b0fc57 · 3b0fc57
1 parent fe1a6c4
commit 3b0fc57
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 20 deletions.
diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_I2C.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_I2C.java
@@ -27,7 +27,10 @@ public boolean init() {
 
     @Override
     public boolean write(byte address, byte value ) {
-        boolean success = port.write(address | 0x80, value);
+    	boolean success;
+    	synchronized(this){
+	        success = port.write(address | 0x80, value);
+    	}
         if ( !success && trace ) System.out.println("navX-MXP I2C Write Error");
         return success;
     }
@@ -40,9 +43,16 @@ public boolean read(byte first_address, byte[] buffer) {
         int buffer_offset = 0;
         while ( len > 0 ) {
             int read_len = (len > MAX_WPILIB_I2C_READ_BYTES) ? MAX_WPILIB_I2C_READ_BYTES : len;
-            byte[] read_buffer = new byte[read_len];            
-            if (port.write(first_address + buffer_offset, read_len) && 
-                port.readOnly(read_buffer, read_len) ) {
+            byte[] read_buffer = new byte[read_len];      
+            boolean write_ok;
+            boolean read_ok = false;
+            synchronized(this){
+            	write_ok = port.write(first_address + buffer_offset, read_len);
+            	if ( write_ok ) {
+            		read_ok = port.readOnly(read_buffer, read_len);
+            	}
+            }
+            if ( write_ok && read_ok ) {                
                 System.arraycopy(read_buffer, 0,  buffer, buffer_offset, read_len);
                 buffer_offset += read_len;
                 len -= read_len;

diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_SPI.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_SPI.java
@@ -49,7 +49,11 @@ public boolean write(byte address, byte value ) {
         cmd[0] = (byte) (address  | (byte)0x80);
         cmd[1] = value;
         cmd[2] = AHRSProtocol.getCRC(cmd, 2);
-        if ( port.write(cmd, cmd.length) != cmd.length) {
+        boolean write_ok;
+        synchronized(this) {
+        	write_ok = (port.write(cmd, cmd.length) == cmd.length);
+        }
+        if ( !write_ok ) {
             if (trace) System.out.println("navX-MXP SPI Read:  Write error");
             return false; // WRITE ERROR
         }
@@ -62,22 +66,24 @@ public boolean read(byte first_address, byte[] buffer) {
         cmd[0] = first_address;
         cmd[1] = (byte)buffer.length;
         cmd[2] = AHRSProtocol.getCRC(cmd, 2);
-        if ( port.write(cmd, cmd.length) != cmd.length ) {
-        	return false; // WRITE ERROR
-        }
-        // delay 200 us /* TODO:  What is min. granularity of delay()? */
-        Timer.delay(0.001);
-        byte[] received_data = new byte[buffer.length+1];
-        if ( port.read(true, received_data, received_data.length) != received_data.length ) {
-            if (trace) System.out.println("navX-MXP SPI Read:  Read error");
-            return false; // READ ERROR
-        }
-        byte crc = AHRSProtocol.getCRC(received_data, received_data.length - 1);
-        if ( crc != received_data[received_data.length-1] ) {
-            if (trace) System.out.println("navX-MXP SPI Read:  CRC error");        	
-            return false; // CRC ERROR
+        synchronized(this) {
+	        if ( port.write(cmd, cmd.length) != cmd.length ) {
+	        	return false; // WRITE ERROR
+	        }
+	        // delay 200 us /* TODO:  What is min. granularity of delay()? */
+	        Timer.delay(0.001);
+	        byte[] received_data = new byte[buffer.length+1];
+	        if ( port.read(true, received_data, received_data.length) != received_data.length ) {
+	            if (trace) System.out.println("navX-MXP SPI Read:  Read error");
+	            return false; // READ ERROR
+	        }
+	        byte crc = AHRSProtocol.getCRC(received_data, received_data.length - 1);
+	        if ( crc != received_data[received_data.length-1] ) {
+	            if (trace) System.out.println("navX-MXP SPI Read:  CRC error");        	
+	            return false; // CRC ERROR
+	        }
+	        System.arraycopy(received_data, 0, buffer, 0, received_data.length - 1);
         }
-        System.arraycopy(received_data, 0, buffer, 0, received_data.length - 1);
         return true;
     }