class SenseHatCSV:
ETX = [255, 0, 0]
OFF = [0, 0, 0] # off
BTX = [0, 255, 0] # on
led_tx_on = [
BTX, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF
]
led_tx_off = [
ETX, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF
]
RADIANS = "RADIANS"
ACCEL = "ACCEL"
GYRO = "GYRO"
def __init__(self):
self.sense = SenseHat()
self.sense.set_imu_config(True, True, True)
def get_record_from_radians(self):
imu_input = self.sense.get_orientation_radians()
data = {
'xRoll': imu_input['pitch'],
'yPitch': imu_input['roll'],
'zYaw': imu_input['yaw'],
'timestamp': round(time.time() * 1000),
}
return data
class SenseClient(object):
def __init__(self):
self.sense = SenseHat()
self.sense.clear()
self.sense.set_imu_config(True, True, True)
def getSensePoints(self, imperial_or_metric, bucket):
dt = datetime.now(tz=pytz.timezone('US/Pacific')).isoformat()
point = Point(measurement_name="sense")
point.time(time=dt)
# % relative
point.field("humidity", self.sense.get_humidity())
if imperial_or_metric == "imperial":
point.field(
"temperature_from_humidity",
convertCToF(self.sense.get_temperature_from_humidity()))
point.field(
"temperature_from_pressure",
convertCToF(self.sense.get_temperature_from_pressure()))
point.field("pressure", convertmbToPSI(self.sense.get_pressure()))
else:
point.field("temperature_from_humidity",
self.sense.get_temperature_from_humidity())
point.field("temperature_from_pressure",
self.sense.get_temperature_from_pressure())
point.field("pressure", self.sense.get_pressure())
point.field("orientation_radians",
self.sense.get_orientation_radians())
point.field("orientation_degress",
self.sense.get_orientation_degrees())
# magnetic intensity in microteslas
point.field("compass_raw", self.sense.get_compass_raw())
# rotational intensity in radians per second
point.field("gyroscope_raw", self.sense.get_gyroscope_raw())
# acceleration intensity in Gs
point.field("accelerometer_raw", self.sense.get_accelerometer_raw())
return [{"bucket": bucket, "point": point}]
xi = trunc(x)
xf = x- xi
yi = trunc(y)
yf = y - yi
r = 1.0 - sqrt (xf*xf+yf*yf) / sqrt(2)
sh.set_pixel (xi, yi, int(red[0] * r), int(red[1] * r), int(red[2] * r))
r = 1.0 - sqrt (xf*xf+(1.0-yf)*(1.0-yf)) / sqrt(2)
sh.set_pixel (xi, yi+1, int(red[0] * r), int(red[1] * r), int(red[2] * r))
r = 1.0 - sqrt ((1.0-xf)*(1.0-xf)+yf*yf) / sqrt(2)
sh.set_pixel (xi+1, yi, int(red[0] * r), int(red[1] * r), int(red[2] * r))
r = 1.0 - sqrt ((1.0-xf)*(1.0-xf)+(1.0-yf)*(1.0-yf)) / sqrt(2)
sh.set_pixel (xi+1, yi+1, int(red[0] * r), int(red[1] * r), int(red[2] * r))
while 1:
orad = sh.get_orientation_radians()
# print("p: {pitch}, r: {roll}, y: {yaw}".format(**orad))
p = orad['pitch']
r = orad['roll']
y = orad['yaw']
drawDot(-p*5.0 + 3.5, r*5.0 + 3.5)
# print (r)
# sleep(1)
#for a in range (0, 20):
# sh.set_pixel (a, a, red)
# sleep(1)
# sh.clear()
# drawDot (float(a+0.5),float(a+0.5))
# drawDot (a*0.1 + 3, a*0.15 + 3)
# sleep(1)
def start(self):
self.enable = True
self.imuPub = rospy.Publisher(self.robot_host + '/imu',
Imu,
queue_size=10)
self.imuRawPub = rospy.Publisher(self.robot_host + '/imu/raw',
Imu,
queue_size=10)
self.accelerometerPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer',
Accelerometer,
queue_size=10)
self.accelerometerPitchPub = rospy.Publisher(
self.robot_host + '/imu/accelerometer/pitch', Pitch, queue_size=10)
self.accelerometerRollPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/roll',
Roll,
queue_size=10)
self.accelerometerYawPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/yaw',
Yaw,
queue_size=10)
self.accelerometerRawPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw',
Accelerometer,
queue_size=10)
self.accelerometerRawXPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw/x',
Float64,
queue_size=10)
self.accelerometerRawYPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw/y',
Float64,
queue_size=10)
self.accelerometerRawZPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw/z',
Float64,
queue_size=10)
self.gyroscopePub = rospy.Publisher(self.robot_host + '/imu/gyroscope',
Gyroscope,
queue_size=10)
self.gyroscopePitchPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/pitch',
Pitch,
queue_size=10)
self.gyroscopeRollPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/roll',
Roll,
queue_size=10)
self.gyroscopeYawPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/yaw',
Yaw,
queue_size=10)
self.gyroscopeRawPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw',
Gyroscope,
queue_size=10)
self.gyroscopeRawXPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw/x',
Float64,
queue_size=10)
self.gyroscopeRawYPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw/y',
Float64,
queue_size=10)
self.gyroscopeRawZPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw/z',
Float64,
queue_size=10)
self.magnetometerPub = rospy.Publisher(self.robot_host +
'/imu/magnetometer',
Magnetometer,
queue_size=10)
self.magnetometerRawPub = rospy.Publisher(self.robot_host +
'/imu/magnetometer/raw',
Orientation,
queue_size=10)
self.orientationPub = rospy.Publisher(self.robot_host +
'/imu/orientation',
Orientation,
queue_size=10)
self.orientationDegreePub = rospy.Publisher(self.robot_host +
'/imu/orientation/degrees',
Orientation,
queue_size=10)
self.orientationRadiansPub = rospy.Publisher(
self.robot_host + '/imu/orientation/radians',
Orientation,
queue_size=10)
self.orientationNorthPub = rospy.Publisher(self.robot_host +
'/imu/orientation/north',
Magnetometer,
queue_size=10)
sense = SenseHat()
while not rospy.is_shutdown():
accel_only = sense.get_accelerometer()
accel_raw = sense.get_accelerometer_raw()
gyro_only = sense.get_gyroscope()
gyro_raw = sense.get_gyroscope_raw()
north = sense.get_compass()
compass = sense.get_compass_raw()
orientation = sense.get_orientation()
orientation_deg = sense.get_orientation_degrees()
orientation_rad = sense.get_orientation_radians()
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = "/base_link"
imu_msg.linear_acceleration.x = accel_only['pitch']
imu_msg.linear_acceleration.y = accel_only['roll']
imu_msg.linear_acceleration.z = accel_only['yaw']
imu_msg.angular_velocity.x = gyro_only['pitch']
imu_msg.angular_velocity.y = gyro_only['roll']
imu_msg.angular_velocity.z = gyro_only['yaw']
imu_msg.orientation.x = orientation['pitch']
imu_msg.orientation.y = orientation['roll']
imu_msg.orientation.z = orientation['yaw']
imu_msg.orientation.w = 0
imu_msg.orientation_covariance = [
99999.9, 0.0, 0.0, 0.0, 99999.9, 0.0, 0.0, 0.0, 99999.9
]
imu_msg.angular_velocity_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_msg.linear_acceleration_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_raw_msg = Imu()
imu_raw_msg.header.stamp = rospy.Time.now()
imu_raw_msg.header.frame_id = "/base_link"
imu_raw_msg.linear_acceleration.x = accel_raw['x']
imu_raw_msg.linear_acceleration.y = accel_raw['y']
imu_raw_msg.linear_acceleration.z = accel_raw['z']
imu_raw_msg.angular_velocity.x = gyro_raw['x']
imu_raw_msg.angular_velocity.y = gyro_raw['y']
imu_raw_msg.angular_velocity.z = gyro_raw['z']
imu_raw_msg.orientation.x = compass['x']
imu_raw_msg.orientation.y = compass['y']
imu_raw_msg.orientation.z = compass['z']
imu_raw_msg.orientation.w = north
imu_raw_msg.orientation_covariance = [
99999.9, 0.0, 0.0, 0.0, 99999.9, 0.0, 0.0, 0.0, 99999.9
]
imu_raw_msg.angular_velocity_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_raw_msg.linear_acceleration_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
accel_msg = Accelerometer()
accel_msg.header.stamp = rospy.Time.now()
accel_msg.header.frame_id = "/base_link"
accel_msg.x = accel_only['pitch']
accel_msg.y = accel_only['roll']
accel_msg.z = accel_only['yaw']
accel_pitch_msg = Pitch()
accel_pitch_msg.header.stamp = rospy.Time.now()
accel_pitch_msg.header.frame_id = "/base_link"
accel_pitch_msg.data = accel_only['pitch']
accel_roll_msg = Roll()
accel_roll_msg.header.stamp = rospy.Time.now()
accel_roll_msg.header.frame_id = "/base_link"
accel_roll_msg.data = accel_only['roll']
accel_yaw_msg = Yaw()
accel_yaw_msg.header.stamp = rospy.Time.now()
accel_yaw_msg.header.frame_id = "/base_link"
accel_yaw_msg.data = accel_only['yaw']
accel_raw_msg = Accelerometer()
accel_raw_msg.header.stamp = rospy.Time.now()
accel_raw_msg.header.frame_id = "/base_link"
accel_raw_msg.x = accel_raw['x']
accel_raw_msg.y = accel_raw['y']
accel_raw_msg.z = accel_raw['z']
accel_raw_x_msg = Float64()
accel_raw_x_msg.header.stamp = rospy.Time.now()
accel_raw_x_msg.header.frame_id = "/base_link"
accel_raw_x_msg.data = accel_raw['x']
accel_raw_y_msg = Float64()
accel_raw_y_msg.header.stamp = rospy.Time.now()
accel_raw_y_msg.header.frame_id = "/base_link"
accel_raw_y_msg.data = accel_raw['y']
accel_raw_z_msg = Float64()
accel_raw_z_msg.header.stamp = rospy.Time.now()
accel_raw_z_msg.header.frame_id = "/base_link"
accel_raw_z_msg.data = accel_raw['z']
gyro_msg = Gyroscope()
gyro_msg.header.stamp = rospy.Time.now()
gyro_msg.header.frame_id = "/base_link"
gyro_msg.x = gyro_only['pitch']
gyro_msg.y = gyro_only['roll']
gyro_msg.z = gyro_only['yaw']
gyro_pitch_msg = Pitch()
gyro_pitch_msg.header.stamp = rospy.Time.now()
gyro_pitch_msg.header.frame_id = "/base_link"
gyro_pitch_msg.data = gyro_only['pitch']
gyro_roll_msg = Roll()
gyro_roll_msg.header.stamp = rospy.Time.now()
gyro_roll_msg.header.frame_id = "/base_link"
gyro_roll_msg.data = gyro_only['roll']
gyro_yaw_msg = Yaw()
gyro_yaw_msg.header.stamp = rospy.Time.now()
gyro_yaw_msg.header.frame_id = "/base_link"
gyro_yaw_msg.data = gyro_only['yaw']
gyro_raw_msg = Gyroscope()
gyro_raw_msg.header.stamp = rospy.Time.now()
gyro_raw_msg.header.frame_id = "/base_link"
gyro_raw_msg.x = gyro_raw['x']
gyro_raw_msg.y = gyro_raw['y']
gyro_raw_msg.z = gyro_raw['z']
gyro_raw_x_msg = Float64()
gyro_raw_x_msg.header.stamp = rospy.Time.now()
gyro_raw_x_msg.header.frame_id = "/base_link"
gyro_raw_x_msg.data = gyro_raw['x']
gyro_raw_y_msg = Float64()
gyro_raw_y_msg.header.stamp = rospy.Time.now()
gyro_raw_y_msg.header.frame_id = "/base_link"
gyro_raw_y_msg.data = gyro_raw['y']
gyro_raw_z_msg = Float64()
gyro_raw_z_msg.header.stamp = rospy.Time.now()
gyro_raw_z_msg.header.frame_id = "/base_link"
gyro_raw_z_msg.data = gyro_raw['z']
north_msg = Magnetometer()
north_msg.header.stamp = rospy.Time.now()
north_msg.header.frame_id = "/base_link"
north_msg.north = north
compass_msg = Orientation()
compass_msg.header.stamp = rospy.Time.now()
compass_msg.header.stamp = rospy.Time.now()
compass_msg.x = compass['x']
compass_msg.y = compass['y']
compass_msg.z = compass['z']
orientation_msg = Orientation()
orientation_msg.header.stamp = rospy.Time.now()
orientation_msg.header.stamp = rospy.Time.now()
orientation_msg.x = orientation['pitch']
orientation_msg.y = orientation['roll']
orientation_msg.z = orientation['yaw']
orientation_degree_msg = Orientation()
orientation_degree_msg.header.stamp = rospy.Time.now()
orientation_degree_msg.header.stamp = rospy.Time.now()
orientation_degree_msg.x = orientation_deg['pitch']
orientation_degree_msg.y = orientation_deg['roll']
orientation_degree_msg.z = orientation_deg['yaw']
orientation_rad_msg = Orientation()
orientation_rad_msg.header.stamp = rospy.Time.now()
orientation_rad_msg.header.stamp = rospy.Time.now()
orientation_rad_msg.x = orientation_rad['pitch']
orientation_rad_msg.y = orientation_rad['roll']
orientation_rad_msg.z = orientation_rad['yaw']
rospy.loginfo(
"imu/accelerometer: p: {pitch}, r: {roll}, y: {yaw}".format(
**accel_only))
rospy.loginfo(
"imu/accelerometer/raw: x: {x}, y: {y}, z: {z}".format(
**accel_raw))
rospy.loginfo(
"imu/gyroscope: p: {pitch}, r: {roll}, y: {yaw}".format(
**gyro_only))
rospy.loginfo(
"imu/gyroscope/raw: x: {x}, y: {y}, z: {z}".format(**gyro_raw))
rospy.loginfo("imu/magnetometer: North: %s" % north)
rospy.loginfo(
"imu/magnetometer/raw: x: {x}, y: {y}, z: {z}".format(
**compass))
rospy.loginfo(
"imu/orientation: p: {pitch}, r: {roll}, y: {yaw}".format(
**orientation))
rospy.loginfo(
"imu/orientation/degrees: p: {pitch}, r: {roll}, y: {yaw}".
format(**orientation_deg))
rospy.loginfo(
"imu/orientation/radians: p: {pitch}, r: {roll}, y: {yaw}".
format(**orientation_rad))
rospy.loginfo("imu/orientation/north: North: %s" % north)
self.imuPub.publish(imu_msg)
self.imuRawPub.publish(imu_raw_msg)
self.accelerometerPub.publish(accel_msg)
self.accelerometerPitchPub.publish(accel_pitch_msg)
self.accelerometerRollPub.publish(accel_roll_msg)
self.accelerometerYawPub.publish(accel_yaw_msg)
self.accelerometerRawPub.publish(accel_raw_msg)
self.accelerometerRawXPub.publish(accel_raw_x_msg)
self.accelerometerRawYPub.publish(accel_raw_y_msg)
self.accelerometerRawZPub.publish(accel_raw_z_msg)
self.gyroscopePub.publish(gyro_msg)
self.gyroscopePitchPub.publish(gyro_pitch_msg)
self.gyroscopeRollPub.publish(gyro_roll_msg)
self.gyroscopeYawPub.publish(gyro_yaw_msg)
self.gyroscopeRawPub.publish(gyro_raw_msg)
self.gyroscopeRawXPub.publish(gyro_raw_x_msg)
self.gyroscopeRawYPub.publish(gyro_raw_y_msg)
self.gyroscopeRawZPub.publish(gyro_raw_z_msg)
self.magnetometerPub.publish(north_msg)
self.magnetometerRawPub.publish(compass_msg)
self.orientationPub.publish(orientation_msg)
self.orientationDegreePub.publish(orientation_degree_msg)
self.orientationRadiansPub.publish(orientation_rad_msg)
self.orientationNorthPub.publish(north_msg)
self.rate.sleep()
class Roll:
def __init__(self, layout: Layout, dim=8):
self.dim = dim
# prep hat
self.hat = SenseHat()
self.hat.clear()
self.hat.set_imu_config(False, True, False)
# set maze colors
self.ball_color = (255, 0, 0)
self.target_color = (0, 255, 0)
self.background_color = (0, 0, 0)
self.wall_color = (255, 255, 255)
# set the layout of the maze
self.layout = layout
self.set_layout()
def set_layout(self):
for i in range(self.dim):
for j in range(self.dim):
if self.layout.arr[i][j] == 1:
self.hat.set_pixel(i, j, self.wall_color)
# create ball
self.x = self.layout.start.x
self.y = self.layout.start.y
self.hat.set_pixel(self.x, self.y, self.ball_color)
# create target
self.target_x = self.layout.end.x
self.target_y = self.layout.end.y
self.hat.set_pixel(self.target_x, self.target_y, self.target_color)
# is ball == target?
self.done = False
def celebrate_win(self):
for _ in range(10):
self.hat.set_pixels(Special.random())
time.sleep(0.25)
self.hat.set_pixels(Face.happy)
time.sleep(1)
self.hat.set_pixels(Face.wink_left)
time.sleep(0.3)
self.hat.set_pixels(Face.happy)
time.sleep(1)
def move_ball(self, roll, pitch):
self.hat.set_pixel(self.x, self.y, self.background_color)
if roll < 0:
if self.y - 1 >= 0:
if self.layout.arr[self.x][self.y - 1] == 0:
self.y -= 1
elif roll > 0:
if self.y + 1 < self.dim:
if self.layout.arr[self.x][self.y + 1] == 0:
self.y += 1
if pitch > 0:
if self.x - 1 >= 0:
if self.layout.arr[self.x - 1][self.y] == 0:
self.x -= 1
elif pitch < 0:
if self.x + 1 < self.dim:
if self.layout.arr[self.x + 1][self.y] == 0:
self.x += 1
self.hat.set_pixel(self.x, self.y, self.ball_color)
def run(self):
while self.done is False:
o = self.hat.get_orientation_radians()
roll = o['roll']
pitch = o['pitch']
if abs(roll) > 0.3 or abs(pitch) > 0.3:
epsilon = 0.1
else:
epsilon = 0.3
self.move_ball(roll=roll, pitch=pitch)
if self.x == self.target_x and self.y == self.target_y:
self.done = True
time.sleep(epsilon)
self.celebrate_win()
def main():
sensehat = SenseHat()
sensehat.set_imu_config(True, True, True)
velocityMatrix = [0.0, 0.0, 0.0]
resultMatrixKeys = ['x', 'y', 'z']
velocityMatrix = dict(zip(resultMatrixKeys, velocityMatrix))
displacementX = 0
displacementY = 0
displacementZ = 0
sensehat.show_message("calibrating", 0.1, [100, 0, 0])
sensehat.show_letter('!', [100, 0, 0])
gCoeff = calculateGCoeff(sensehat)
sensehat.show_message("Complete", 0.1, [0, 100, 0])
flag = True
while flag:
starttime = time.time()
'''get raw data'''
accel_raw = sensehat.get_accelerometer_raw()
orient_rad = sensehat.get_orientation_radians()
orient_deg = sensehat.get_orientation_degrees()
external_temp = sensehat.get_temperature()
pressure = sensehat.get_pressure()
eulermatrixtrans = createTransformMatrix(orient_rad)
accel_procc = removeGravity(accel_raw, eulermatrixtrans, gCoeff)
accel_real = matrixMultiply3x9(accel_procc, eulermatrixtrans)
endtime = time.time()
'''calculate velocity change and displacement'''
total_time = starttime - endtime
'''if motion is detected we output distance moved'''
if abs(accel_procc[0]) > 0.03 or abs(accel_procc[1]) > 0.03 or abs(
accel_procc[2]) > 0.03:
print(
("acceleration x = {0},y={1},z={2}, degrees to north = {3},{4}"
.format(accel_real['x'], accel_real['y'], accel_real['z'],
orient_rad, orient_deg)))
config = {
"apiKey": "AIzaSyA20qu9ddnRJPAQgGpn9ySQLuqjLH2WWPI",
"authDomain": "firefightingmonitoringsystem.firebaseapp.com",
"databaseURL":
"https://firefightingmonitoringsystem.firebaseio.com/",
"storageBucket": "firefightingmonitoringsystem.appspot.com"
}
fireBase = pyrebase.initialize_app(config)
db = firebase.database()
'''Need to add acceleration data'''
data = {
"external_temp": external_temp,
"pressure": pressure,
}
results = db.push(data)
print(results)
return
def pi3d_model():
from sense_hat import SenseHat
import math
import pi3d
sense = SenseHat()
display = pi3d.Display.create()
cam = pi3d.Camera.instance()
shader = pi3d.Shader("mat_light")
model = pi3d.Model(
file_string="cow2.obj",
name="model", x=0, y=-1, z=40, sx=2.5, sy=2.5, sz=2.5)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 0
while display.loop_running():
orientation = sense.get_orientation_radians()
if orientation is None:
pass
pitch = orientation["pitch"]
roll = orientation["roll"]
yaw = orientation["yaw"]
yaw_total = yaw + math.radians(yaw_offset)
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
sin_r = math.sin(roll)
cos_r = math.cos(roll)
abs_roll = math.degrees(math.asin(sin_p * cos_y + cos_p * sin_r * sin_y))
abs_pitch = math.degrees(math.asin(sin_p * sin_y - cos_p * sin_r * cos_y))
model.rotateToZ(abs_roll)
model.rotateToX(abs_pitch)
model.rotateToY(math.degrees(yaw_total))
model.draw()
keypress = keyb.read()
if keypress == 27:
keyb.close()
display.destroy()
break
elif keypress == ord('m'):
compass = not compass
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('g'):
gyro = not gyro
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('a'):
accel = not accel
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('='):
yaw_offset += 1
elif keypress == ord('-'):
yaw_offset -= 1
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
#sense.set_imu_config(compass, gyro, accel)
sense.set_imu_config(False, True, False )
pitch=math.pi/4
roll=0
yaw=0
yaw_offset = 0
while display.loop_running():
o = sense.get_orientation_radians()
if o is None:
pass
pitch = o["pitch"]
roll = o["roll"]
yaw = o["yaw"]
#roll +=math.pi/180
yaw_total = yaw + math.radians(yaw_offset)
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
#print(sense.compass_raw)
sense.set_imu_config(False, True, False) #只启用陀螺仪
roll, pitch, yaw = sense.get_orientation().values() #获取当前俯仰轴，滚转轴，偏航轴的角度信息
roll = round(roll, 2)
pitch = round(pitch, 2)
yaw = round(yaw, 2)
print("Pitch Axis = {} , Roll Axis = {} , Yaw Axis = {}".format(
pitch, roll, yaw))
orientation = sense.get_orientation() #获取当前俯仰轴，滚转轴，偏航轴的角度信息。
print("pitch = {}, roll = {}, yell = {}".format(
round(orientation["pitch"], 2), round(orientation["roll"], 2),
round(orientation["yaw"], 2)))
orientation = sense.get_orientation_radians() #获取当前俯仰轴，滚转轴，偏航轴的弧度信息。
print("pitch = {}, roll = {}, yell = {}".format(
round(orientation["pitch"], 2), round(orientation["roll"], 2),
round(orientation["yaw"], 2)))
#gyro_only = sense.get_gyroscope()#仅从陀螺仪获取当前方向
#print("p: {pitch}, r: {roll}, y: {yaw}".format(**gyro_only))
#print(sense.gyro)
#print(sense.gyroscope)
#raw = sense.get_gyroscope_raw()#获取原始的x，y和z轴陀螺仪数据
#print("x: {x}, y: {y}, z: {z}".format(**raw))
#print(sense.gyro_raw)
#print(sense.gyroscope_raw)
sense.set_imu_config(False, False, True) #只启用加速度计
sensehat.clear( sens_defn.V )
time.sleep( .1 )
sensehat.clear( )
time.sleep( .1 )
### sensor
print( sensehat.gamma )
print( "Humidity : %.4f %%rH" % sensehat.get_humidity( ) )
print( "Temp main: %.4f C" % sensehat.get_temperature( ) )
print( "Temp humd: %.4f C" % sensehat.get_temperature_from_humidity( ) )
print( "Temp prss: %.4f C" % sensehat.get_temperature_from_pressure( ) )
print( "Pressure : %.4f mb" % sensehat.get_pressure( ) ) # millibar
sensehat.set_imu_config( True , True , True )
print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_orientation_radians( ) ) )
print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_orientation_degrees( ) ) )
print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_orientation( ) ) )
print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_gyroscope( ) ) )
print( "p: {pitch} , r: {roll} , y: {yaw}".format( **sensehat.get_accelerometer( ) ) )
print( "North: %s" % sensehat.get_compass( ) )
print( sensehat.get_compass_raw( ) )
print( "x: {x} , y: {y} , z: {z}".format( **sensehat.get_compass_raw( ) ) )
print( "x: %.4f" % sensehat.get_compass_raw( )['x']
+ " , y: %.4f" % sensehat.get_compass_raw( )['y']
+ " , Z: %.4f" % sensehat.get_compass_raw( )['z'] )
print( "x: %.4f" % sensehat.get_gyroscope_raw( )['x']
+ " , y: %.4f" % sensehat.get_gyroscope_raw( )['y']
+ " , Z: %.4f" % sensehat.get_gyroscope_raw( )['z'] )
def pi3d_model():
from sense_hat import SenseHat
import math
import pi3d
sense = SenseHat()
display = pi3d.Display.create()
cam = pi3d.Camera.instance()
shader = pi3d.Shader("mat_light")
# .obj file is read in and x,y,z size(s) are determined
model = pi3d.Model(
file_string="apollo-soyuz.obj",
name="model", x=0, y=-1, z=40, sx=1.5, sy=1.5, sz=1.5)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 133 # This offset aligns the model with the Pi
while display.loop_running():
orientation = sense.get_orientation_radians()
if orientation is None:
pass
pitch = orientation["pitch"]
roll = orientation["roll"]
yaw = orientation["yaw"]
yaw_total = yaw + math.radians(yaw_offset)
# Maths!
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
sin_r = math.sin(roll)
cos_r = math.cos(roll)
abs_roll = math.degrees(math.asin(sin_p * cos_y + cos_p * sin_r * sin_y))
abs_pitch = math.degrees(math.asin(sin_p * sin_y - cos_p * sin_r * cos_y))
model.rotateToZ(abs_roll)
model.rotateToX(abs_pitch)
model.rotateToY(math.degrees(yaw_total))
model.draw()
keypress = keyb.read()
if keypress == 27:
keyb.close()
display.destroy()
break
elif keypress == ord('m'): # Toggles Magnetometer
compass = not compass
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('g'): # Toggles Gyroscope
gyro = not gyro
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('a'): # Toggles Accelerometer
accel = not accel
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('='): # Increases yaw offset
yaw_offset += 1
elif keypress == ord('-'): # Decreases yaw offset
yaw_offset -= 1
class SenseHATPlugin(EPLPluginBase):
def __init__(self, init):
super(SenseHATPlugin, self).__init__(init)
self.getLogger().info("SenseHATPlugin initialised with config: %s" %
self.getConfig())
self.sense = SenseHat()
@EPLAction("action<integer >")
def setRotation(self, angle=None):
"""
Orientation of the message/image
@param message: Orientation of the message, The supported values of orientation are 0, 90, 180, and 270..
"""
if angle == None: angle = 90
self.sense.set_rotation(angle)
@EPLAction("action<integer, integer, integer, integer, integer >")
def setPixel(self, x, y, r, g, b):
self.sense.set_pixel(x, y, r, g, b)
@EPLAction("action< sequence <sequence <integer> > >")
def setPixels(self, pixels):
if not isinstance(pixels, list): return
self.sense.set_pixels(pixels)
@EPLAction("action< >")
def clear(self):
self.sense.clear()
@EPLAction("action< integer, integer, integer >")
def Clear(self, r, g, b):
self.sense.clear(r, g, b)
@EPLAction("action<string >")
def showMessage(self, message):
self.sense.show_message(message)
@EPLAction("action<string, float, sequence<integer>, sequence<integer> >")
def displayMessage(self,
message,
scrollingSpeed=None,
textColor=None,
backgroundColor=None):
"""
This method will scrolls a text message across the LED Matrix.
@param message: The Scrolling message to be displayed.
@param scrollingSpeed: Scrolling message speed.
@param textColor: The text color of the scrolling message i.e [R G B] .
@param backgroundColor: The background color of the scrolling message [R G B].
"""
if scrollingSpeed == None: scrollingSpeed = 0.1
if (textColor == None) or (not isinstance(textColor, list)):
textColor = [255, 255, 255]
if (backgroundColor
== None) or (not isinstance(backgroundColor, list)):
backgroundColor = [0, 0, 0]
self.sense.show_message(message,
scroll_speed=scrollingSpeed,
text_colour=textColor,
back_colour=backgroundColor)
@EPLAction("action<string >")
def showLetter(self, letter):
if not isinstance(letter, basestring): return
self.sense.show_letter(letter)
@EPLAction("action<boolean >")
def lowLight(self, islowLight):
self.sense.low_light = islowLight
@EPLAction("action<string >")
def loadImage(self, imagePath):
self.sense.load_image(imagePath)
##########################################################################################################
# Environmental sensors
##########################################################################################################
@EPLAction("action<> returns float")
def getHumidity(self):
humidity = self.sense.get_humidity()
return round(humidity, 2)
@EPLAction("action<> returns float")
def getTemperature(self):
temp = self.sense.get_temperature()
return round(temp, 2)
@EPLAction("action<> returns float")
def getTemperatureFromHumidity(self):
temp = self.sense.get_temperature_from_humidity()
return round(temp, 2)
@EPLAction("action<> returns float")
def getTemperatureFromPressure(self):
temp = self.sense.get_temperature_from_pressure()
return round(temp, 2)
@EPLAction("action<> returns float")
def getPressure(self):
pressure = self.sense.get_pressure()
return round(pressure, 2)
##########################################################################################################
# Joystick
##########################################################################################################
@EPLAction("action<boolean > returns com.apamax.sensehat.InputEvent")
def waitForEvent(self, emptyBuffer=False):
jevent = self.sense.stick.wait_for_event(emptybuffer=emptyBuffer)
evtInstance = Event(
'com.apamax.sensehat.InputEvent', {
"actionValue": jevent.action,
"directionValue": jevent.direction,
"timestamp": jevent.timestamp
})
return evtInstance
@EPLAction("action<> returns sequence<com.apamax.sensehat.InputEvent >")
def getEvents(self):
events = list()
for event in self.sense.stick.get_events():
evtInstance = Event(
'com.apamax.sensehat.InputEvent', {
"actionValue": jevent.action,
"directionValue": jevent.direction,
"timestamp": jevent.timestamp
})
events.append(evtInstance)
return events
def pushed_up(event):
if event.action == ACTION_RELEASED:
joyevt = Event('com.apamax.sensehat.JoystickControl',
{"controlType": 1})
Correlator.sendTo("sensedhat_data", joyevt)
def pushed_down(event):
if event.action == ACTION_RELEASED:
joyevt = Event('com.apamax.sensehat.JoystickControl',
{"controlType": 2})
Correlator.sendTo("sensedhat_data", joyevt)
def pushed_left(event):
if event.action == ACTION_RELEASED:
joyevt = Event('com.apamax.sensehat.JoystickControl',
{"controlType": 3})
Correlator.sendTo("sensedhat_data", joyevt)
def pushed_right(event):
if event.action == ACTION_RELEASED:
joyevt = Event('com.apamax.sensehat.JoystickControl',
{"controlType": 4})
Correlator.sendTo("sensedhat_data", joyevt)
def pushed_in(event):
if event.action == ACTION_RELEASED:
self.sense.show_message(str(round(sense.temp, 1)), 0.05, b)
##########################################################################################################
# IMU Sensor
##########################################################################################################
@EPLAction("action<boolean, boolean, boolean >")
def setIMUConfig(self, compassEnabled, gyroscopeEnabled,
accelerometerEnabled):
'''
Enables and disables the gyroscope, accelerometer and/or magnetometer contribution to the get orientation functions
@param compassEnabled : enable compass
@param gyroscopeEnabled : enable gyroscope
@param accelerometerEnabled : enable accelerometer
'''
self.sense.set_imu_config(compassEnabled, gyroscopeEnabled,
accelerometerEnabled)
@EPLAction("action< > returns com.apamax.sensehat.IMUData")
def getOrientationRadians(self):
'''
Gets the current orientation in radians using the aircraft principal axes of pitch, roll and yaw
@return event with pitch, roll and yaw values. Values are floats representing the angle of the axis in radians
'''
pitch, roll, yaw = self.sense.get_orientation_radians().values()
evtInstance = Event('com.apamax.sensehat.IMUData', {
"pitch": pitch,
"roll": roll,
"yaw": yaw
})
return evtInstance
@EPLAction("action< > returns com.apamax.sensehat.IMUData")
def getOrientationDegrees(self):
'''
Gets the current orientation in degrees using the aircraft principal axes of pitch, roll and yaw
@return event with pitch, roll and yaw values. Values are Floats representing the angle of the axis in degrees
'''
pitch, roll, yaw = self.sense.get_orientation_degrees().values()
evtInstance = Event('com.apamax.sensehat.IMUData', {
"pitch": pitch,
"roll": roll,
"yaw": yaw
})
return evtInstance
@EPLAction("action< > returns com.apamax.sensehat.IMUData")
def getOrientation(self):
'''
@return event with pitch, roll and yaw representing the angle of the axis in degrees
'''
pitch, roll, yaw = self.sense.get_orientation().values()
evtInstance = Event('com.apamax.sensehat.IMUData', {
"pitch": pitch,
"roll": roll,
"yaw": yaw
})
return evtInstance
@EPLAction("action< > returns float")
def getCompass(self):
'''
Calls set_imu_config internally in Python core to disable the gyroscope and accelerometer
then gets the direction of North from the magnetometer in degrees
@return The direction of North
'''
return self.sense.get_compass()
@EPLAction("action< > returns com.apamax.sensehat.IMUDataRaw")
def getCompassRaw(self):
'''
Gets the raw x, y and z axis magnetometer data
@return event representing the magnetic intensity of the axis in microteslas (uT)
'''
x, y, z = self.sense.get_compass_raw().values()
evtInstance = Event('com.apamax.sensehat.IMUDataRaw', {
"x": x,
"y": y,
"z": z
})
return evtInstance
@EPLAction("action< > returns com.apamax.sensehat.IMUData")
def getGyroscope(self):
'''
Calls set_imu_config internally in Python core to disable the magnetometer and accelerometer
then gets the current orientation from the gyroscope only
@return event with pitch, roll and yaw representing the angle of the axis in degrees
'''
pitch, roll, yaw = self.sense.get_gyroscope().values()
evtInstance = Event('com.apamax.sensehat.IMUData', {
"pitch": pitch,
"roll": roll,
"yaw": yaw
})
return evtInstance
@EPLAction("action< > returns com.apamax.sensehat.IMUDataRaw")
def getGyroscopeRaw(self):
'''
Gets the raw x, y and z axis gyroscope data
@return event representing the rotational intensity of the axis in radians per second
'''
x, y, z = sense.get_gyroscope_raw().values()
evtInstance = Event('com.apamax.sensehat.IMUDataRaw', {
"x": x,
"y": y,
"z": z
})
return evtInstance
@EPLAction("action< > returns com.apamax.sensehat.IMUData")
def getAccelerometer(self):
'''
Calls set_imu_config in Python core to disable the magnetometer and gyroscope
then gets the current orientation from the accelerometer only
@return Object representing the angle of the axis in degrees
'''
pitch, roll, yaw = self.sense.get_accelerometer().values()
evtInstance = Event('com.apamax.sensehat.IMUData', {
"pitch": pitch,
"roll": roll,
"yaw": yaw
})
return evtInstance
@EPLAction("action< > returns com.apamax.sensehat.IMUDataRaw")
def getAccelerometerRaw(self):
'''
Gets the raw x, y and z axis accelerometer data
@return event representing the acceleration intensity of the axis in Gs
'''
x, y, z = sense.get_accelerometer_raw().values()
evtInstance = Event('com.apamax.sensehat.IMUDataRaw', {
"x": x,
"y": y,
"z": z
})
return evtInstance
class OrientationSensor:
def __init__(self):
self.start = True
self.sensehat = SenseHat()
self.accel_procc = 0
def calculateGCoeff(self):
g = []
samples = 100
for i in range(samples):
accel_raw = self.sensehat.get_accelerometer_raw()
orient_rad = self.sensehat.get_orientation_radians()
eulermatrixtrans = self.createTransformMatrix(orient_rad)
gX = accel_raw['x'] / eulermatrixtrans[0][2]
gY = accel_raw['y'] / eulermatrixtrans[1][2]
gZ = accel_raw['z'] / eulermatrixtrans[2][2]
gVal = (gX + gY + gZ) / 3
g.append(gVal)
sum = 0.0
for j in range(samples):
sum += g[j]
return sum / 100.0
def removeGravity(self, accel_raw, eulertransmatrix, g):
rotated_gravity = [0.0, 0.0, 0.0, 0.0]
rotated_gravity[0] = g * eulertransmatrix[0][2]
rotated_gravity[1] = g * eulertransmatrix[1][2]
rotated_gravity[2] = g * eulertransmatrix[2][2]
''' we can then calculate the impact of gravity with these quations'''
x = accel_raw['x'] - rotated_gravity[0]
y = accel_raw['y'] - rotated_gravity[1]
z = accel_raw['z'] - rotated_gravity[2]
return x, y, z
def createTransformMatrix(self, orient_rad):
'''We find the orientation of our device so that the orientation can match'''
yaw = orient_rad['yaw']
pitch = orient_rad['pitch']
roll = orient_rad['roll']
sinRoll = math.sin(roll)
cosRoll = math.cos(roll)
sinPitch = math.sin(pitch)
cosPitch = math.cos(pitch)
sinYaw = math.sin(yaw)
cosYaw = math.cos(yaw)
eulervalues = {
'sinRoll': sinRoll,
'sinPitch': sinPitch,
'sinYaw': sinYaw,
'cosRoll': cosRoll,
'cosPitch': cosPitch,
'cosYaw': cosYaw,
}
'''we transform our euler matrix so we can use it to normalize our accelerations'''
eulermatrixtrans = []
row3 = [
cosYaw * sinPitch * cosRoll + sinYaw * sinRoll,
sinYaw * sinPitch * cosRoll - cosYaw * sinRoll, cosPitch * cosRoll
]
row2 = [
cosYaw * sinPitch * sinRoll - sinYaw * cosRoll,
sinYaw * sinPitch * sinRoll + cosYaw * cosRoll, cosPitch * sinRoll
]
row1 = [cosYaw * sinPitch, sinYaw * cosPitch, sinPitch * (-1)]
eulermatrixtrans.append(row1)
eulermatrixtrans.append(row2)
eulermatrixtrans.append(row3)
return eulermatrixtrans
def matrixMultiply3x9(self, firstMatrix, secondMatrix):
resultMatrix = [0.0, 0.0, 0.0]
resultMatrixKeys = ['x', 'y', 'z']
resultMatrix[0] = firstMatrix[0] * secondMatrix[0][0] + firstMatrix[
1] * secondMatrix[1][0] + firstMatrix[2] * secondMatrix[2][0]
resultMatrix[1] = firstMatrix[0] * secondMatrix[1][0] + firstMatrix[
1] * secondMatrix[1][1] + firstMatrix[2] * secondMatrix[2][1]
resultMatrix[2] = firstMatrix[0] * secondMatrix[2][0] + firstMatrix[
1] * secondMatrix[1][2] + firstMatrix[2] * secondMatrix[2][2]
return dict(zip(resultMatrixKeys, resultMatrix))
def mainOrientationLoop(self):
self.sensehat.set_imu_config(True, True, True)
velocityMatrix = [0.0, 0.0, 0.0]
resultMatrixKeys = ['x', 'y', 'z']
velocityMatrix = dict(zip(resultMatrixKeys, velocityMatrix))
displacementX = 0
displacementY = 0
displacementZ = 0
gCoeff = self.calculateGCoeff()
flag = True
while not self.thread.stopped:
starttime = time.time()
'''get raw data'''
accel_raw = self.sensehat.get_accelerometer_raw()
orient_rad = self.sensehat.get_orientation_radians()
orient_deg = self.sensehat.get_orientation_degrees()
external_temp = self.sensehat.get_temperature()
pressure = self.sensehat.get_pressure()
eulermatrixtrans = self.createTransformMatrix(orient_rad)
self.accel_procc = self.removeGravity(accel_raw, eulermatrixtrans,
gCoeff)
'''endtime = time.time()'''
'''calculate velocity change and displacement'''
'''total_time = starttime - endtime'''
'''if motion is detected we output distance moved'''
'''if abs(accel_procc[0]) > 0.03 or abs(accel_procc[1]) > 0.03 or abs(accel_procc[2]) > 0.03 :
print(("acceleration x = {0},y={1},z={2}, degrees to north = {3},{4}"
.format(accel_procc[0] ,accel_procc[1] ,accel_procc[2], orient_rad,orient_deg )))
'''
return
def startAsyncOrientation(self):
self.thread = threading.Thread(target=self.mainOrientationLoop)
self.thread.stopped = False
self.thread.start()
return
def stopAsyncOrientation(self):
self.thread.stopped = true
self.accel_procc = 0
return
class SenseHatREST:
ETX = [255, 0, 0]
OFF = [0, 0, 0] # off
BTX = [0, 255, 0] # on
led_tx_on = [
BTX, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF
]
led_tx_off = [
ETX, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF,
OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF
]
RADIANS = "RADIANS"
ACCEL = "ACCEL"
GYRO = "GYRO"
def __init__(self):
self.sense = SenseHat()
self.sense.set_imu_config(True, True, True)
def get_record_from_radians(self):
imu_input = self.sense.get_orientation_radians()
data = {
'xRoll': imu_input['pitch'],
'yPitch': imu_input['roll'],
'zYaw': imu_input['yaw'],
'timestamp': round(time.time() * 1000),
}
return data
def make_post_call(self, data, config):
data['experimentId'] = config.get('experiment_id'),
data['type'] = config.get('sensor_type'),
data['sensorLocation'] = config.get('sensor_location'),
data['subject'] = config.get('subject'),
data['exercise'] = config.get('exercise'),
data['isRaw'] = config.get('False'),
data['timestamp'] = round(time.time() * 1000),
data['deviceId'] = config.get('inputDeviceId')
sense.set_pixels(led_tx_on)
requests.post(config.get('restEndpoint'), json=data)
sense.set_pixels(led_tx_off)
def pi3d_model():
from sense_hat import SenseHat
import math
import pi3d
sense = SenseHat()
display = pi3d.Display.create()
cam = pi3d.Camera.instance()
shader = pi3d.Shader("mat_light")
# .obj file is read in and x,y,z size(s) are determined
model = pi3d.Model(file_string="apollo-soyuz.obj",
name="model",
x=0,
y=-1,
z=40,
sx=1.5,
sy=1.5,
sz=1.5)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 133 # This offset aligns the model with the Pi
while display.loop_running():
orientation = sense.get_orientation_radians()
if orientation is None:
pass
pitch = orientation["pitch"]
roll = orientation["roll"]
yaw = orientation["yaw"]
yaw_total = yaw + math.radians(yaw_offset)
# Maths!
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
sin_r = math.sin(roll)
cos_r = math.cos(roll)
abs_roll = math.degrees(
math.asin(sin_p * cos_y + cos_p * sin_r * sin_y))
abs_pitch = math.degrees(
math.asin(sin_p * sin_y - cos_p * sin_r * cos_y))
model.rotateToZ(abs_roll)
model.rotateToX(abs_pitch)
model.rotateToY(math.degrees(yaw_total))
model.draw()
keypress = keyb.read()
if keypress == 27:
keyb.close()
display.destroy()
break
elif keypress == ord('m'): # Toggles Magnetometer
compass = not compass
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('g'): # Toggles Gyroscope
gyro = not gyro
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('a'): # Toggles Accelerometer
accel = not accel
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('='): # Increases yaw offset
yaw_offset += 1
elif keypress == ord('-'): # Decreases yaw offset
yaw_offset -= 1
class PiSenseHat(object):
"""Raspberry Pi 'IoT Sense Hat API Driver Class'."""
# Constructor
def __init__(self):
self.sense = SenseHat()
# enable all IMU functions
self.sense.set_imu_config(True, True, True)
# pixel display
def set_pixel(self, x, y, color):
# red = (255, 0, 0)
# green = (0, 255, 0)
# blue = (0, 0, 255)
self.sense.set_pixel(x, y, color)
# clear pixel display
def clear_display(self):
self.sense.clear()
# Pressure
def getPressure(self):
return self.sense.get_pressure()
# Temperature
def getTemperature(self):
return self.sense.get_temperature()
# Humidity
def getHumidity(self):
return self.sense.get_humidity()
def getHumidityTemperature(self):
return self.sense.get_temperature_from_humidity()
def getPressureTemperature(self):
return self.sense.get_temperature_from_pressure()
def getOrientationRadians(self):
return self.sense.get_orientation_radians()
def getOrientationDegrees(self):
return self.sense.get_orientation_degrees()
# degrees from North
def getCompass(self):
return self.sense.get_compass()
def getAccelerometer(self):
return self.sense.get_accelerometer_raw()
def getEnvironmental(self):
sensors = {'name': 'sense-hat', 'environmental': {}}
return sensors
def getJoystick(self):
sensors = {'name': 'sense-hat', 'joystick': {}}
return sensors
def getInertial(self):
sensors = {'name': 'sense-hat', 'inertial': {}}
def getAllSensors(self):
sensors = {
'name': 'sense-hat',
'environmental': {},
'inertial': {},
'joystick': {},
'location': {}
} # add location
sensors['environmental']['pressure'] = {
'value': self.sense.get_pressure(),
'unit': 'mbar'
}
sensors['environmental']['temperature'] = {
'value': self.sense.get_temperature(),
'unit': 'C'
}
sensors['environmental']['humidity'] = {
'value': self.sense.get_humidity(),
'unit': '%RH'
}
accel = self.sense.get_accelerometer_raw()
sensors['inertial']['accelerometer'] = {
'x': accel['x'],
'y': accel['y'],
'z': accel['z'],
'unit': 'g'
}
orientation = self.sense.get_orientation_degrees()
sensors['inertial']['orientation'] = {
'compass': self.sense.get_compass(),
'pitch': orientation['pitch'],
'roll': orientation['roll'],
'yaw': orientation['yaw'],
'unit': 'degrees'
}
sensors['location']['lat'] = {
'value': 0
} # initialize these with 0 values to start
sensors['location']['lon'] = {'value': 0}
sensors['location']['alt'] = {'value': 0}
sensors['location']['sats'] = {'value': 0}
sensors['location']['speed'] = {'value': 0}
return sensors
class PiSenseHat(object):
"""Raspberry Pi 'IoT Sense Hat API Driver Class'."""
# Constructor
def __init__(self):
self.sense = SenseHat()
# enable all IMU functions
self.sense.set_imu_config(True, True, True)
# pixel display
def set_pixel(self,x,y,color):
# red = (255, 0, 0)
# green = (0, 255, 0)
# blue = (0, 0, 255)
self.sense.set_pixel(x, y, color)
# clear pixel display
def clear_display(self):
self.sense.clear()
# Pressure
def getPressure(self):
return self.sense.get_pressure()
# Temperature
def getTemperature(self):
return self.sense.get_temperature()
# Humidity
def getHumidity(self):
return self.sense.get_humidity()
def getHumidityTemperature(self):
return self.sense.get_temperature_from_humidity()
def getPressureTemperature(self):
return self.sense.get_temperature_from_pressure()
def getOrientationRadians(self):
return self.sense.get_orientation_radians()
def getOrientationDegrees(self):
return self.sense.get_orientation_degrees()
# degrees from North
def getCompass(self):
return self.sense.get_compass()
def getAccelerometer(self):
return self.sense.get_accelerometer_raw()
def getEnvironmental(self):
sensors = {'name' : 'sense-hat', 'environmental':{}}
return sensors
def getJoystick(self):
sensors = {'name' : 'sense-hat', 'joystick':{}}
return sensors
def getInertial(self):
sensors = {'name' : 'sense-hat', 'inertial':{}}
def getAllSensors(self):
sensors = {'name' : 'sense-hat', 'environmental':{}, 'inertial':{}, 'joystick':{}}
sensors['environmental']['pressure'] = { 'value':self.sense.get_pressure(), 'unit':'mbar'}
sensors['environmental']['temperature'] = { 'value':self.sense.get_temperature(), 'unit':'C'}
sensors['environmental']['humidity'] = { 'value':self.sense.get_humidity(), 'unit': '%RH'}
accel = self.sense.get_accelerometer_raw()
sensors['inertial']['accelerometer'] = { 'x':accel['x'], 'y':accel['y'], 'z': accel['z'], 'unit':'g'}
orientation = self.sense.get_orientation_degrees()
sensors['inertial']['orientation'] = { 'compass':self.sense.get_compass(), 'pitch':orientation['pitch'], 'roll':orientation['roll'], 'yaw': orientation['yaw'], 'unit':'degrees'}
return sensors
def pi3d_model():
from sense_hat import SenseHat
import math
import pi3d
sense = SenseHat()
display = pi3d.Display.create()
cam = pi3d.Camera.instance()
shader = pi3d.Shader("mat_light")
model = pi3d.Model(
file_string="CS294.obj",
name="model", x=0, y=-1, z=40, sx=7.5, sy=7.5, sz=7.5)
model.set_shader(shader)
cam.position((0, 20, 0))
cam.point_at((0, -1, 40))
keyb = pi3d.Keyboard()
compass = gyro = accel = True
sense.set_imu_config(compass, gyro, accel)
yaw_offset = 133
while display.loop_running():
orientation = sense.get_orientation_radians()
if orientation is None:
pass
pitch = orientation["pitch"]
roll = orientation["roll"]
yaw = orientation["yaw"]
yaw_total = yaw + math.radians(yaw_offset)
sin_y = math.sin(yaw_total)
cos_y = math.cos(yaw_total)
sin_p = math.sin(pitch)
cos_p = math.cos(pitch)
sin_r = math.sin(roll)
cos_r = math.cos(roll)
abs_roll = math.degrees(math.asin(sin_p * cos_y + cos_p * sin_r * sin_y))
abs_pitch = math.degrees(math.asin(sin_p * sin_y - cos_p * sin_r * cos_y))
model.rotateToZ(abs_roll)
model.rotateToX(abs_pitch)
model.rotateToY(math.degrees(yaw_total))
model.draw()
keypress = keyb.read()
if keypress == 27:
keyb.close()
display.destroy()
break
elif keypress == ord('m'):
compass = not compass
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('g'):
gyro = not gyro
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('a'):
accel = not accel
sense.set_imu_config(compass, gyro, accel)
elif keypress == ord('='):
yaw_offset += 1
elif keypress == ord('-'):
yaw_offset -= 1
#!/usr/bin/env python
import rospy
import time
import math
from sense_hat import SenseHat
from std_msgs.msg import Header
from tf.transformations import quaternion_from_euler
from sensor_msgs.msg import Imu, Temperature, RelativeHumidity, FluidPressure
sense = SenseHat()
sense.set_imu_config(True, True, True) #compass, gyro, accele
calibration = sense.get_orientation_radians()
start_roll = calibration['yaw']
start_pitch = calibration['roll']
start_yaw = calibration['pitch ']
start_time = int(time.time()) * 1000
def talker():
temp_pub = rospy.Publisher('rov/int_temperature', Temperature, queue_size = 1) #Publisher for the different sensors: Temperature, Humidity, Pressure,
pressure_pub = rospy.Publisher('rov/int_pressure', FluidPressure, queue_size = 1)
humidity_pub = rospy.Publisher('rov/int_humidity', RelativeHumidity, queue_size = 1)
imu_pub = rospy.Publisher("rov/imu", Imu, queue_size = 1) #Imu publisher
rate = rospy.Rate(60)
while not rospy.is_shutdown():
global calibration, start_roll, start_pitch, start_yaw, start_time
header = Header()
acc_vec = Vector3()
acceleration = sense.get_accelerometer_raw()
acc_vec.x = acceleration['x']
acc_vec.y = acceleration['y']
acc_vec.z = acceleration['z']
pub_accelerometer.publish(acc_vec)
gyro_vec = Vector3()
gyroscope = sense.get_gyroscope()
gyro_vec.x = gyroscope['roll']
gyro_vec.y = gyroscope['pitch']
gyro_vec.z = gyroscope['yaw']
pub_gyroscope.publish(gyro_vec)
ori_rad_pose = Quaternion()
orientation_rad = sense.get_orientation_radians()
quat = quaternion_from_euler(orientation_rad['roll'],
orientation_rad['pitch'],
orientation_rad['yaw'])
ori_rad_pose.x = quat[0]
ori_rad_pose.y = quat[1]
ori_rad_pose.z = quat[2]
ori_rad_pose.w = quat[3]
pub_orientation_quat.publish(ori_rad_pose)
pub_humidity.publish(sense.get_humidity())
pub_temperature.publish(sense.get_temperature())
pub_pressure.publish(sense.get_pressure())
for event in sense.stick.get_events():
if event.action == "pressed":
class PiSenseHat(object):
"""Raspberry Pi 'IoT Sense Hat API Driver Class'."""
# Constructor
def __init__(self):
self.sense = SenseHat()
# enable all IMU functions
self.sense.set_imu_config(True, True, True)
# Pressure
def getPressure(self):
return self.sense.get_pressure()
# Temperature
def getTemperature(self):
return self.sense.get_temperature()
# Humidity
def getHumidity(self):
return self.sense.get_humidity()
def getHumidityTemperature(self):
return self.sense.get_temperature_from_humidity()
def getPressureTemperature(self):
return self.sense.get_temperature_from_pressure()
def getOrientationRadians(self):
return self.sense.get_orientation_radians()
def getOrientationDegrees(self):
return self.sense.get_orientation_degrees()
# degrees from North
def getCompass(self):
return self.sense.get_compass()
def getAccelerometer(self):
return self.sense.get_accelerometer_raw()
def getEnvironmental(self):
sensors = {'name' : 'sense-hat', 'environmental':{}}
return sensors
def getJoystick(self):
sensors = {'name' : 'sense-hat', 'joystick':{}}
return sensors
def getInertial(self):
sensors = {'name' : 'sense-hat', 'inertial':{}}
def getAllSensors(self):
sensors = {'name' : 'sense-hat', 'environmental':{}, 'inertial':{}, 'joystick':{}}
sensors['environmental']['pressure'] = { 'value':self.sense.get_pressure(), 'unit':'mbar'}
sensors['environmental']['temperature'] = { 'value':self.sense.get_temperature(), 'unit':'C'}
sensors['environmental']['humidity'] = { 'value':self.sense.get_humidity(), 'unit': '%RH'}
accel = self.sense.get_accelerometer_raw()
sensors['inertial']['accelerometer'] = { 'x':accel['x'], 'y':accel['y'], 'z': accel['z'], 'unit':'g'}
orientation = self.sense.get_orientation_degrees()
sensors['inertial']['orientation'] = { 'compass':self.sense.get_compass(), 'pitch':orientation['pitch'], 'roll':orientation['roll'], 'yaw': orientation['yaw'], 'unit':'degrees'}
return sensors
