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 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)