def refreshState(self):
with self._packetLock:
packet = deepcopy(self._packet)
q = self._imu.dmpGetQuaternion(packet)
g = self._imu.dmpGetGravity(q)
ypr = self._imu.dmpGetYawPitchRoll(q, g)
previousAngles = self._angles
self._angles = [ypr["pitch"], ypr["roll"], ypr["yaw"]]
previousTime = self._readTime
self._readTime = time.time()
dt = self._readTime - previousTime
for index in range(3):
self._angleSpeeds[index] = (
self._angles[index] -
previousAngles[index]) / dt #self._imu.dmpGetGyro(packet)
accelRaw = self._imu.dmpGetAccel(packet)
linearAccel = self._imu.dmpGetLinearAccel(accelRaw, g)
self._accels = Vector.rotateVector3D(linearAccel, self._angles)
def _plotHeight(self, x, y, angle, canvas, lines):
cosine = cos(angle)
sine = sin(angle)
vectors = [Vector.rotateVector(vector, sine, cosine) \
for vector in [[-Display.ARM_LENGTH, 0], \
[Display.ARM_LENGTH, 0]] ]
for index in range(2):
canvas.coords(lines[index], x, y, x+vectors[index][0], y+vectors[index][1])
def readAccels(self):
accelX = self._readRawAccelFilteredX() * Imu3000Emulated.ACCEL2MS2
accelY = self._readRawAccelFilteredY() * Imu3000Emulated.ACCEL2MS2
accelZ = self._readRawAccelFilteredZ() * Imu3000Emulated.ACCEL2MS2
#TODO Avoid multiple angle reading within PID loop
angles = [math.radians(angle) for angle in self.readAngles()]
anglesX = [angles[0], Imu3000Emulated.PI2 + angles[1]]
anglesY = [angles[0] - Imu3000Emulated.PI2, angles[1]]
anglesZ = angles
descX = Vector._descomposeVector(accelX, anglesX)
descY = Vector._descomposeVector(accelY, anglesY)
descZ = Vector._descomposeVector(accelZ, anglesZ)
accels = [descX[0] + descY[0] + descZ[0], \
descX[1] + descY[1] + descZ[1], \
descX[2] + descY[2] + descZ[2]]
return accels
def _update(self):
effectiveThrottle = self._throttle - Propeller.THROTTLE_THRESHOLD \
if self._throttle > Propeller.THROTTLE_THRESHOLD else 0.0
newThrustModule = effectiveThrottle * self._throttleThrustRate * Propeller.GRAVITY
self._thrustModule += self._lowPassFilter * (newThrustModule - self._thrustModule)
self._thrust = Vector.rotateVector3D([0.0, 0.0, self._thrustModule], self._angles)
self._thrust[2] -= Propeller.GRAVITY * self._thrustedWeight
#Counter-rotation
self._counterRotation = self._thrustModule * self._counterRotationRate
self._drone.onPropellerUpdated()
def _readAccels(self):
accelX = self._readRawAccelX() * Imu6050.ACCEL2MS2
accelY = self._readRawAccelY() * Imu6050.ACCEL2MS2
accelZ = self._readRawAccelZ() * Imu6050.ACCEL2MS2
angles = [math.radians(angle) for angle in self.readAngles()]
accels = Vector.rotateVector3D([accelX, accelY, accelZ], angles + [0.0])
#Eliminate gravity acceleration
accels[2] -= self._localGravity
self._state.accels = accels
def _readAccels(self):
accelX = self._readRawAccelX() * Imu6050.ACCEL2MS2
accelY = self._readRawAccelY() * Imu6050.ACCEL2MS2
accelZ = self._readRawAccelZ() * Imu6050.ACCEL2MS2
angles = [math.radians(angle) for angle in self.readAngles()]
accels = Vector.rotateVector3D([accelX, accelY, accelZ],
angles + [0.0])
#Eliminate gravity acceleration
accels[2] -= self._localGravity
self._state.accels = accels
def calibrate(self):
'''
Calibrates sensor
'''
print "Calibrating accelerometer..."
self._accOffset = [0.0] * 3
i = 0
while i < 100:
self._accOffset[0] += self._readRawAccelX()
self._accOffset[1] += self._readRawAccelY()
self._accOffset[2] += self._readRawAccelZ()
time.sleep(0.02)
i += 1
for index in range(3):
self._accOffset[index] /= float(i)
#Calibrate gyro
print "Calibrating gyro..."
self._gyroOffset = [0.0] * 3
i = 0
while i < 100:
self._gyroOffset[0] += self._readRawGyroX()
self._gyroOffset[1] += self._readRawGyroY()
self._gyroOffset[2] += self._readRawGyroZ()
time.sleep(0.02)
i += 1
for index in range(3):
self._gyroOffset[index] /= float(i)
#Calculate sensor installation angles
self._accAnglesOffset[0] = self._previousAngles[0] = math.degrees(
math.atan2(self._accOffset[1], self._accOffset[2]))
self._accAnglesOffset[1] = self._previousAngles[1] = -math.degrees(
math.atan2(self._accOffset[0], self._accOffset[2]))
#Calculate local gravity
angles = [math.radians(angle) for angle in self._accAnglesOffset]
accels = [accel * Imu6050.ACCEL2MS2 for accel in self._accOffset]
self._localGravity = Vector.rotateVector3D(accels, angles + [0.0])[2]
def calibrate(self):
'''
Calibrates sensor
'''
print("Calibrating accelerometer...")
self._accOffset = [0.0]*3
i = 0
while i < 100:
self._accOffset[0] += self._readRawAccelX()
self._accOffset[1] += self._readRawAccelY()
self._accOffset[2] += self._readRawAccelZ()
time.sleep(0.01)
i+=1
for index in range(3):
self._accOffset[index] /= float(i)
#Calibrate gyro
print("Calibrating gyro...")
self._gyroOffset = [0.0]*3
i = 0
while i < 100:
self._gyroOffset[0] += self._readRawGyroX()
self._gyroOffset[1] += self._readRawGyroY()
self._gyroOffset[2] += self._readRawGyroZ()
time.sleep(0.01)
i += 1
for index in range(3):
self._gyroOffset[index] /= float(i)
#Calculate sensor installation angles
self._accAnglesOffset[0] = self._previousAngles[0] = math.degrees(math.atan2(self._accOffset[1], self._accOffset[2]))
self._accAnglesOffset[1] = self._previousAngles[1] = -math.degrees(math.atan2(self._accOffset[0], self._accOffset[2]))
#Calculate local gravity
angles = [math.radians(angle) for angle in self._accAnglesOffset]
accels = [accel * Sensor.ACCEL2MS2 for accel in self._accOffset]
self._localGravity = Vector.rotateVector3D(accels, angles + [0.0])[2]
def _plotXY(self, coord, angles):
x = int((coord[0]*Display.PIXELS_METER + 200.0) % 400.0)
y = int((100.0 - coord[1]*Display.PIXELS_METER) % 200.0)
sinz = sin(angles[2])
cosz = cos(angles[2])
lines = [Vector.rotateVector(line, sinz, cosz) \
for line in [ [0, Display.ARM_LENGTH], \
[Display.ARM_LENGTH, 0], \
[0, -Display.ARM_LENGTH], \
[-Display.ARM_LENGTH, 0]] ]
for index in range(4):
self._canvasXY.coords(self._linesXY[index], x, y, x+lines[index][0], y-lines[index][1])
def calibrate(self):
print("Calibrating...")
time.sleep(20)
self._imu.resetFIFO()
#Wait for next packet
time.sleep(0.05)
with self._packetLock:
packet = deepcopy(self._packet)
q = self._imu.dmpGetQuaternion(packet)
g = self._imu.dmpGetGravity(q)
if path.exists(Imu6050Dmp.CALIBRATION_FILE_PATH):
with open(Imu6050Dmp.CALIBRATION_FILE_PATH,
"r") as calibrationFile:
serializedCalibration = " ".join(calibrationFile.readlines())
calibrationFile.close()
self._angleOffset = json.loads(serializedCalibration)
else:
ypr = self._imu.dmpGetYawPitchRoll(q, g)
self._angleOffset = [ypr["pitch"], ypr["roll"], ypr["yaw"]]
#Save calibration
serializedCalibration = json.dumps(self._angleOffset)
with open(Imu6050Dmp.CALIBRATION_FILE_PATH,
"w+") as calibrationFile:
calibrationFile.write(serializedCalibration + "\n")
calibrationFile.close()
accelRaw = self._imu.dmpGetAccel(packet)
linearAccel = self._imu.dmpGetLinearAccel(accelRaw, g)
self._gravityOffset = Vector.rotateVector3D(linearAccel,
self._angleOffset)
def refreshState(self):
with self._packetLock:
packet = deepcopy(self._packet)
q = self._imu.dmpGetQuaternion(packet)
g = self._imu.dmpGetGravity(q)
ypr = self._imu.dmpGetYawPitchRoll(q, g)
previousAngles = self._angles
self._angles = [ypr["pitch"], ypr["roll"], ypr["yaw"]]
previousTime = self._readTime
self._readTime = time.time()
dt = self._readTime - previousTime
for index in range(3):
self._angleSpeeds[index] = (self._angles[index] - previousAngles[index]) / dt #self._imu.dmpGetGyro(packet)
accelRaw = self._imu.dmpGetAccel(packet)
linearAccel = self._imu.dmpGetLinearAccel(accelRaw, g)
self._accels = Vector.rotateVector3D(linearAccel, self._angles)
def calibrate(self):
print("Calibrating...")
time.sleep(20)
self._imu.resetFIFO()
#Wait for next packet
time.sleep(0.05)
with self._packetLock:
packet = deepcopy(self._packet)
q = self._imu.dmpGetQuaternion(packet)
g = self._imu.dmpGetGravity(q)
if path.exists(Imu6050Dmp.CALIBRATION_FILE_PATH):
with open(Imu6050Dmp.CALIBRATION_FILE_PATH, "r") as calibrationFile:
serializedCalibration = " ".join(calibrationFile.readlines())
calibrationFile.close()
self._angleOffset = json.loads(serializedCalibration)
else:
ypr = self._imu.dmpGetYawPitchRoll(q, g)
self._angleOffset = [ypr["pitch"], ypr["roll"], ypr["yaw"]]
#Save calibration
serializedCalibration = json.dumps(self._angleOffset)
with open(Imu6050Dmp.CALIBRATION_FILE_PATH, "w+") as calibrationFile:
calibrationFile.write(serializedCalibration + "\n")
calibrationFile.close()
accelRaw = self._imu.dmpGetAccel(packet)
linearAccel = self._imu.dmpGetLinearAccel(accelRaw, g)
self._gravityOffset = Vector.rotateVector3D(linearAccel, self._angleOffset)