def roll(self):
#Read the accelerometer,gyroscope and magnetometer values
ACCx = self._bus.read_acc(_Axis.X)
ACCy = self._bus.read_acc(_Axis.Y)
ACCz = self._bus.read_acc(_Axis.Z)
#Normalize accelerometer raw values.
accXnorm = ACCx / math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
accYnorm = ACCy / math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
####################################################################
###################Calculate pitch and roll#########################
####################################################################
#Use these two lines when the IMU is up the right way. Skull logo is facing down
pitch = math.asin(accXnorm)
roll = -math.asin(accYnorm / math.cos(pitch))
#
#Us these four lines when the IMU is upside down. Skull logo is facing up
#accXnorm = -accXnorm #flip Xnorm as the IMU is upside down
#accYnorm = -accYnorm #flip Ynorm as the IMU is upside down
#pitch = math.asin(accXnorm)
#roll = math.asin(accYnorm/math.cos(pitch))
#
############################ END ##################################
return unit.Angle(roll, unit.Angle.RADIAN, unit.Angle.RELATIVE_RANGE)
def heading(self):
MAGx = self._bus.read_mag(_Axis.X)
MAGy = self._bus.read_mag(_Axis.Y)
####################################################################
############################MAG direction ##########################
####################################################################
#If IMU is upside down, then use this line. It isnt needed if the
# IMU is the correct way up
#MAGy = -MAGy
#
############################ END ##################################
#Calculate heading
heading = math.atan2(MAGy, MAGx)
return unit.Angle(heading, unit.Angle.RADIAN, unit.Angle.HEADING_RANGE)
def magnetic_variation(self):
if self._mag_var:
return unit.Angle(self._mag_var, unit.Angle.DEGREE,
unit.Angle.RELATIVE_RANGE)
else:
return None
def ground_course(self):
if self._truck:
return unit.Angle(self._track, unit.Angle.DEGREE,
unit.Angle.HEADING_RANGE)
else:
return None
def latitude(self):
if self._lat:
return unit.Angle(self._lat, unit.Angle.DEGREE,
unit.Angle.LATITUDE_RANGE)
else:
return None
def longitude(self):
if self._lon:
return unit.Angle(self._lon, unit.Angle.DEGREE,
unit.Angle.LONGITUDE_RANGE)
else:
return None
def heading(self):
return unit.Angle(self._sense.get_compass(), unit.Angle.DEGREE, unit.Angle.HEADING_RANGE)
def roll(self):
orientation = self._sense.get_orientation_degrees()
return unit.Angle(orientation['roll'], unit.Angle.DEGREE, unit.Angle.RELATIVE_RANGE)
