├── lepton_camera
    ├── FLIR_Lepton_Data_Brief.pdf
    ├── src
    │   ├── video
    │   │   ├── CMakeLists.txt
    │   │   └── lepton.cpp
    │   └── SPI
    │   │   ├── CMakeLists.txt
    │   │   ├── SPI.h
    │   │   └── SPI.cpp
    ├── README.md
    ├── package.xml
    └── CMakeLists.txt
├── README.md
└── I2C_Lepton
    ├── FLIR_Lepton_Software_Interface_Description_Document_110_0144_03.pdf
    ├── README.md
    ├── main.cpp
    ├── I2C_Interface.h
    └── I2C_Interface.cpp


/lepton_camera/FLIR_Lepton_Data_Brief.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PX4/Lepton/HEAD/lepton_camera/FLIR_Lepton_Data_Brief.pdf


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Lepton IR Camera ROS NODE
2 | Package is currently built and aims to establishing stable ROS node for Lepton IR camera.
3 | 


--------------------------------------------------------------------------------
/lepton_camera/src/video/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | # Add executable leptonvideo
2 | add_executable (leptonvideo lepton.cpp )
3 | target_link_libraries (leptonvideo SPI ${catkin_LIBRARIES})
4 | 


--------------------------------------------------------------------------------
/lepton_camera/README.md:
--------------------------------------------------------------------------------
1 | # Video stream
2 | This interfaces reads out the VoSPIStream from the lepton camera. Details of the data transfer can be found in FLIR_Lepton_Data_Brief.pdf. 
3 | 


--------------------------------------------------------------------------------
/I2C_Lepton/FLIR_Lepton_Software_Interface_Description_Document_110_0144_03.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PX4/Lepton/HEAD/I2C_Lepton/FLIR_Lepton_Software_Interface_Description_Document_110_0144_03.pdf


--------------------------------------------------------------------------------
/lepton_camera/src/SPI/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | 
 2 | 
 3 | # Include directory for header file
 4 | find_path(SPI_INCLUDE SPI.h ./)
 5 | include_directories(${SPI_INCLUDE})
 6 | 
 7 | #Add SPI library
 8 | add_library (SPI SPI.cpp)
 9 | target_include_directories (SPI PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
10 | 


--------------------------------------------------------------------------------
/I2C_Lepton/README.md:
--------------------------------------------------------------------------------
1 | # I2C_Lepton
2 | I2C interface for Lepton Camera. This interface is based on the software interface description delivered by FLIR (FLIR_Lepton_Software_Interface_Description_Document_110_0144_03.pdf). The I2C interfaces works. However not all possible commands have been implemented. Also the application does not deliver a userfriendly readout of the data yet. 
3 | 


--------------------------------------------------------------------------------
/lepton_camera/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format='2'>
 3 |   <name>lepton_camera</name>
 4 |   <version>0.1.0</version>
 5 |   <description>The lepton package</description>
 6 |   <maintainer email="kaiserfabian@hotmail.com">Chao Qu</maintainer>
 7 | 
 8 |   <license>BSD</license>
 9 | 
10 |   <buildtool_depend>catkin</buildtool_depend>
11 | 
12 |   <depend>roscpp</depend>
13 |   <depend>nodelet</depend>
14 |   <build_depend>message_generation</build_depend>
15 |   <exec_depend>message_runtime</exec_depend>
16 | 
17 |   <export>
18 |       <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
19 |   </export>
20 | </package>
21 | 


--------------------------------------------------------------------------------
/lepton_camera/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(lepton_camera)
 3 | 
 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake)
 6 | 
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |     roscpp nodelet
 9 |     message_generation
10 |     )
11 | find_package(Boost COMPONENTS program_options REQUIRED)
12 | 
13 | 
14 | 
15 | 
16 | catkin_package(
17 |     #  INCLUDE_DIRS include
18 |     #  LIBRARIES bluefox2
19 |     CATKIN_DEPENDS message_runtime
20 |     #  DEPENDS system_lib
21 |     )
22 | 
23 | include_directories(
24 |     include
25 |     ${catkin_INCLUDE_DIRS}
26 |     )
27 | 
28 | add_subdirectory(src/video)
29 | add_subdirectory(src/SPI)
30 | 
31 | # dependencies
32 | add_dependencies(
33 |     ${catkin_EXPORTED_TARGETS}
34 |     ${PROJECT_NAME}_gencfg
35 |     ${PROJECT_NAME}_gencpp
36 |     ${${PROJECT_NAME}_EXPORTED_TARGETS}
37 |     )
38 | 
39 | 


--------------------------------------------------------------------------------
/lepton_camera/src/SPI/SPI.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * SPI testing utility (using spidev driver)
 3 |  *
 4 |  * Copyright (c) 2007  MontaVista Software, Inc.
 5 |  * Copyright (c) 2007  Anton Vorontsov <avorontsov@ru.mvista.com>
 6 |  *
 7 |  * This program is free software; you can redistribute it and/or modify
 8 |  * it under the terms of the GNU General Public License as published by
 9 |  * the Free Software Foundation; either version 2 of the License.
10 |  *
11 |  * Cross-compile with cross-gcc -I/path/to/cross-kernel/include
12 |  */
13 | 
14 | #ifndef SPI_H
15 | #define SPI_H
16 | 
17 | #include <string>
18 | #include <stdint.h>
19 | #include <unistd.h>
20 | #include <stdio.h>
21 | #include <stdlib.h>
22 | #include <getopt.h>
23 | #include <fcntl.h>
24 | #include <sys/ioctl.h>
25 | #include <linux/types.h>
26 | #include <linux/spi/spidev.h>
27 | 
28 | extern int spi_cs0_fd;
29 | extern int spi_cs1_fd;
30 | extern unsigned char spi_mode;
31 | extern unsigned char spi_bitsPerWord;
32 | extern unsigned int spi_speed;
33 | extern unsigned int spi_delay;
34 | 
35 | int SpiOpenPort(int spi_device);
36 | int SpiClosePort(int spi_device);
37 | 
38 | #endif
39 | 


--------------------------------------------------------------------------------
/I2C_Lepton/main.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | #include "I2C_Interface.h"
 3 | #include <stdio.h>
 4 | 
 5 | 
 6 | unsigned char buf_out[100]; // output from master
 7 | unsigned char buf_in[100];// input for master from slave
 8 | unsigned char data_out[100]; // data to be transferred from master to slave
 9 | const char i2c_name[40] = "/dev/i2c-1";
10 | const int adress_lepton = 0x2A;
11 | 
12 | int main() {
13 | 
14 | 	unsigned short CommandID;
15 | 	unsigned short CommandWordLength;// number of words that are transferred
16 | 	unsigned short CommandType; // (Get=0; Set=1; Run=2)
17 | 
18 | 	// Read in desired I2C command from user over command prompt
19 | 	int CommandIdx;
20 | 	CommandIdx=ReadI2CCommand();
21 | 	CommandID=commandList[CommandIdx][0];
22 | 	CommandWordLength=commandList[CommandIdx][1];
23 | 	CommandType=commandList[CommandIdx][2];
24 | 
25 | 	unsigned short CommandByteLength=CommandWordLength<<1; // number of bytes that are transferred
26 | 
27 | 
28 | 
29 | 	// Establish I2C bus to Lepton camera
30 | 	file = i2c_init(i2c_name, adress_lepton);
31 | 
32 | 	// Execute desired Command
33 | 	WriteToCommandReg(CommandID,CommandWordLength,CommandType);
34 | 
35 | 	// Read data from data register in master input buffer
36 | 	ReadDataReg(CommandByteLength);
37 | 
38 | 	// Display content in master input buffer
39 | 	ReadOut(CommandByteLength);
40 | 
41 | 	// Close I2C bus to Lepton camera
42 | 	i2c_close();
43 | 
44 | 	return 0;
45 | 
46 | }
47 | 
48 | 


--------------------------------------------------------------------------------
/I2C_Lepton/I2C_Interface.h:
--------------------------------------------------------------------------------
 1 | 
 2 | #ifndef I2C_INTERFACE
 3 | #define I2C_INTERFACE
 4 | 
 5 | extern int ak; // acknowledged integer
 6 | extern unsigned char buf_out[100]; // output from master
 7 | extern unsigned char buf_in[100];// input for master from slave
 8 | extern unsigned char data_out[100]; //data that is transferred from master to slave
 9 | extern int file; // file descriptor for lepton camera
10 | extern const int adress_lepton;
11 | extern const char i2c_name[40];
12 | 
13 | /* Commands
14 | ***************
15 | Total number of known commands is saved in "n_commands"
16 | Commands are saved in two dimensional array "commandList":
17 |  	First column: 	command ID
18 | 	Second column: 	number of words transmitted
19 | 	Third column: 	type of command (Get=0; Set=1; Run=2)
20 | */
21 | extern const int n_commands;
22 | extern unsigned short commandList[19][3];
23 | 
24 | // Functions
25 | extern int i2c_init(const char *filename, int addr);
26 | extern void i2c_close();
27 | extern void i2c_read(int n_in, int n_out);
28 | extern void WriteToCommandReg(unsigned short CommandID, unsigned short  CommandDataLength, unsigned short CommandIdx);
29 | extern void WriteToDatalenghtReg(unsigned short DataLength);
30 | extern void WriteToDataReg(unsigned short n_words);
31 | extern void WaitingTillAccomplished();
32 | extern void ReadDataReg(unsigned short n_read);
33 | extern void ReadOut(unsigned short n_read);
34 | extern int ReadI2CCommand();
35 | 
36 | #endif
37 | 


--------------------------------------------------------------------------------
/lepton_camera/src/SPI/SPI.cpp:
--------------------------------------------------------------------------------
 1 | #include "SPI.h"
 2 | 
 3 | int spi_cs0_fd = -1;
 4 | 
 5 | unsigned char spi_mode = SPI_MODE_3;
 6 | unsigned char spi_bitsPerWord = 8;
 7 | unsigned int spi_speed = 2000000;
 8 | unsigned int spi_delay = 0;
 9 | 
10 | int  SpiOpenPort(int spi_device) {
11 | 	int status_value = -1;
12 | 	int *spi_cs_fd;
13 | 
14 | 	//----- SET SPI MODE -----
15 | 	//SPI_MODE_0 (0,0)  CPOL=0 (Clock Idle low level), CPHA=0 (SDO transmit/change edge active to idle)
16 | 	//SPI_MODE_1 (0,1)  CPOL=0 (Clock Idle low level), CPHA=1 (SDO transmit/change edge idle to active)
17 | 	//SPI_MODE_2 (1,0)  CPOL=1 (Clock Idle high level), CPHA=0 (SDO transmit/change edge active to idle)
18 | 	//SPI_MODE_3 (1,1)  CPOL=1 (Clock Idle high level), CPHA=1 (SDO transmit/change edge idle to active)
19 | 	spi_mode = SPI_MODE_3;
20 | 
21 | 	//----- SET BITS PER WORD -----
22 | 	spi_bitsPerWord = 8;
23 | 
24 | 	//----- SET SPI BUS SPEED -----
25 | 	spi_speed = 20000000; //1000000 = 1MHz (1uS per bit)
26 | 
27 | 	spi_cs_fd = &spi_cs0_fd;
28 | 
29 | 	*spi_cs_fd = open(std::string("/dev/spidev0.0").c_str(), O_RDWR);
30 | 
31 | 	if (*spi_cs_fd < 0) {
32 | 		perror("Error - Could not open SPI device");
33 | 		exit(1);
34 | 	}
35 | 
36 | 	status_value = ioctl(*spi_cs_fd, SPI_IOC_WR_MODE, &spi_mode);
37 | 	if (status_value < 0) {
38 | 		perror("Could not set SPIMode (WR)...ioctl fail");
39 | 		exit(1);
40 | 	}
41 | 
42 | 	status_value = ioctl(*spi_cs_fd, SPI_IOC_RD_MODE, &spi_mode);
43 | 	if (status_value < 0) {
44 | 		perror("Could not set SPIMode (RD)...ioctl fail");
45 | 		exit(1);
46 | 	}
47 | 
48 | 	status_value = ioctl(*spi_cs_fd, SPI_IOC_WR_BITS_PER_WORD,
49 | 			&spi_bitsPerWord);
50 | 	if (status_value < 0) {
51 | 		perror("Could not set SPI bitsPerWord (WR)...ioctl fail");
52 | 		exit(1);
53 | 	}
54 | 
55 | 	status_value = ioctl(*spi_cs_fd, SPI_IOC_RD_BITS_PER_WORD,
56 | 			&spi_bitsPerWord);
57 | 	if (status_value < 0) {
58 | 		perror("Could not set SPI bitsPerWord(RD)...ioctl fail");
59 | 		exit(1);
60 | 	}
61 | 
62 | 	status_value = ioctl(*spi_cs_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
63 | 	if (status_value < 0) {
64 | 		perror("Could not set SPI speed (WR)...ioctl fail");
65 | 		exit(1);
66 | 	}
67 | 
68 | 	status_value = ioctl(*spi_cs_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
69 | 	if (status_value < 0) {
70 | 		perror("Could not set SPI speed (RD)...ioctl fail");
71 | 		exit(1);
72 | 	}
73 | 	return (status_value);
74 | }
75 | 
76 | int SpiClosePort(int spi_device) {
77 | 	int status_value = -1;
78 | 	int *spi_cs_fd;
79 | 
80 | 
81 | 		spi_cs_fd = &spi_cs0_fd;
82 | 
83 | 	status_value = close(*spi_cs_fd);
84 | 	if (status_value < 0) {
85 | 		perror("Error - Could not close SPI device");
86 | 		exit(1);
87 | 	}
88 | 	return (status_value);
89 | }
90 | 


--------------------------------------------------------------------------------
/I2C_Lepton/I2C_Interface.cpp:
--------------------------------------------------------------------------------
  1 | #include <errno.h>
  2 | #include <string.h>
  3 | #include <stdio.h>
  4 | #include <stdlib.h>
  5 | #include <unistd.h>
  6 | #include <linux/i2c-dev.h>
  7 | #include <sys/ioctl.h>
  8 | #include <sys/types.h>
  9 | #include <sys/stat.h>
 10 | #include <fcntl.h>
 11 | #include "I2C_Interface.h"
 12 | 
 13 | int ak; // acknowledge integer
 14 | int file; // file descriptor for lepton camera
 15 | 
 16 | unsigned short commandList[19][3]={
 17 | 		{(unsigned short)0x0100,2,0}, 	// Get AGC Enable State
 18 | 		{(unsigned short)0x0101,2,1}, 	// Set AGC Enable State (1.Word:{0,1,2}; 2.Word:0)
 19 | 		{(unsigned short)0x0124,1,0},	// Get AGC HEQ Damping factor
 20 | 		{(unsigned short)0x0125,1,1}, 	// Set AGC HEQ Damping factor
 21 | 		{(unsigned short)0x012C,1,0},	// Get AGC HEQ Clip Limit High
 22 | 		{(unsigned short)0x012D,1,1}, 	// Set AGC HEQ Clip Limit High (1.Word:[0,4800])
 23 | 		{(unsigned short)0x0144,2,0},	// Get AGC HEQ Output Scale Factor
 24 | 		{(unsigned short)0x0145,2,1},	// Set AGC HEQ Output Scale Factor (1.Word:{0,1}; 2.Word:0)
 25 | 		{(unsigned short)0x0148,2,0},	// Get AGC Calculation enable state
 26 | 		{(unsigned short)0x0149,2,1},	// Set AGC Calculation enable state (1.Word:{0,1,2}; 2.Word:0)
 27 | 		{(unsigned short)0x0208,4,0},
 28 | 		{(unsigned short)0x0210,1,0},
 29 | 		{(unsigned short)0x2014,1,0},
 30 | 		{(unsigned short)0x0218,2,0}, // Get SYS Telemetry state
 31 | 		{(unsigned short)0x0219,2,1}, // Set SYS Telemetry state (1.Word:{0,1,2}; 2.Word:0)
 32 | 		{(unsigned short)0x021C,2,0},// Get SYS Telemetry location
 33 | 		{(unsigned short)0x021D,2,1},// Set SYS Telemetry location (1.Word:{0,1,2}; 2.Word:0)
 34 | 		{(unsigned short)0x0324,2,0}, // Get Video Freeze Enable State
 35 | 		{(unsigned short)0x0325,2,1} // Set Video Freeze Enable State (1.Word:{0,1,2}; 2.Word:0)
 36 | };
 37 | const int n_commands=19;
 38 | 
 39 | int i2c_init(const char *filename, const int addr) {
 40 | 
 41 | 	// Check I2C connectivity
 42 | 	file = open(filename, O_RDWR);
 43 | 	if (file < 0) {
 44 | 		printf("Failed to open bus\n");
 45 | 
 46 | 	}
 47 | 
 48 | 	// Check connectivity to lepton camera
 49 | 	ak = ioctl(file, I2C_SLAVE, addr);
 50 | 	if (ak < 0) {
 51 | 		printf("Failed to acquire bus access and/or talk to slave\n");
 52 | 	}
 53 | 	return file;
 54 | }
 55 | void i2c_close(){
 56 | 	// closes I2C connection
 57 | 	close(file);
 58 | 	return;
 59 | }
 60 | 
 61 | 
 62 | void i2c_read(int n_in, int n_out) {
 63 | 
 64 | 	// Setting pointer to desired register address
 65 | 	ak = write(file, buf_out, n_out);
 66 | 
 67 | 	if (ak != n_out) {
 68 | 		printf("Failed to access desired register\n");
 69 | 	}
 70 | 
 71 | 	// Reading in content of desired register and saving it in master input buffer
 72 | 	ak = read(file, buf_in, n_in);
 73 | 	if (ak != n_in) {
 74 | 		printf("Could not read content in status register\n");
 75 | 	}
 76 | 
 77 | }
 78 | 
 79 | void WriteToCommandReg( unsigned short CommandID, unsigned short  CommandDataLength, unsigned short CommandType) {
 80 | 
 81 | 	// Write data to data register if it is a set command
 82 | 	if(CommandType==1){
 83 | 		WriteToDataReg(CommandDataLength);
 84 | 	}
 85 | 
 86 | 	// Write to Data length register
 87 | 	WriteToDatalenghtReg(CommandDataLength);
 88 | 
 89 | 	// Write command into master output buffer
 90 | 	buf_out[0] = 0x00;
 91 | 	buf_out[1] = 0x04;
 92 | 	buf_out[2]=(CommandID>>8)&0xFF;
 93 | 	buf_out[3]=CommandID&0xFF;
 94 | 
 95 | 	// Write command into command register
 96 | 	ak = write(file, buf_out, 4);
 97 | 	if (ak != 4) {
 98 | 		printf("Error writing %i bytes\n", 4);
 99 | 	}
100 | 	// Wait until execution of command is accomplished
101 | 	WaitingTillAccomplished();
102 | 
103 | 	return;
104 | 
105 | }
106 | 
107 | void WriteToDataReg(unsigned short n_words){
108 | 
109 | 	// Write n_words data into master output buffer
110 | 		buf_out[0] = 0x00;
111 | 		buf_out[1] = 0x08;
112 | 
113 | 		for(int j=0;j<n_words*2;j++){
114 | 					buf_out[j+2]=data_out[j];
115 | 					}
116 | 
117 | 		// Write data into data register
118 | 		ak = write(file, buf_out, 4);
119 | 		if (ak != 4) {
120 | 			printf("Error writing %i bytes\n", 4);
121 | 		}
122 | 		// Wait until execution of command is accomplished
123 | 		WaitingTillAccomplished();
124 | 		return;
125 | }
126 | 
127 | void WriteToDatalenghtReg(unsigned short DataLength) {
128 | 
129 | 	// Write address and data length into master output buffer
130 | 	buf_out[0] = 0x00;
131 | 	buf_out[1] = 0x06;
132 | 	buf_out[2] = (DataLength>>8)&0xFF;;
133 | 	buf_out[3] = DataLength&0xFF;
134 | 
135 | 
136 | 	// Write data length into data length register
137 | 	ak = write(file, buf_out, 4);
138 | 
139 | 	if (ak != 4) {
140 | 		printf("Error writing %i bytes to data length register\n", 4);
141 | 	}
142 | 	return;
143 | 
144 | }
145 | 
146 | 
147 | void ReadDataReg(unsigned short n_read){
148 | 
149 | 	// Write address of data register into master output buffer
150 | 	buf_out[0]=0x00;
151 | 	buf_out[1]=0x08;
152 | 
153 | 	// reads n_read bytes from data register and saves it in master input buffer
154 | 	i2c_read(n_read,2);
155 | 
156 | 	return;
157 | }
158 | 
159 | void ReadOut(unsigned short n_read) {
160 |  short output_int=0;
161 | 
162 | 	// Reads out n_out bytes content of master input buffer in hexadecimal format
163 | 	printf("Master Input buffer\n");
164 | 	printf("*******************\n");
165 | 	for (unsigned short i = 0; i < n_read; i++) {
166 | 
167 | 		printf("%d. byte:0x%0*x\n", i + 1, 2, buf_in[i]);
168 | 	}
169 | 	printf("Total in hexadecimal format: 0x");
170 | 	for (unsigned short i = 0; i < n_read; i++) {
171 | 
172 | 
173 | 		}
174 | 	printf("\n");
175 | 	for (unsigned short i = 0; i < n_read/2; i++) {
176 | 		for (unsigned short j=0;j<2;j++){
177 | output_int=(((short)buf_in[i+j])<<8*(1-j))+output_int;
178 | 
179 | 		}
180 | 		printf("%d. word: %d\n",i+1,output_int);
181 | 		output_int=0;
182 | 			}
183 | 		printf("\n\n");
184 | 	return;
185 | }
186 | 
187 | 
188 | int ReadI2CCommand(){
189 | 	int temp;
190 | 	// Read in input command
191 | 	 printf("Please enter command in hexadecimal format : ");
192 | 	  scanf("%x", &temp);
193 | 
194 | 	 // Check whether command is valid
195 | 	int i;
196 | 	int j;
197 | 	for(i=0;i<n_commands;i++){
198 | 	if( commandList[i][0]==(unsigned short)temp){
199 | 		break;
200 | 		}
201 | 	}
202 | 
203 | if (i==n_commands){
204 | 	printf("Unknown command.\n");
205 | 	exit(EXIT_FAILURE);
206 | 		}
207 | 
208 | else{
209 | 
210 | 	printf("Chosen command\n");
211 | 	printf("***************\n");
212 | 	printf("CommandID=0x%0*x\n",4,commandList[i][0]);
213 | 	printf("Number of words=%d\n",commandList[i][1]);
214 | 
215 | 	// Load set data
216 | 		if (commandList[i][2]==1){
217 | 			for(j=0;j<commandList[i][1];j++){
218 | 			printf("Please enter data of %d. word to be transferred : ",j+1);
219 | 			scanf("%d", &temp);
220 | 			data_out[j*2]=(((unsigned short)temp)>>8)&0xFF;
221 | 			data_out[j*2+1]=((unsigned short)temp)&0xFF;
222 | 			}
223 | 		}
224 | 		printf("\n");
225 | 	return i;
226 | 	}
227 | }
228 | 
229 | 
230 | 
231 | void WaitingTillAccomplished(){
232 | 
233 | // Checks BUSY bit of status register and returns when cleared
234 | 
235 | 	buf_out[0]=0x00;
236 | 	buf_out[1]=0x02;
237 | 	i2c_read(2,2);
238 | 
239 | 	while(buf_in[1] & 1)	{
240 | 					printf("Waiting for command to be accomplished\n");
241 | 					i2c_read(2,2);
242 | 					}
243 | 
244 | 	// Sanity check
245 | 	printf("Status register\n");
246 | 	printf("***************\n");
247 | 	printf("Error Code: %d\n", (int)buf_in[0]);
248 | 	printf("second byte:0x%0*x\n", 2, buf_in[1]);
249 | 	printf("\n");
250 | 	return;
251 | }
252 | 
253 | 
254 | 


--------------------------------------------------------------------------------
/lepton_camera/src/video/lepton.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  Copyright (c) 2014, Pure Engineering LLC
  3 |  All rights reserved.
  4 | 
  5 |  Redistribution and use in source and binary forms, with or without
  6 |  modification, are permitted provided that the following conditions are met:
  7 | 
  8 |  * Redistributions of source code must retain the above copyright notice, this
  9 |  list of conditions and the following disclaimer.
 10 | 
 11 |  * Redistributions in binary form must reproduce the above copyright notice,
 12 |  this list of conditions and the following disclaimer in the documentation
 13 |  and/or other materials provided with the distribution.
 14 | 
 15 |  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 16 |  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 17 |  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 18 |  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 19 |  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 20 |  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 21 |  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 22 |  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 23 |  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 24 |  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 25 |  */
 26 | #include <stdlib.h>
 27 | #include <stdint.h>
 28 | #include <unistd.h>
 29 | #include <stdio.h>
 30 | #include <stdlib.h>
 31 | #include <getopt.h>
 32 | #include <fcntl.h>
 33 | #include <sys/ioctl.h>
 34 | #include <linux/types.h>
 35 | #include <linux/spi/spidev.h>
 36 | #include <limits.h>
 37 | #include "ros/ros.h"
 38 | #include "ros/time.h"
 39 | #include "std_msgs/String.h"
 40 | #include "std_msgs/MultiArrayLayout.h"
 41 | #include "std_msgs/MultiArrayDimension.h"
 42 | #include "std_msgs/Int32MultiArray.h"
 43 | #include "sensor_msgs/Image.h"
 44 | #include "SPI.h"
 45 | 
 46 | #include <sstream>
 47 | #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
 48 | 
 49 | static void pabort(const char *s) {
 50 | 	perror(s);
 51 | 	abort();
 52 | }
 53 | 
 54 | #define VOSPI_FRAME_SIZE (164)
 55 | uint8_t lepton_frame_packet[VOSPI_FRAME_SIZE];
 56 | static unsigned int lepton_image[80][80] = { 0 };
 57 | ros::Time capture_time;
 58 | 
 59 | 
 60 | 
 61 | void resync() {
 62 | 	// This function tries to resynchronize the video stream
 63 | 
 64 | 	// Close SPI Port
 65 | 	SpiClosePort(0);
 66 | 
 67 | 	// Sleep for 0.75 second to trigger resynch.
 68 | 	printf("Synchronizing video stream...\n");
 69 | 	usleep(750000);
 70 | 
 71 | 	// Open Port again
 72 | 	SpiOpenPort(0);
 73 | 
 74 | 	return;
 75 | 
 76 | }
 77 | 
 78 | void reboot() {
 79 | 	// This functions reboots lepton camera
 80 | 
 81 | 	// Close SPI Port
 82 | 	SpiClosePort(0);
 83 | 
 84 | 	// Unplug power
 85 | 	printf("Unplug power for 40 ms\n");
 86 | 	system("gpio -g write 97 0");
 87 | 	usleep(40000);
 88 | 	system("gpio -g write 97 1");
 89 | 
 90 | 	// Open SPI Port again
 91 | 	SpiOpenPort(0);
 92 | 
 93 | 	return;
 94 | 
 95 | }
 96 | 
 97 | int transfer(int fd, int current_frame) {
 98 | 	int ret;
 99 | 	int i;
100 | 	int packet_number;
101 | 	uint8_t tx[VOSPI_FRAME_SIZE] = { 0, };
102 | 
103 | 
104 | 	// Take capture time of image corresponding to first package
105 | 	if(current_frame==0){
106 | 		 capture_time=ros::Time::now();
107 | 	}
108 | 
109 | 	struct spi_ioc_transfer tr;
110 | 
111 | 	tr.tx_buf = (unsigned long) tx;
112 | 	tr.rx_buf = (unsigned long) lepton_frame_packet;
113 | 	tr.len = VOSPI_FRAME_SIZE;
114 | 	tr.delay_usecs = spi_delay;
115 | 	tr.speed_hz = spi_speed;
116 | 	tr.bits_per_word = spi_bitsPerWord;
117 | 
118 | 	ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
119 | 
120 | 	if (ret < 1)
121 | 		pabort("can't send spi message");
122 | 
123 | 	if (((lepton_frame_packet[0] & 0xf) != 0x0f)) {
124 | 		packet_number = lepton_frame_packet[1];
125 | 
126 | 		//printf("%d", packet_number);
127 | 
128 | 		if (packet_number == current_frame) {
129 | 			for (i = 0; i < 80; i++) {
130 | 				lepton_image[packet_number][i] = (lepton_frame_packet[2 * i + 4]
131 | 						<< 8 | lepton_frame_packet[2 * i + 5]);
132 | 			}
133 | 		} else {
134 | 			packet_number = -1;
135 | 		}
136 | 	}
137 | 	return packet_number;
138 | }
139 | 
140 | sensor_msgs::Image normalize(sensor_msgs::Image I,
141 | 		unsigned int Data_raw[80][80]) {
142 | 
143 | 	int i;
144 | 	int j;
145 | 	unsigned int maxval = 0;
146 | 	unsigned int minval = UINT_MAX;
147 | 
148 | 	// Calculating extrema
149 | 	for (i = 0; i < 60; i++) {
150 | 		for (j = 0; j < 80; j++) {
151 | 			if (Data_raw[i][j] > maxval) {
152 | 				maxval = Data_raw[i][j];
153 | 			}
154 | 			if (Data_raw[i][j] < minval) {
155 | 				minval = Data_raw[i][j];
156 | 			}
157 | 		}
158 | 	}
159 | 	float diff = maxval - minval;
160 | 	if (diff == 0.0) {
161 | 		diff = 1.0;
162 | 	}
163 | 	float scale = 255 / diff;
164 | 
165 | 	// File array with normalized pixel points
166 | 	for (i = 0; i < 80; i++) {
167 | 		for (j = 0; j < 60; j++) {
168 | 			I.data[j * 80 + i] = (Data_raw[j][i] - minval) * scale;
169 | 		}
170 | 	}
171 | 
172 | 	return I;
173 | }
174 | 
175 | sensor_msgs::Image agc(sensor_msgs::Image I,
176 | 	unsigned int Data_raw[80][80]) {
177 | 	int i;
178 | 	int j;
179 | 	// File array with normal
180 | 	for (i = 0; i < 80; i++) {
181 | 		for (j = 0; j < 60; j++) {
182 | 			I.data[j * 80 + i] = Data_raw[j][i];
183 | 		}
184 | 	}
185 | 
186 | 	return I;
187 | }
188 | 
189 | 
190 | sensor_msgs::Image calib(sensor_msgs::Image I,
191 | 		unsigned int Data_raw[80][80], int threshold) {
192 | 
193 | 	int i;
194 | 	int j;
195 | 	unsigned int maxval = 0;
196 | 	unsigned int minval = UINT_MAX;
197 | 
198 | 	// Calculating extrema
199 | 	for (i = 0; i < 60; i++) {
200 | 		for (j = 0; j < 80; j++) {
201 | 			if (Data_raw[i][j] > maxval) {
202 | 				maxval = Data_raw[i][j];
203 | 			}
204 | 			if (Data_raw[i][j] < minval) {
205 | 				minval = Data_raw[i][j];
206 | 			}
207 | 		}
208 | 	}
209 | 	float diff = maxval - minval;
210 | 	if (diff == 0.0) {
211 | 		diff = 1.0;
212 | 	}
213 | 	float scale = 255 / diff;
214 | 
215 | 	// File array with thresholded pixel points
216 | 	for (i = 0; i < 80; i++) {
217 | 		for (j = 0; j < 60; j++) {
218 | 			I.data[j * 80 + i] = (Data_raw[j][i] - minval) * scale;
219 | 			if (I.data[j * 80 + i]<threshold){
220 | 				I.data[j * 80 + i]=0;
221 | 			}
222 | 			else{
223 | 				I.data[j * 80 + i]=255;
224 | 			}
225 | 
226 | 		}
227 | 	}
228 | 
229 | 	return I;
230 | }
231 | sensor_msgs::Image inversion(sensor_msgs::Image I,
232 | 		unsigned int Data_raw[80][80]) {
233 | 
234 | 	int i;
235 | 	int j;
236 | 	unsigned int maxval = 0;
237 | 	unsigned int minval = UINT_MAX;
238 | 
239 | 	// Calculating extrema
240 | 	for (i = 0; i < 60; i++) {
241 | 		for (j = 0; j < 80; j++) {
242 | 			if (Data_raw[i][j] > maxval) {
243 | 				maxval = Data_raw[i][j];
244 | 			}
245 | 			if (Data_raw[i][j] < minval) {
246 | 				minval = Data_raw[i][j];
247 | 			}
248 | 		}
249 | 	}
250 | 	float diff = maxval - minval;
251 | 	if (diff == 0.0) {
252 | 		diff = 1.0;
253 | 	}
254 | 	float scale = 255 / diff;
255 | 
256 | 	// File array with normalized inverted pixel points
257 | 	for (i = 0; i < 80; i++) {
258 | 		for (j = 0; j < 60; j++) {
259 | 			I.data[j * 80 + i] = 255 - (Data_raw[j][i] - minval) * scale;
260 | 		}
261 | 	}
262 | 
263 | 	return I;
264 | }
265 | 
266 | int AskFormat() {
267 | 	int Videoformat;
268 | 	printf("Please enter video format? (Press 0 to list different formats)\n");
269 | 	scanf("%d", &Videoformat);
270 | 	if (Videoformat == 0) {
271 | 		printf("Available formats\n");
272 | 		printf("*****************\n");
273 | 		printf("1: Linear normalization (1 Pixel=8 Bit)\n");
274 | 		printf("2: AGC mode (1 Pixel=8 Bit)\n");
275 | 		printf("3: Calibration mode (1 Pixel=8 Bit)\n");
276 | 		printf("4: Inversion mode (1 Pixel=8 Bit)\n");
277 | 		exit(0);
278 | 	}
279 | 	return Videoformat;
280 | 
281 | }
282 | 
283 | int AskThreshold() {
284 | 	int temp;
285 | 	printf("Please define threshold value [0,255]\n");
286 | 	scanf("%d", &temp);
287 | 	return temp;
288 | 
289 | }
290 | 
291 | 
292 | 
293 | 
294 | 
295 | int main(int argc, char *argv[]) {
296 | 
297 | 	// Ask user which mode video format shall be
298 | 	int format = AskFormat();
299 | 
300 | 	// Optionaly ask user which threshold should be used
301 | 	int c_threshold;
302 | 	if (format == 3) {
303 | 		c_threshold = AskThreshold();
304 | 	}
305 | 
306 | 	int i, j;
307 | 	int i_sync = 0;
308 | 	ros::init(argc, argv, "arrayPublisher");
309 | 	ros::NodeHandle n;
310 | 	ros::Publisher pub = n.advertise < std_msgs::Int32MultiArray
311 | 			> ("array", 100);
312 | 	ros::Publisher data = n.advertise < sensor_msgs::Image > ("IR_data", 100);
313 | 
314 | 	sensor_msgs::Image IR_data;
315 | 	IR_data.height = 60;
316 | 	IR_data.width = 80;
317 | 	IR_data.step = 80;
318 | 	IR_data.encoding = "mono8";
319 | 	system("gpio -g write 97 1");
320 | 	int ret = 0;
321 | 	int reset = 0;
322 | 	while (ros::ok()) {
323 | 
324 | 		IR_data.data.clear();
325 | 		for (j = 0; j < (int) IR_data.height; j++) {
326 | 			for (i = 0; i < (int) IR_data.width; i++) {
327 | 				IR_data.data.push_back(0);
328 | 			}
329 | 		}
330 | 		//open spi port
331 | 		SpiOpenPort(0);
332 | 
333 | 		// Retrieving frame data
334 | 		reset = 0;
335 | 		for (j = 0; j < 60; j++) {
336 | 			j = transfer(spi_cs0_fd, j); // j:=Packet number
337 | 
338 | 			if (j < 0 && j > -2) {
339 | 				reset += 1;
340 | 				usleep(1000);
341 | 
342 | 			} else if (j < -2) {
343 | 				j = -1;
344 | 			}
345 | 
346 | 			// Try to resynchronize video stream or reboot camera after 750 false packages
347 | 			// First try to resynchronize video stream.
348 | 			// After 3 failed attempts. -> Reboot camera
349 | 			if (reset == 750) {
350 | 
351 | 				if (i_sync < 3) {
352 | 					resync();
353 | 					i_sync++;
354 | 					reset = 0;
355 | 				} else {
356 | 					reboot();
357 | 					i_sync = 0;
358 | 					reset = 0;
359 | 
360 | 				}
361 | 			}
362 | 		}
363 | 
364 | 		printf("\n");
365 | 
366 | 		// Process raw data
367 | 		switch (format) {
368 | 		case 1: // linear normalization
369 | 			IR_data = normalize(IR_data, lepton_image);
370 | 			break;
371 | 		case 2: // AGC mode
372 | 			IR_data = agc(IR_data, lepton_image);
373 | 			break;
374 | 		case 3: // calib mode
375 | 					IR_data = calib(IR_data, lepton_image,c_threshold);
376 | 					break;
377 | 		case 4: // inversion mode
378 | 						IR_data = inversion(IR_data, lepton_image);
379 | 						break;
380 | 		default:
381 | 			printf("Unknown video data. Cannot process data\n");
382 | 			return 0;
383 | 
384 | 		}
385 | 
386 | 		// Publish data over ROS
387 | 
388 | 		IR_data.header.stamp.sec=capture_time.sec;
389 | 		IR_data.header.stamp.nsec=capture_time.nsec;
390 | 		data.publish(IR_data);
391 | 		ROS_INFO("Successfully published IR image!");
392 | 
393 | 		ros::spinOnce();
394 | 
395 | 		SpiClosePort(0);
396 | 		//usleep(37038);
397 | 	}
398 | 
399 | 	return ret;
400 | }
401 | 


--------------------------------------------------------------------------------