Let’s say you have a 9DOF imu that you want to use through an arduino to design a kalman filter. So you need to send data to your computer (PC/Linux) using serial command. I will develop an example here by sending 9 variables which are 3 axis gyroscope, 3 axis accelerometer and 3 axis magnetometer.
The sending of data through Arduino’s serial function occurs in units of bytes. So any double/integers needs to be converted in bytes before being sent.
So I will be very brief by showing you my code:
sendToPC(double* data1, double* data2, double* data3,
double* data4, double* data5, double* data6,
double* data7, double* data8, double* data9){
byte* byteData1 = (byte*)(data1);
byte* byteData2 = (byte*)(data2);
byte* byteData3 = (byte*)(data3);
byte* byteData4 = (byte*)(data4);
byte* byteData5 = (byte*)(data5);
byte* byteData6 = (byte*)(data6);
byte* byteData7 = (byte*)(data7);
byte* byteData8 = (byte*)(data8);
byte* byteData9 = (byte*)(data9);
byte buf[36] = {byteData1[0], byteData1[1], byteData1[2], byteData1[3],
byteData2[0], byteData2[1], byteData2[2], byteData2[3],
byteData3[0], byteData3[1], byteData3[2], byteData3[3],
byteData4[0], byteData4[1], byteData4[2], byteData4[3],
byteData5[0], byteData5[1], byteData5[2], byteData5[3],
byteData6[0], byteData6[1], byteData6[2], byteData6[3],
byteData7[0], byteData7[1], byteData7[2], byteData7[3],
byteData8[0], byteData8[1], byteData8[2], byteData8[3],
byteData9[0], byteData9[1], byteData9[2], byteData9[3]};
Serial.write(buf, 36);
}
If you are wondering why 36… double in arduino takes 4 bytes, so 9×4=36
Here is the main arduino code with the setup function and the loop function:
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
}
void loop() {
// read the sensor
timeSync(loopTime);
IMU.readSensor();
// display the data
int gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z,mag_x,mag_y,mag_z;
gyro_x = IMU.getGyroX_rads();
gyro_y = IMU.getGyroY_rads();
gyro_z = IMU.getGyroZ_rads();
accel_x = IMU.getAccelX_mss();
accel_y = IMU.getAccelY_mss();
accel_z = IMU.getAccelZ_mss();
mag_x = IMU.getMagX_uT();
mag_y = IMU.getMagY_uT();
mag_z = IMU.getMagZ_uT();
sendToPC(&gyro_x, &gyro_y, &gyro_z,
&accel_x, &accel_y, &accel_z,
&mag_x, &mag_y, &mag_z);
}
so, how do we retrieve these data from computer side, let’s analyse this python code snippet together:
class ProjectionViewer:
def run(self, sensorInstance)
try:
running = True
while running:
data = sensorInstance.getSerialData()
imu = np.array([[data[0],data[1],data[2]],[data[3],data[4],data[5]],[data[6],data[7],data[7]]])
print(imu)
except KeyboardInterrupt:
sensorInstance.close()
print('interrupted!')
sys.exit(0)
portName = 'COM30'
baudRate = 115200
dataNumBytes = 2
numParams = 9
s = rs.SerialRead(portName, baudRate, dataNumBytes, numParams)
s.readSerialStart()
pv = ProjectionViewer()
pv.run(s)
This is a bit of an advanced code but I hope you will find it easy, the most important line is 7, where the get.SerialData() is called from the SerialRead class described below:
class SerialRead:
def __init__(self, serialPort='/dev/ttyUSB0', serialBaud=38400, dataNumBytes=2, numParams=1):
self.port = serialPort
self.baud = serialBaud
self.dataNumBytes = dataNumBytes
self.numParams = numParams
self.rawData = bytearray(numParams * dataNumBytes)
self.dataType = None
if dataNumBytes == 2:
self.dataType = 'h' # 2 byte integer
elif dataNumBytes == 4:
self.dataType = 'f' # 4 byte float
self.data = np.zeros(numParams)
self.isRun = True
self.isReceiving = False
self.thread = None
# self.csvData = []
print('Trying to connect to: ' + str(serialPort) + ' at ' + str(serialBaud) + ' BAUD.')
try:
self.serialConnection = serial.Serial(serialPort, serialBaud, timeout=4)
print('Connected to ' + str(serialPort) + ' at ' + str(serialBaud) + ' BAUD.')
except:
print("Failed to connect with " + str(serialPort) + ' at ' + str(serialBaud) + ' BAUD.')
exit()
def readSerialStart(self):
if self.thread == None:
self.thread = Thread(target=self.backgroundThread)
self.thread.start()
# Block till we start receiving values
while self.isReceiving != True:
time.sleep(0.1)
def getSerialData(self):
privateData = self.rawData[:]
for i in range(self.numParams):
data = privateData[(i*self.dataNumBytes):(self.dataNumBytes + i*self.dataNumBytes)]
value, = struct.unpack(self.dataType, data)
if i == 0:
value = ((value * 0.00875) - 3.30) / 180.0 * np.pi
elif i == 1:
value = ((value * 0.00875) + 5.92) / 180.0 * np.pi
elif i == 2:
value = ((value * 0.00875) + 3.34) / 180.0 * np.pi
elif i == 3:
value = (value * 0.061) - 65
elif i == 4:
value = (value * 0.061) + 14.66
elif i == 5:
value = (value * 0.061) + 962.57
elif i == 6:
value = value * 0.080
elif i == 7:
value = value * 0.080
elif i == 8:
value = value * 0.080
self.data[i] = value
return self.data
The SerialRead class incorporates many useful functions, actually the getSerialData() runs as a background thread, different function are there to close the serial connection, start reading, and cancelling the thread.
Have a look at my repo here, with the complete working code for both the arduino and the computer/raspberry pi
