From 28532322c61d105f68a5f7b7064d711d1b890369 Mon Sep 17 00:00:00 2001
From: Andrew Myers <amyers@novacoast.com>
Date: Wed, 13 Jul 2016 22:05:36 +0000
Subject: [PATCH] Updated /raspberry_video/LeptonThread to work for Lepton 3.
 Increased SPI speed and video now correctly assembles 4 segment image from
 Lepton 3.

---
 software/raspberrypi_video/LeptonThread.cpp | 126 +++++----
 software/raspberrypi_video/LeptonThread.h   |   3 +-
 software/raspberrypi_video/Makefile         | 269 ++++++++++++++++++++
 software/raspberrypi_video/SPI.cpp          |  13 +-
 4 files changed, 350 insertions(+), 61 deletions(-)
 create mode 100644 software/raspberrypi_video/Makefile

diff --git a/software/raspberrypi_video/LeptonThread.cpp b/software/raspberrypi_video/LeptonThread.cpp
index 6be567d..a077e43 100644
--- a/software/raspberrypi_video/LeptonThread.cpp
+++ b/software/raspberrypi_video/LeptonThread.cpp
@@ -1,5 +1,5 @@
 #include "LeptonThread.h"
-
+#include "stdio.h"
 #include "Palettes.h"
 #include "SPI.h"
 #include "Lepton_I2C.h"
@@ -8,37 +8,40 @@
 #define PACKET_SIZE_UINT16 (PACKET_SIZE/2)
 #define PACKETS_PER_FRAME 60
 #define FRAME_SIZE_UINT16 (PACKET_SIZE_UINT16*PACKETS_PER_FRAME)
-#define FPS 27;
+#define FPS 27
 
-LeptonThread::LeptonThread() : QThread()
-{
-}
+#define HAND_TEMP_THRESHOLD 8050
 
-LeptonThread::~LeptonThread() {
-}
+int readyToToggle = 1;
+
+LeptonThread::LeptonThread() : QThread(){}
+
+LeptonThread::~LeptonThread() {}
 
 void LeptonThread::run()
 {
-	//create the initial image
-	myImage = QImage(80, 60, QImage::Format_RGB888);
-
-	//open spi port
+	//Create the initial image and open the Spi port.
+	myImage = QImage(160, 120, QImage::Format_RGB888);
 	SpiOpenPort(0);
-
+	uint32_t totalCounts = 0;
+	uint16_t minValue = 65535;
+	uint16_t maxValue = 0;
+	int checkSegment = 1;
+	//Camera Loop
 	while(true) {
-
-		//read data packets from lepton over SPI
 		int resets = 0;
+
+		//Read through one segment, line by line.	
 		for(int j=0;j<PACKETS_PER_FRAME;j++) {
-			//if it's a drop packet, reset j to 0, set to -1 so he'll be at 0 again loop
 			read(spi_cs0_fd, result+sizeof(uint8_t)*PACKET_SIZE*j, sizeof(uint8_t)*PACKET_SIZE);
 			int packetNumber = result[j*PACKET_SIZE+1];
+
+			//Check to make sure packet number is correct, or else the Lepton is out of sync.
+			//We have 750 'resets' before re-syncing.
 			if(packetNumber != j) {
 				j = -1;
 				resets += 1;
 				usleep(1000);
-				//Note: we've selected 750 resets as an arbitrary limit, since there should never be 750 "null" packets between two valid transmissions at the current poll rate
-				//By polling faster, developers may easily exceed this count, and the down period between frames may then be flagged as a loss of sync
 				if(resets == 750) {
 					SpiClosePort(0);
 					usleep(750000);
@@ -46,64 +49,79 @@ void LeptonThread::run()
 				}
 			}
 		}
-		if(resets >= 30) {
-			qDebug() << "done reading, resets: " << resets;
-		}
 
-		frameBuffer = (uint16_t *)result;
 		int row, column;
 		uint16_t value;
-		uint16_t minValue = 65535;
-		uint16_t maxValue = 0;
 
-		
+		//Obtain the segment number from the header of line 20. It should never be 0.
+		uint8_t segNum = result[20*PACKET_SIZE]>>4;
+		if(segNum != checkSegment){
+			checkSegment = 1;
+			continue;
+		}
+		checkSegment++;
+		if(checkSegment > 4) {
+			checkSegment = 1;
+		}
+
+		 //Iterate through the current segment one line at a time. In this loop, we find the
+		 //minimum and maximum values of the image for AGC.
 		for(int i=0;i<FRAME_SIZE_UINT16;i++) {
-			//skip the first 2 uint16_t's of every packet, they're 4 header bytes
-			if(i % PACKET_SIZE_UINT16 < 2) {
-				continue;
-			}
-			
-			//flip the MSB and LSB at the last second
-			int temp = result[i*2];
-			result[i*2] = result[i*2+1];
-			result[i*2+1] = temp;
+			//Skip the header of each line
+			if(i % PACKET_SIZE_UINT16 < 2) continue; 
 			
-			value = frameBuffer[i];
+			//Flip the MSB and LSB for correct coloring.
+			frameBuffer[i + (segNum-1)*(FRAME_SIZE_UINT16)] = result[i*2]<<8 | result[i*2+1];
+	
+			value = frameBuffer[i+ (segNum-1)*FRAME_SIZE_UINT16];
+			totalCounts += value;
 			if(value > maxValue) {
 				maxValue = value;
 			}
-			if(value < minValue) {
+			if(value < minValue && value > 0) {
 				minValue = value;
 			}
 			column = i % PACKET_SIZE_UINT16 - 2;
-			row = i / PACKET_SIZE_UINT16 ;
+			row = i / (PACKET_SIZE_UINT16 * 2);
 		}
-
+	
+		//We only finalize the image for emission once all four segments are in.
+		if(segNum != 4)continue; 	
+		
+		//If the difference between Max and Min is 0, we need to get a new frame before emitting.
 		float diff = maxValue - minValue;
-		float scale = 255/diff;
-		QRgb color;
-		for(int i=0;i<FRAME_SIZE_UINT16;i++) {
-			if(i % PACKET_SIZE_UINT16 < 2) {
-				continue;
-			}
-			value = (frameBuffer[i] - minValue) * scale;
-			const int *colormap = colormap_ironblack;
-			color = qRgb(colormap[3*value], colormap[3*value+1], colormap[3*value+2]);
-			column = (i % PACKET_SIZE_UINT16 ) - 2;
-			row = i / PACKET_SIZE_UINT16;
-			myImage.setPixel(column, row, color);
-		}
+		if(diff != 0){
+			float scale = 255/diff;
+			QRgb color;
 
-		//lets emit the signal for update
-		emit updateImage(myImage);
+			//Iterates through the entire frame, all four segments, for colorization.
+			for(int i=0;i<FRAME_SIZE_UINT16*4;i++) {
+				//Skip the header of each line.
+				if(i % PACKET_SIZE_UINT16 < 2) continue; 
+				
+				value = (frameBuffer[i] - minValue) * scale;
+				const int *colormap = colormap_ironblack;
+				if(value > 255) value = 255;	
+				color = qRgb(colormap[3*value], colormap[3*value+1], colormap[3*value+2]);
+			
+				column = (i % PACKET_SIZE_UINT16 ) - 2;
+				row = i / (PACKET_SIZE_UINT16);
+				int new_row = (row/2);
+				int new_column = (row % 2 == 0) ? column : column + 80 ;
+				myImage.setPixel(new_column, new_row, color);
+			}
 
+			//Emit the finalized image for update.
+			emit updateImage(myImage);
+		}
+		minValue = 65535;
+		maxValue = 0;
+		totalCounts = 0;
 	}
 	
-	//finally, close SPI port just bcuz
 	SpiClosePort(0);
 }
 
 void LeptonThread::performFFC() {
-	//perform FFC
 	lepton_perform_ffc();
 }
diff --git a/software/raspberrypi_video/LeptonThread.h b/software/raspberrypi_video/LeptonThread.h
index 9d28d65..a909b05 100644
--- a/software/raspberrypi_video/LeptonThread.h
+++ b/software/raspberrypi_video/LeptonThread.h
@@ -36,7 +36,8 @@ public slots:
   QImage myImage;
 
   uint8_t result[PACKET_SIZE*PACKETS_PER_FRAME];
-  uint16_t *frameBuffer;
+//  uint16_t* frameBuffer;
+  uint16_t frameBuffer[FRAME_SIZE_UINT16*4];
 
 };
 
diff --git a/software/raspberrypi_video/Makefile b/software/raspberrypi_video/Makefile
new file mode 100644
index 0000000..5b2e25b
--- /dev/null
+++ b/software/raspberrypi_video/Makefile
@@ -0,0 +1,269 @@
+#############################################################################
+# Makefile for building: raspberrypi_video
+# Generated by qmake (2.01a) (Qt 4.8.6) on: Wed Jul 13 21:59:35 2016
+# Project:  raspberrypi_video.pro
+# Template: app
+# Command: /usr/lib/arm-linux-gnueabihf/qt4/bin/qmake -o Makefile raspberrypi_video.pro
+#############################################################################
+
+####### Compiler, tools and options
+
+CC            = gcc
+CXX           = g++
+DEFINES       = -DQT_WEBKIT -DQT_NO_DEBUG -DQT_GUI_LIB -DQT_CORE_LIB -DQT_SHARED
+CFLAGS        = -pipe -O2 -Wall -W -D_REENTRANT $(DEFINES)
+CXXFLAGS      = -pipe -O2 -Wall -W -D_REENTRANT $(DEFINES)
+INCPATH       = -I/usr/share/qt4/mkspecs/linux-g++ -I. -I/usr/include/qt4/QtCore -I/usr/include/qt4/QtGui -I/usr/include/qt4 -I. -I../raspberrypi_libs -Igen_mocs
+LINK          = g++
+LFLAGS        = -Wl,-O1
+LIBS          = $(SUBLIBS)  -L/usr/lib/arm-linux-gnueabihf -L../raspberrypi_libs/leptonSDKEmb32PUB/Debug -lLEPTON_SDK -lQtGui -lQtCore -lpthread 
+AR            = ar cqs
+RANLIB        = 
+QMAKE         = /usr/lib/arm-linux-gnueabihf/qt4/bin/qmake
+TAR           = tar -cf
+COMPRESS      = gzip -9f
+COPY          = cp -f
+SED           = sed
+COPY_FILE     = $(COPY)
+COPY_DIR      = $(COPY) -r
+STRIP         = strip
+INSTALL_FILE  = install -m 644 -p
+INSTALL_DIR   = $(COPY_DIR)
+INSTALL_PROGRAM = install -m 755 -p
+DEL_FILE      = rm -f
+SYMLINK       = ln -f -s
+DEL_DIR       = rmdir
+MOVE          = mv -f
+CHK_DIR_EXISTS= test -d
+MKDIR         = mkdir -p
+
+####### Output directory
+
+OBJECTS_DIR   = gen_objs/
+
+####### Files
+
+SOURCES       = Lepton_I2C.cpp \
+		LeptonThread.cpp \
+		main.cpp \
+		MyLabel.cpp \
+		Palettes.cpp \
+		SPI.cpp gen_mocs/moc_LeptonThread.cpp \
+		gen_mocs/moc_MyLabel.cpp
+OBJECTS       = gen_objs/Lepton_I2C.o \
+		gen_objs/LeptonThread.o \
+		gen_objs/main.o \
+		gen_objs/MyLabel.o \
+		gen_objs/Palettes.o \
+		gen_objs/SPI.o \
+		gen_objs/moc_LeptonThread.o \
+		gen_objs/moc_MyLabel.o
+DIST          = /usr/share/qt4/mkspecs/common/unix.conf \
+		/usr/share/qt4/mkspecs/common/linux.conf \
+		/usr/share/qt4/mkspecs/common/gcc-base.conf \
+		/usr/share/qt4/mkspecs/common/gcc-base-unix.conf \
+		/usr/share/qt4/mkspecs/common/g++-base.conf \
+		/usr/share/qt4/mkspecs/common/g++-unix.conf \
+		/usr/share/qt4/mkspecs/qconfig.pri \
+		/usr/share/qt4/mkspecs/modules/qt_webkit.pri \
+		/usr/share/qt4/mkspecs/features/qt_functions.prf \
+		/usr/share/qt4/mkspecs/features/qt_config.prf \
+		/usr/share/qt4/mkspecs/features/exclusive_builds.prf \
+		/usr/share/qt4/mkspecs/features/default_pre.prf \
+		/usr/share/qt4/mkspecs/features/release.prf \
+		/usr/share/qt4/mkspecs/features/default_post.prf \
+		/usr/share/qt4/mkspecs/features/shared.prf \
+		/usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf \
+		/usr/share/qt4/mkspecs/features/warn_on.prf \
+		/usr/share/qt4/mkspecs/features/qt.prf \
+		/usr/share/qt4/mkspecs/features/unix/thread.prf \
+		/usr/share/qt4/mkspecs/features/moc.prf \
+		/usr/share/qt4/mkspecs/features/resources.prf \
+		/usr/share/qt4/mkspecs/features/uic.prf \
+		/usr/share/qt4/mkspecs/features/yacc.prf \
+		/usr/share/qt4/mkspecs/features/lex.prf \
+		/usr/share/qt4/mkspecs/features/include_source_dir.prf \
+		raspberrypi_video.pro
+QMAKE_TARGET  = raspberrypi_video
+DESTDIR       = 
+TARGET        = raspberrypi_video
+
+first: all
+####### Implicit rules
+
+.SUFFIXES: .o .c .cpp .cc .cxx .C
+
+.cpp.o:
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<"
+
+.cc.o:
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<"
+
+.cxx.o:
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<"
+
+.C.o:
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<"
+
+.c.o:
+	$(CC) -c $(CFLAGS) $(INCPATH) -o "$@" "$<"
+
+####### Build rules
+
+all: Makefile $(TARGET)
+
+$(TARGET): sdk $(OBJECTS)  
+	$(LINK) $(LFLAGS) -o $(TARGET) $(OBJECTS) $(OBJCOMP) $(LIBS)
+
+Makefile: raspberrypi_video.pro  /usr/share/qt4/mkspecs/linux-g++/qmake.conf /usr/share/qt4/mkspecs/common/unix.conf \
+		/usr/share/qt4/mkspecs/common/linux.conf \
+		/usr/share/qt4/mkspecs/common/gcc-base.conf \
+		/usr/share/qt4/mkspecs/common/gcc-base-unix.conf \
+		/usr/share/qt4/mkspecs/common/g++-base.conf \
+		/usr/share/qt4/mkspecs/common/g++-unix.conf \
+		/usr/share/qt4/mkspecs/qconfig.pri \
+		/usr/share/qt4/mkspecs/modules/qt_webkit.pri \
+		/usr/share/qt4/mkspecs/features/qt_functions.prf \
+		/usr/share/qt4/mkspecs/features/qt_config.prf \
+		/usr/share/qt4/mkspecs/features/exclusive_builds.prf \
+		/usr/share/qt4/mkspecs/features/default_pre.prf \
+		/usr/share/qt4/mkspecs/features/release.prf \
+		/usr/share/qt4/mkspecs/features/default_post.prf \
+		/usr/share/qt4/mkspecs/features/shared.prf \
+		/usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf \
+		/usr/share/qt4/mkspecs/features/warn_on.prf \
+		/usr/share/qt4/mkspecs/features/qt.prf \
+		/usr/share/qt4/mkspecs/features/unix/thread.prf \
+		/usr/share/qt4/mkspecs/features/moc.prf \
+		/usr/share/qt4/mkspecs/features/resources.prf \
+		/usr/share/qt4/mkspecs/features/uic.prf \
+		/usr/share/qt4/mkspecs/features/yacc.prf \
+		/usr/share/qt4/mkspecs/features/lex.prf \
+		/usr/share/qt4/mkspecs/features/include_source_dir.prf \
+		/usr/lib/arm-linux-gnueabihf/libQtGui.prl \
+		/usr/lib/arm-linux-gnueabihf/libQtCore.prl
+	$(QMAKE) -o Makefile raspberrypi_video.pro
+/usr/share/qt4/mkspecs/common/unix.conf:
+/usr/share/qt4/mkspecs/common/linux.conf:
+/usr/share/qt4/mkspecs/common/gcc-base.conf:
+/usr/share/qt4/mkspecs/common/gcc-base-unix.conf:
+/usr/share/qt4/mkspecs/common/g++-base.conf:
+/usr/share/qt4/mkspecs/common/g++-unix.conf:
+/usr/share/qt4/mkspecs/qconfig.pri:
+/usr/share/qt4/mkspecs/modules/qt_webkit.pri:
+/usr/share/qt4/mkspecs/features/qt_functions.prf:
+/usr/share/qt4/mkspecs/features/qt_config.prf:
+/usr/share/qt4/mkspecs/features/exclusive_builds.prf:
+/usr/share/qt4/mkspecs/features/default_pre.prf:
+/usr/share/qt4/mkspecs/features/release.prf:
+/usr/share/qt4/mkspecs/features/default_post.prf:
+/usr/share/qt4/mkspecs/features/shared.prf:
+/usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf:
+/usr/share/qt4/mkspecs/features/warn_on.prf:
+/usr/share/qt4/mkspecs/features/qt.prf:
+/usr/share/qt4/mkspecs/features/unix/thread.prf:
+/usr/share/qt4/mkspecs/features/moc.prf:
+/usr/share/qt4/mkspecs/features/resources.prf:
+/usr/share/qt4/mkspecs/features/uic.prf:
+/usr/share/qt4/mkspecs/features/yacc.prf:
+/usr/share/qt4/mkspecs/features/lex.prf:
+/usr/share/qt4/mkspecs/features/include_source_dir.prf:
+/usr/lib/arm-linux-gnueabihf/libQtGui.prl:
+/usr/lib/arm-linux-gnueabihf/libQtCore.prl:
+qmake:  FORCE
+	@$(QMAKE) -o Makefile raspberrypi_video.pro
+
+dist: 
+	@$(CHK_DIR_EXISTS) gen_objs/raspberrypi_video1.0.0 || $(MKDIR) gen_objs/raspberrypi_video1.0.0 
+	$(COPY_FILE) --parents $(SOURCES) $(DIST) gen_objs/raspberrypi_video1.0.0/ && $(COPY_FILE) --parents Lepton_I2C.h LeptonThread.h MyLabel.h Palettes.h SPI.h gen_objs/raspberrypi_video1.0.0/ && $(COPY_FILE) --parents Lepton_I2C.cpp LeptonThread.cpp main.cpp MyLabel.cpp Palettes.cpp SPI.cpp gen_objs/raspberrypi_video1.0.0/ && (cd `dirname gen_objs/raspberrypi_video1.0.0` && $(TAR) raspberrypi_video1.0.0.tar raspberrypi_video1.0.0 && $(COMPRESS) raspberrypi_video1.0.0.tar) && $(MOVE) `dirname gen_objs/raspberrypi_video1.0.0`/raspberrypi_video1.0.0.tar.gz . && $(DEL_FILE) -r gen_objs/raspberrypi_video1.0.0
+
+
+clean:compiler_clean 
+	-$(DEL_FILE) $(OBJECTS)
+	-$(DEL_FILE) -r $(OBJECTS_DIR) gen_mocs
+	-$(DEL_FILE) *~ core *.core
+
+
+####### Sub-libraries
+
+distclean: clean
+	-$(DEL_FILE) $(TARGET) 
+	-$(DEL_FILE) Makefile
+
+
+sdk:
+	make -C ../raspberrypi_libs/leptonSDKEmb32PUB
+
+sdkclean:
+	make -C ../raspberrypi_libs/leptonSDKEmb32PUB clean
+
+check: first
+
+mocclean: compiler_moc_header_clean compiler_moc_source_clean
+
+mocables: compiler_moc_header_make_all compiler_moc_source_make_all
+
+compiler_moc_header_make_all: gen_mocs/moc_LeptonThread.cpp gen_mocs/moc_MyLabel.cpp
+compiler_moc_header_clean:
+	-$(DEL_FILE) gen_mocs/moc_LeptonThread.cpp gen_mocs/moc_MyLabel.cpp
+gen_mocs/moc_LeptonThread.cpp: LeptonThread.h
+	/usr/lib/arm-linux-gnueabihf/qt4/bin/moc $(DEFINES) $(INCPATH) LeptonThread.h -o gen_mocs/moc_LeptonThread.cpp
+
+gen_mocs/moc_MyLabel.cpp: MyLabel.h
+	/usr/lib/arm-linux-gnueabihf/qt4/bin/moc $(DEFINES) $(INCPATH) MyLabel.h -o gen_mocs/moc_MyLabel.cpp
+
+compiler_rcc_make_all:
+compiler_rcc_clean:
+compiler_image_collection_make_all: qmake_image_collection.cpp
+compiler_image_collection_clean:
+	-$(DEL_FILE) qmake_image_collection.cpp
+compiler_moc_source_make_all:
+compiler_moc_source_clean:
+compiler_uic_make_all:
+compiler_uic_clean:
+compiler_yacc_decl_make_all:
+compiler_yacc_decl_clean:
+compiler_yacc_impl_make_all:
+compiler_yacc_impl_clean:
+compiler_lex_make_all:
+compiler_lex_clean:
+compiler_clean: compiler_moc_header_clean 
+
+####### Compile
+
+gen_objs/Lepton_I2C.o: Lepton_I2C.cpp Lepton_I2C.h
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/Lepton_I2C.o Lepton_I2C.cpp
+
+gen_objs/LeptonThread.o: LeptonThread.cpp LeptonThread.h \
+		Palettes.h \
+		SPI.h \
+		Lepton_I2C.h
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/LeptonThread.o LeptonThread.cpp
+
+gen_objs/main.o: main.cpp LeptonThread.h \
+		MyLabel.h
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/main.o main.cpp
+
+gen_objs/MyLabel.o: MyLabel.cpp MyLabel.h
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/MyLabel.o MyLabel.cpp
+
+gen_objs/Palettes.o: Palettes.cpp Palettes.h
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/Palettes.o Palettes.cpp
+
+gen_objs/SPI.o: SPI.cpp SPI.h
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/SPI.o SPI.cpp
+
+gen_objs/moc_LeptonThread.o: gen_mocs/moc_LeptonThread.cpp 
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/moc_LeptonThread.o gen_mocs/moc_LeptonThread.cpp
+
+gen_objs/moc_MyLabel.o: gen_mocs/moc_MyLabel.cpp 
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o gen_objs/moc_MyLabel.o gen_mocs/moc_MyLabel.cpp
+
+####### Install
+
+install:   FORCE
+
+uninstall:   FORCE
+
+FORCE:
+
diff --git a/software/raspberrypi_video/SPI.cpp b/software/raspberrypi_video/SPI.cpp
index 690d743..ded34bc 100644
--- a/software/raspberrypi_video/SPI.cpp
+++ b/software/raspberrypi_video/SPI.cpp
@@ -5,7 +5,7 @@ int spi_cs1_fd = -1;
 
 unsigned char spi_mode = SPI_MODE_3;
 unsigned char spi_bitsPerWord = 8;
-unsigned int spi_speed = 10000000;
+unsigned int spi_speed = 32000000;
 
 int SpiOpenPort (int spi_device)
 {
@@ -13,18 +13,19 @@ int SpiOpenPort (int spi_device)
 	int *spi_cs_fd;
 
 
-	//----- SET SPI MODE -----
+	//----- SET SPI MODE -----//
 	//SPI_MODE_0 (0,0)  CPOL=0 (Clock Idle low level), CPHA=0 (SDO transmit/change edge active to idle)
 	//SPI_MODE_1 (0,1)  CPOL=0 (Clock Idle low level), CPHA=1 (SDO transmit/change edge idle to active)
 	//SPI_MODE_2 (1,0)  CPOL=1 (Clock Idle high level), CPHA=0 (SDO transmit/change edge active to idle)
 	//SPI_MODE_3 (1,1)  CPOL=1 (Clock Idle high level), CPHA=1 (SDO transmit/change edge idle to active)
 	spi_mode = SPI_MODE_3;
 
-	//----- SET BITS PER WORD -----
+	//----- SET BITS PER WORD -----//
 	spi_bitsPerWord = 8;
 
-	//----- SET SPI BUS SPEED -----
-	spi_speed = 10000000;				//1000000 = 1MHz (1uS per bit)
+	//----- SET SPI BUS SPEED -----//
+	spi_speed = 32000000;		// Use 24000000 = 24MHz for RBPi 2
+					// Use 32000000 = 32MHz for RBPi 3
 
 
 	if (spi_device)
@@ -36,7 +37,7 @@ int SpiOpenPort (int spi_device)
 	if (spi_device)
 		*spi_cs_fd = open(std::string("/dev/spidev0.1").c_str(), O_RDWR);
 	else
-		*spi_cs_fd = open(std::string("/dev/spidev0.0").c_str(), O_RDWR);
+		*spi_cs_fd = open(std::string("/dev/spidev0.1").c_str(), O_RDWR);
 
 	if (*spi_cs_fd < 0)
 	{