Я использую Sense_HAT с Raspberry Pi 4 и использую на нем Ubuntu 18.04. Я пытаюсь получить данные об ускорении по смыслу HAT, но продолжаю получать большие пики в моих данных, которые примерно одинаковой величины.
Обратите внимание, что синий график - это график необработанных показаний акселерометра по оси x в терминах g, и смысл HAT полностью неподвижен при получении этого образца.
Что это может быть причиной, и как мне это исправить?
Код и снимок экрана с данными графического акселерометра приведены ниже.
Снимок экрана
код:
from sense_hat import SenseHat
import time
import threading
from PyQt5 import QtWidgets, QtCore, uic
from pyqtgraph import PlotWidget, plot
import pyqtgraph as pg
import sys
import os
from math import sin, radians
import numpy as np
from math import sqrt
g = 9.81
filterCoefficient = 0.05
sense = SenseHat()
class MainWindow(QtWidgets.QMainWindow):
def __init__(self, *args, **kwargs):
super(MainWindow, self).__init__(*args, **kwargs)
# Declare variables
self.ax = 0
self.ay = 0
self.az = 0
self.adjustedA1 = np.array([0,0,0])
self.accel = 0
self.startTime = time.time()
self.time1 = time.time()
self.time2 = time.time()
self.timeBetweenSamples = 0
self.timeElapsed = 0
self.graphWidget = pg.PlotWidget()
self.setCentralWidget(self.graphWidget)
self.time_plot = []
self.accelAdjusted_plot = []
self.accelRaw_plot = []
self.accelFiltered_plot = []
print("check init")
for h in range (1000):
self.accelRaw_plot.append(0)
for i in range (1000):
self.accelAdjusted_plot.append(self.adjustedA1[0])
for k in range (1000):
self.accelFiltered_plot.append(self.ax)
for j in range (1000):
self.time_plot.append(self.timeElapsed)
self.calc_rotation_matrix()
self.threading()
self.graphWidget.setBackground('w') #set background colour, 'w' = white
pen1 = pg.mkPen(color=(255, 0, 0)) #set pen colour, red acceladjusted plot (R, G, B)
pen2 = pg.mkPen(color=(0, 255, 0)) #set pen colour, green accelFiltered plot
pen3 = pg.mkPen(color=(0, 0, 255)) #set pen colour, blue accelRaw plot
#invert = self.graphWidget.setYRange(-20, 20) #set Y range
self.data_line1 = self.graphWidget.plot (self.time_plot, self.accelAdjusted_plot, pen = pen1)
self.data_line2 = self.graphWidget.plot (self.time_plot, self.accelFiltered_plot, pen = pen2)
self.data_line3 = self.graphWidget.plot (self.time_plot, self.accelRaw_plot, pen = pen3)
self.timer = QtCore.QTimer()
self.timer.setInterval(50)
self.timer.timeout.connect(self.update_plot_data)
self.timer.start()
def update_plot_data(self):
print("check update")
self.time_plot = self.time_plot[1:] # Remove the first y element.
self.time_plot.append(self.timeElapsed) # Add a new value 1 higher than the last.
self.accelAdjusted_plot= self.accelAdjusted_plot[1:]
self.accelAdjusted_plot.append(self.adjustedA1[0]) # Add a new accelAdjusted value.
self.accelRaw_plot= self.accelRaw_plot[1:]
self.accelRaw_plot.append(self.accel['x']) # Add a new accelRaw value.
self.accelFiltered_plot= self.accelFiltered_plot[1:]
self.accelFiltered_plot.append(self.ax) # Add a new accelFiltered value.
self.data_line1.setData(self.time_plot, self.accelAdjusted_plot) # Update the data_line.
self.data_line2.setData(self.time_plot, self.accelFiltered_plot)
self.data_line3.setData(self.time_plot, self.accelRaw_plot)
def threading(self):
print("check thread")
self.t1 = threading.Thread(target = self.calc_accel)
self.t1.daemon =True
self.t1.start()
def calc_rotation_matrix(self):
print("check rotation matrix")
for i in range(100):
self.accelraw = sense.get_accelerometer_raw()
self.ax += self.accelraw["x"] * g
self.ay += self.accelraw["y"] * g
self.az += self.accelraw["z"] * g
self.ax = self.ax/100
self.ay = self.ay/100
self.az = self.az/100
self.A = np.array([self.ax, self.ay, self.az]) # Accelerometer reading vector (A vector)
self.AMag = sqrt(self.ax**2 + self.ay**2 + self.az**2) # Scalar value of A vector
self.ANormalized = ([self.ax/self.AMag, self.ay/self.AMag, self.az/self.AMag]) # Normalized A vector
self.unitax = self.ANormalized[0]
self.unitay = self.ANormalized[1]
self.unitaz = self.ANormalized[2]
self.repdom = self.unitax**2 + self.unitay**2 # Reapeating denominator to obtain rotationMatrix
self.rotationMatrix = np.array([[(self.unitay**2 - (self.unitax**2)*self.unitaz)/self.repdom, (-self.unitax*self.unitay - self.unitax*self.unitay*self.unitaz)/self.repdom, self.unitax], [(-self.unitax*self.unitay - self.unitax*self.unitay*self.unitaz)/self.repdom, (self.unitax**2 - (self.unitay**2)*self.unitaz)/self.repdom, self.unitay], [-self.unitax, -self.unitay, -self.unitaz]])
self.corrected = np.dot(self.rotationMatrix, self.A)
print(self.A)
#print(self.angle)
print(self.rotationMatrix)
print(self.corrected)
def calc_accel(self):
while True:
self.accel = sense.get_accelerometer_raw()
self.ax -= filterCoefficient*(self.ax -self.accel["x"])
self.ay -= filterCoefficient*(self.ay -self.accel["y"])
self.az -= filterCoefficient*(self.az -self.accel["z"])
self.A1 = np.array([self.ax, self.ay, self.az])
self.adjustedA1 = np.dot(self.rotationMatrix, self.A1)
self.timeElapsed = time.time()- self.startTime
#print(self.accel)
#print(self.A1)
print("time between: " + str(self.timeBetweenSamples))
print("accel: " + str(self.accel))
if __name__ == "__main__":
app = QtWidgets.QApplication(sys.argv)
w = MainWindow()
w.show()
sys.exit(app.exec_())