def getData():
#connect with IMU
imu = lib.lsm9ds1_create()
lib.lsm9ds1_begin(imu)
if lib.lsm9ds1_begin(imu) == 0:
print("Failed to communicate with LSM9DS1.")
quit()
lib.lsm9ds1_calibrate(imu)
while True:
#get IMU accel data
global lasttime
#new structure testing
if lib.lsm9ds1_accelAvailable(imu) > 0 and ser.in_waiting > 8:
lib.lsm9ds1_readAccel(imu)
ax = lib.lsm9ds1_getAccelX(imu)
ay = lib.lsm9ds1_getAccelY(imu)
cax = lib.lsm9ds1_calcAccel(imu, ax)
cay = lib.lsm9ds1_calcAccel(imu, ay)
#TFmini data
recv = ser.read(9)
ser.reset_input_buffer()
if recv[0] == 0x59 and recv[1] == 0x59:
distance = recv[2] + recv[3] * 256
ser.reset_input_buffer()
currentTime = str(datetime.datetime.now())
#timestamp data
row = [
currentTime, distance, cax, cay
] #the row being written to csv file, just x and y accel
diff = time.time() - lasttime
print(diff)
lasttime = time.time()
print(row)
def getData():
global lasttime
#connect with IMU
imu = lib.lsm9ds1_create()
lib.lsm9ds1_begin(imu)
if lib.lsm9ds1_begin(imu) == 0:
print("Failed to communicate with LSM9DS1.")
quit()
lib.lsm9ds1_calibrate(imu)
current = time.time()
while current - startTime < 10:
current = time.time()
if lib.lsm9ds1_accelAvailable(imu) > 0 and ser.in_waiting > 8:
lib.lsm9ds1_readAccel(imu)
ax = lib.lsm9ds1_getAccelX(imu)
ay = lib.lsm9ds1_getAccelY(imu)
az = lib.lsm9ds1_getAccelZ(imu)
cax = lib.lsm9ds1_calcAccel(imu, ax)
cay = lib.lsm9ds1_calcAccel(imu, ay)
caz = lib.lsm9ds1_calcAccel(imu, az)
gx = lib.lsm9ds1_getGyroX(imu)
gy = lib.lsm9ds1_getGyroY(imu)
gz = lib.lsm9ds1_getGyroZ(imu)
cgx = lib.lsm9ds1_calcGyro(imu, gx)
cgy = lib.lsm9ds1_calcGyro(imu, gy)
cgz = lib.lsm9ds1_calcGyro(imu, gz)
#TFmini data
recv = ser.read(9)
ser.reset_input_buffer()
if recv[0] == 0x59 and recv[1] == 0x59:
distance = recv[2] + recv[3] * 256
ser.reset_input_buffer()
currentTime = str(datetime.datetime.now()); #timestamp data
row = [currentTime,distance,cax,cay,caz,cgx,cgy,cgz] #the row being written to csv file, just x and y accel
rowList.append(row)
diff = time.time() - lasttime
timeDiffer.append(diff)
#print(diff)
lasttime = time.time()
#print(row)
print("start writing data")
with open(datafile,"a",newline = '') as csvfile:
spamwriter = csv.writer(csvfile, delimiter=',', quoting=csv.QUOTE_MINIMAL)
for i in range(len(rowList)):
row = [rowList[i]]
spamwriter.writerow(row)
print("start writing frequency")
with open(filename,"a",newline = '') as csvfile:
spamwriter = csv.writer(csvfile, delimiter=',', quoting=csv.QUOTE_MINIMAL)
for i in range(len(timeDiffer)):
row = [timeDiffer[i]]
spamwriter.writerow(row)
def getData():
global lasttime
#connect with IMU
imu = lib.lsm9ds1_create()
lib.lsm9ds1_begin(imu)
if lib.lsm9ds1_begin(imu) == 0:
print("Failed to communicate with LSM9DS1.")
quit()
lib.lsm9ds1_calibrate(imu)
while True:
#beforeIf = time.time()
time.sleep(0.2)
if lib.lsm9ds1_accelAvailable(imu) > 0:
afterIf = time.time()
lib.lsm9ds1_readAccel(imu)
ax = lib.lsm9ds1_getAccelX(imu)
ay = lib.lsm9ds1_getAccelY(imu)
az = lib.lsm9ds1_getAccelZ(imu)
cax = lib.lsm9ds1_calcAccel(imu, ax)
cay = lib.lsm9ds1_calcAccel(imu, ay)
caz = lib.lsm9ds1_calcAccel(imu, az)
gx = lib.lsm9ds1_getGyroX(imu)
gy = lib.lsm9ds1_getGyroY(imu)
gz = lib.lsm9ds1_getGyroZ(imu)
cgx = lib.lsm9ds1_calcGyro(imu, gx)
cgy = lib.lsm9ds1_calcGyro(imu, gy)
cgz = lib.lsm9ds1_calcGyro(imu, gz)
## #add to list
## caxl.append(cax)
## cayl.append(cay)
## cazl.append(caz)
## cgxl.append(cgx)
## cgyl.append(cgy)
## cgzl.append(cgz)
print(cax, cay, caz, cgx, cgy, cgz)
with picamera.PiCamera(resolution=(480, 480), framerate=40) as camera:
camera.start_preview()
# Give the camera some warm-up time
time.sleep(3)
camera.capture_sequence(filenames(), use_video_port=True)
return 1
#IMU setup
imu = lib.lsm9ds1_create()
lib.lsm9ds1_begin(imu)
#main, which parallelizes sensors and camera reading
if __name__ == '__main__':
if lib.lsm9ds1_begin(imu) == 0:
print("Failed to communicate with LSM9DS1.")
quit()
lib.lsm9ds1_calibrate(imu)
if ser.is_open == False:
ser.open()
start = time.time()
lasttime = time.time()
pool = Pool()
sensors = pool.apply_async(getData)
camera = pool.apply_async(capture)
#sensorAnswer = sensors.get()
cameraAnswer = camera.get()
end = time.time()
print("total time is %f" % (end - start))
def calibrate(self):
"""
A wrapper for IMU calibration
"""
lib.lsm9ds1_calibrate(self.__conn)
def getSensorAndCamera(host='192.168.1.121',
port=6000,
save=False,
duration=5,
endless=False,
trAccRate=6,
trGyroRate=6,
trMagRate=7,
accScale=2,
gyroScale=245,
magScale=4,
cameraFreq=5,
imuRate=50,
lidarRate=50,
precision=0.001):
beginTime = str(datetime.datetime.now())
os.makedirs(beginTime + "/camera")
datafile = beginTime + '/Lidar_IMU_Data.csv'
rowList = []
if endless:
duration = 1000
lidarRate = float(1) / lidarRate - 0.0007
imuRate = float(1) / imuRate - 0.0007
cameraFreq = float(1) / cameraFreq
setIMUScale(accScale, gyroScale, magScale)
setIMUodr(trAccRate, trGyroRate, trMagRate)
if lib.lsm9ds1_begin(imu) == 0:
print("Failed to communicate with LSM9DS1.")
quit()
lib.lsm9ds1_calibrate(imu)
try:
if ser.is_open == False:
ser.open()
print(time.time())
alive = Event()
#multicore process
pic = Process(target=capture,
args=(
alive,
duration,
cameraFreq,
beginTime,
))
sensor = Process(target=getData,
args=(
alive,
duration,
precision,
imuRate,
lidarRate,
datafile,
rowList,
))
pic.start()
sensor.start()
pic.join()
sensor.join()
#subprocess.Popen("python3 socket_folder_server.py localhost 60004 \""+beginTime+"\"",shell=True)
except KeyboardInterrupt: # Ctrl+C
if ser != None:
ser.close()
alive.set()
pic.join()
sensor.join()
print(time.time())
send(host, port, beginTime)
if not save:
os.system("rm -r \"" + beginTime + "\"")