def get_sensors(precision):
"""
get temp, pressure, humidity from the Sense HAT
:param precision: Decimal point round precision, e.g. with 3 the results will be 24.054. Default 2
:return: returns a data dictionary
"""
sense = SenseHat()
data = {}
data['temperature'] = round(sense.get_temperature(), precision)
data['pressure'] = round(sense.get_pressure(), precision)
data['humidity'] = round(sense.get_humidity(), precision)
data['temperature_h'] = round(sense.get_temperature_from_humidity(),
precision)
data['temperature_p'] = round(sense.get_temperature_from_pressure(),
precision)
magnetometer_raw = sense.get_compass_raw()
data['magnetometer_x'] = magnetometer_raw['x']
data['magnetometer_y'] = magnetometer_raw['y']
data['magnetometer_z'] = magnetometer_raw['z']
gyroscope_raw = sense.get_gyroscope_raw()
data['gyroscope_x'] = gyroscope_raw['x']
data['gyroscope_y'] = gyroscope_raw['y']
data['gyroscope_z'] = gyroscope_raw['z']
accelerometer_raw = sense.get_accelerometer_raw()
data['accelerometer_x'] = accelerometer_raw['x']
data['accelerometer_y'] = accelerometer_raw['y']
data['accelerometer_z'] = accelerometer_raw['z']
return data
class SenseHAT(Block, EnrichSignals):
imu = ObjectProperty(IMUsensor, title='IMU Sensor')
version = VersionProperty('0.1.0')
def __init__(self):
super().__init__()
self.hat = None
def configure(self, context):
super().configure(context)
self.hat = SenseHat()
self.hat.set_imu_config(
self.imu().accel(),
self.imu().compass(),
self.imu().gyro())
def process_signals(self, signals):
data = {}
if self.imu().accel():
data['accelerometer'] = self.hat.get_accelerometer_raw()
if self.imu().compass():
data['compass'] = self.hat.get_compass_raw()
if self.imu().gyro():
data['gyroscope'] = self.hat.get_gyroscope_raw()
outgoing_signals = []
for signal in signals:
outgoing_signals.append(self.get_output_signal(data, signal))
self.notify_signals(outgoing_signals)
def SenseHatData(conf, dataobj):
sense = SenseHat()
sense.clear()
zero_pressure = sense.get_pressure()
while not conf.shutdown:
# Adjust ground pressure in case of anomaly
if conf.state == "HALT":
zero_pressure = zero_pressure * .9 + sense.get_pressure() * .1
# Altimeter
data = dataobj.current_data
current_pressure = sense.get_pressure()
data["sensors"]["alt"] = get_altitude(
zero_pressure, sense.get_pressure(),
sense.get_temperature()) # meters
data["sensors"]["hum"] = sense.get_humidity() # %
data["sensors"]["temp"] = (sense.get_temperature() * 9 / 5) + 32 # F
data["sensors"]["pres"] = current_pressure
conf.data.add_dp(current_pressure - conf.data.last_pressure)
conf.data.last_pressure = current_pressure
# IMU
data["sensors"]["acc"] = sense.get_accelerometer_raw() # Gs
data["sensors"]["pitch"] = sense.get_accelerometer()[
"pitch"] # degrees
data["sensors"]["yaw"] = sense.get_accelerometer()["yaw"] # degrees
data["sensors"]["roll"] = sense.get_accelerometer()["roll"] # degrees
data["sensors"]["compass"] = sense.get_compass() # rad/sec
data["sensors"]["gyro"] = sense.get_gyroscope_raw() # rad/sec
data["sensors"]["mag"] = sense.get_compass_raw() # microteslas
def main():
sense = SenseHat()
sense.set_imu_config(True, True, True)
gyro = sense.get_gyroscope_raw()
acc = sense.get_accelerometer_raw()
comp = sense.get_compass_raw()
temp = sense.temp
humidity = sense.humidity
pressure = sense.pressure
data = [{
"id": "gyro",
"values": [gyro['x'], gyro['y'], gyro['z']]
}, {
"id": "acc",
"values": [acc['x'], acc['y'], acc['z']]
}, {
"id": "comp",
"values": [comp['x'], comp['y'], comp['z']]
}, {
"id": "temp",
"values": [temp]
}, {
"id": "humidity",
"values": [humidity]
}, {
"id": "pressure",
"values": [pressure]
}]
print(json.dumps(data))
class SenseLogger:
def __init__(self):
self.sense = SenseHat()
self.filename = "./logs/Senselogg-" + str(datetime.now()) + ".csv"
self.file_setup(self.filename)
def write_line(self, line):
with open(self.filename, "a") as f:
f.write(line + "\n")
def log_data(self):
sense_data = self.get_sense_data()
line = ",".join(str(value) for value in sense_data)
self.write_line(line)
def file_setup(self, filename):
header = [
"datetime", "temp_h", "temp_p", "humidity", "pressure", "pitch",
"roll", "yaw", "mag_x", "mag_y", "mag_z", "accel_x", "accel_y",
"accel_z", "gyro_x", "gyro_y", "gyro_z"
]
with open(filename, "w") as f:
f.write(",".join(str(value) for value in header) + "\n")
def get_sense_data(self):
sense_data = []
sense_data.append(datetime.now())
sense_data.append(self.sense.get_temperature_from_humidity())
sense_data.append(self.sense.get_temperature_from_pressure())
sense_data.append(self.sense.get_humidity())
sense_data.append(self.sense.get_pressure())
o = self.sense.get_orientation()
yaw = o["yaw"]
pitch = o["pitch"]
roll = o["roll"]
sense_data.extend([pitch, roll, yaw])
mag = self.sense.get_compass_raw()
x = mag["x"]
y = mag["y"]
z = mag["z"]
sense_data.extend([x, y, z])
acc = self.sense.get_accelerometer_raw()
x = acc["x"]
y = acc["y"]
z = acc["z"]
sense_data.extend([x, y, z])
gyro = self.sense.get_gyroscope_raw()
x = gyro["x"]
y = gyro["y"]
z = gyro["z"]
sense_data.extend([x, y, z])
return sense_data
class SensePublisher:
def __init__(self):
self.sense = SenseHat()
self.sense.clear()
self.pub_humidity = rospy.Publisher('sensehat/humidity',
Float64,
queue_size=10)
self.pub_temperature = rospy.Publisher('sensehat/temperature',
Float64,
queue_size=10)
self.pub_pressure = rospy.Publisher('sensehat/pressure',
Float64,
queue_size=10)
self.pub_accelerometer = rospy.Publisher('sensehat/accelerometer',
Vector3,
queue_size=10)
self.pub_gyroscope = rospy.Publisher('sensehat/gyroscope',
Vector3,
queue_size=10)
self.pub_magnetometer = rospy.Publisher('sensehat/magnetometer',
Vector3,
queue_size=10)
self.pub_compass = rospy.Publisher('sensehat/compass',
Float64,
queue_size=10)
self.pub_stick = rospy.Publisher('sensehat/stick',
SenseInputEvent,
queue_size=10)
def publish(self):
self.pub_humidity.publish(self.sense.get_humidity())
self.pub_temperature.publish(self.sense.get_temperature())
self.pub_pressure.publish(self.sense.get_pressure())
acceleration = self.sense.get_accelerometer_raw()
self.pub_accelerometer.publish(
Vector3(acceleration['x'], acceleration['y'], acceleration['z']))
gyroscope = self.sense.get_gyroscope_raw()
self.pub_gyroscope.publish(
Vector3(gyroscope['x'], gyroscope['y'], gyroscope['z']))
compass = self.sense.get_compass_raw()
self.pub_magnetometer.publish(
Vector3(compass['x'], compass['y'], compass['z']))
self.pub_compass.publish(self.sense.get_compass())
stickEvents = self.sense.stick.get_events()
if len(stickEvents) > 0:
event = SenseInputEvent(stickEvents[-1].direction,
stickEvents[-1].action)
self.pub_stick.publish(event)
def turn_off(self):
self.sense.clear()
def run(self):
rospy.init_node('sense_hat', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
self.publish()
rate.sleep()
class SenseLogger:
def __init__(self):
self.sense = SenseHat()
self.filename = "./logs/Senselogg-"+str(datetime.now())+".csv"
self.file_setup(self.filename)
def write_line(self, line):
with open(self.filename, "a") as f:
f.write(line + "\n")
def log_data(self):
sense_data = self.get_sense_data()
line = ",".join(str(value) for value in sense_data)
self.write_line(line)
def file_setup(self, filename):
header = ["datetime", "temp_h", "temp_p", "humidity", "pressure", "pitch",
"roll", "yaw", "mag_x", "mag_y", "mag_z",
"accel_x", "accel_y", "accel_z",
"gyro_x", "gyro_y", "gyro_z"]
with open(filename, "w") as f:
f.write(",".join(str(value) for value in header)+ "\n")
def get_sense_data(self):
sense_data = []
sense_data.append(datetime.now())
sense_data.append(self.sense.get_temperature_from_humidity())
sense_data.append(self.sense.get_temperature_from_pressure())
sense_data.append(self.sense.get_humidity())
sense_data.append(self.sense.get_pressure())
o = self.sense.get_orientation()
yaw = o["yaw"]
pitch = o["pitch"]
roll = o["roll"]
sense_data.extend([pitch, roll, yaw])
mag = self.sense.get_compass_raw()
x = mag["x"]
y = mag["y"]
z = mag["z"]
sense_data.extend([x, y, z])
acc = self.sense.get_accelerometer_raw()
x = acc["x"]
y = acc["y"]
z = acc["z"]
sense_data.extend([x, y, z])
gyro = self.sense.get_gyroscope_raw()
x = gyro["x"]
y = gyro["y"]
z = gyro["z"]
sense_data.extend([x, y, z])
return sense_data
class IMU:
def __init__(self):
self.sense = SenseHat()
self.sense.clear()
self.zero_pressure = self.sense.get_pressure()
def get_accelerometer():
return self.sense.get_accelerometer()
def get_compass():
return self.sense.get_compass()
def get_accerometer_raw():
return self.sense.get_accelerometer_raw()
def get_gyroscope_raw():
return self.sense.get_gyroscope_raw()
def get_compass_raw():
return self.sense.get_compass_raw()
class SenseHAT(Block, EnrichSignals):
env = ObjectProperty(EnvironmentalSensors,
title='Environmental Sensors',
order=0)
imu = ObjectProperty(IMUsensor, title='IMU Sensor', order=1)
version = VersionProperty('0.1.0')
def __init__(self):
super().__init__()
self.hat = None
def configure(self, context):
super().configure(context)
self.hat = SenseHat()
self.hat.set_imu_config(self.imu().accel(),
self.imu().compass(),
self.imu().gyro())
def process_signals(self, signals):
data = {}
if self.imu().accel():
data['accelerometer'] = self.hat.get_accelerometer_raw()
if self.imu().compass():
data['compass'] = self.hat.get_compass_raw()
if self.imu().gyro():
data['gyroscope'] = self.hat.get_gyroscope_raw()
if self.env().rh():
data['relative_humidity'] = self.hat.get_humidity()
if self.env().temp():
data['temperature_C'] = self.hat.get_temperature()
if self.env().press():
data['pressure_mbar'] = self.hat.get_pressure()
outgoing_signals = []
for signal in signals:
outgoing_signals.append(self.get_output_signal(data, signal))
self.notify_signals(outgoing_signals)
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}]
def get_sense_data():
sense = SenseHat()
# Define list
sense_data = []
# Append sensor data
sense_data.append(sense.get_temperature_from_humidity())
sense_data.append(sense.get_temperature_from_pressure())
sense_data.append(sense.get_pressure())
sense_data.append(sense.get_humidity())
# Get orientation from sensor
yaw,pitch,roll = sense.get_orientation().values()
sense_data.extend([pitch,roll,yaw])
# Get magnetic field (compass)
mag_x,mag_y,mag_z = sense.get_compass_raw().values()
sense_data.extend([mag_x,mag_y,mag_z])
# Get accelerometer values
x,y,z = sense.get_accelerometer_raw().values()
sense_data.extend([x,y,z])
# Get Gyro values
gyro_x,gyro_y,gyro_z = sense.get_gyroscope_raw().values()
sense_data.extend([gyro_x,gyro_y,gyro_z])
# Add capture time
sense_data.append(datetime.now())
# Add precise time
sense_data.append(int(round(time.time()*1000)))
return sense_data
pwm.start(0)
drive1 = 0
drive2 = 0
motorPwm = 0
# GPIO.output(11, False) # Stop motor
# GPIO.output(12, False)
# pwm=GPIO.PWM(7, 100)
# pwm.start(0)
pwm.ChangeDutyCycle(100)
GPIO.output(7, True)
print("Running")
# for i in range(1,101):
for i in range(1,11):
gyroRaw = sense.get_gyroscope_raw()
accelRaw = sense.get_accelerometer_raw()
magRaw = sense.get_compass_raw()
temp = sense.get_temperature()
# drive_motor(100)
print("Stopping")
GPIO.output(12, False)
GPIO.output(11, False)
####
# Main game loop
####
prevUpdateTime = time.time() - startTime
sense.clear()
while True:
elapsedTime = time.time() - startTime
#!/usr/bin/python3 -u
import time
import quaternion
from madgwick import MadgwickAHRS
from sense_hat import SenseHat
heading = MadgwickAHRS()
sense = SenseHat()
while True:
gyro = [value for key, value in sense.get_gyroscope_raw().items()]
accel = [value for key, value in sense.get_accelerometer_raw().items()]
compass = [value for key, value in sense.get_compass_raw().items()]
heading.update(gyro, accel, compass)
ahrs = heading.quaternion.to_euler_angles()
roll = ahrs[0]
pitch = ahrs[1]
yaw = ahrs[2]
#(p,r,y) = heading.quaternion.to_euler_angles()
print(pitch, roll, yaw)
time.sleep(0.1)
SH_orientation_z = SH_orientation.get('z')
# Magnetometer data
#sense.set_imu_config(True,False,False)
time.sleep(0.01) # sleep for 10 ms after changing IMU configuration
SH_compass_north = sense.get_compass() # direction of magnetometer from North, in degrees
SH_compass_raw = sense.get_compass_raw() # magnetic intensity of x, y, z axes in microteslas
SH_compass_raw_x = SH_compass_raw.get('x')
SH_compass_raw_y = SH_compass_raw.get('y')
SH_compass_raw_z = SH_compass_raw.get('z')
# Gyro Data
#sense.set_imu_config(False,True,False)
time.sleep(0.01) # sleep for 10 ms after changing IMU configuration
#SH_gyro = sense.get_gyroscope() # orientation of pitch, roll, yaw axes in degrees
SH_gyro_raw = sense.get_gyroscope_raw() # rotational velocity of pitch, roll, yaw axes in radians per sec
SH_gyro_raw_x = SH_gyro_raw.get('x')
SH_gyro_raw_y = SH_gyro_raw.get('y')
SH_gyro_raw_z = SH_gyro_raw.get('z')
# Accelerometer data
#sense.set_imu_config(False,False,True)
time.sleep(0.01) # sleep for 10 ms after changing IMU configuration
#SH_accel = sense.get_accelerometer() # orientation of pitch, roll, yaw axes in degrees
SH_accel_raw = sense.get_accelerometer_raw() # acceleration intensity of pitch, roll, yaw axes in 'G's
SH_accel_raw_x = SH_accel_raw.get('x')
SH_accel_raw_y = SH_accel_raw.get('y')
SH_accel_raw_z = SH_accel_raw.get('z')
## Readings from the BMP180 installed in the sealed box
BMP_pressure = BMP_sensor.read_pressure() # value in Pascals
#gyro and accel
file = raw_input('Enter file name: ')
file = "./MotionCaptures/" + file + ".csv"
f = open(file, "w")
# Progress indicator
sense.set_pixel(0, 0, 0, 255, 0)
#green light to indicate ready
flag = True
while flag:
# take data measurements
accel = sense.get_accelerometer_raw()
orient = sense.get_gyroscope_raw()
# access variables
x = accel['x']
y = accel['y']
z = accel['z']
p = orient['x']
r = orient['y']
y = orient['z']
# write to CSV
f.write("{0},{1},{2},{3},{4},{5}\n".format(x, y, z, p, r, y))
#check for joystick event to stop gathering data
for event in sense.stick.get_events():
if event.action == ACTION_PRESSED:
#get pitch, roll and yaw orientation from gyro
o = sense.get_orientation()
gyro_pitch = o['pitch']
gyro_roll = o['roll']
gyro_yaw = o['yaw']
#get acceleration
acc_x, acc_y, acc_z = sense.get_accelerometer_raw().values()
acc_roll, acc_pitch, acc_yaw = sense.get_accelerometer().values()
#get magnetometer values
mag_x, mag_y, mag_z = sense.get_compass_raw().values()
compass = sense.get_compass()
#get gyro velocity
gyro_vel = sense.get_gyroscope_raw()
gyro_vel_x = gyro_vel['x']
gyro_vel_y = gyro_vel['y']
gyro_vel_z = gyro_vel['z']
#get humidity
humidity = sense.get_humidity()
#get temperature
temperature = sense.get_temperature()
#get pressure
pressure = sense.get_pressure()
if (datetime.now() > last_catch + timedelta(seconds=12)):
add_csv_data(
async def handle(self, context: RequestContext, responder: BaseResponder):
"""
Message handler logic for basic messages.
Args:
context: request context
responder: responder callback
"""
self._logger.debug(f"ReadSensorHandler called with context {context}")
assert isinstance(context.message, ReadSensor)
self._logger.info("Received read sensor: %s", context.message.sensors)
sensors = context.message.sensors
meta = {"sensors": sensors}
conn_mgr = ConnectionManager(context)
await conn_mgr.log_activity(
context.connection_record,
"read_sensor",
context.connection_record.DIRECTION_RECEIVED,
meta,
)
await responder.send_webhook(
"read_sensor",
{
"message_id": context.message._id,
"sensors": sensors,
"state": "received"
},
)
sense = SenseHat()
temperature = None
humidity = None
pressure = None
orientation = None
accelerometer = None
compass = None
gyroscope = None
stick_events = None
pixels = None
if "temperature" in sensors:
temperature = sense.get_temperature()
if "humidity" in sensors:
humidity = sense.get_humidity()
if "pressure" in sensors:
pressure = sense.get_pressure()
if "orientation" in sensors:
orientation = sense.get_orientation_degrees()
if "accelerometer" in sensors:
accelerometer = sense.get_accelerometer_raw()
if "compass" in sensors:
compass = sense.get_compass_raw()
if "gyroscope" in sensors:
gyroscope = sense.get_gyroscope_raw()
if "stick_events" in sensors:
stick_events = []
stick_event_objects = sense.stick.get_events()
for event in stick_event_objects:
event_dict = {}
event_dict['timestamp'] = event.timestamp
event_dict['direction'] = event.direction
event_dict['action'] = event.action
stick_events.append(event_dict)
if "pixels" in sensors:
pixels = sense.get_pixels()
reply_msg = SensorValue(temperature=temperature,
humidity=humidity,
pressure=pressure,
orientation=orientation,
accelerometer=accelerometer,
compass=compass,
gyroscope=gyroscope,
stick_events=stick_events,
pixels=pixels)
await responder.send_reply(reply_msg)
await conn_mgr.log_activity(
context.connection_record,
"sensor_value",
context.connection_record.DIRECTION_SENT,
{"content": "reply"},
)
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'] )
print( "x: %.4f" % sensehat.get_accelerometer_raw( )['x']
+ " , y: %.4f" % sensehat.get_accelerometer_raw( )['y']
+ " , Z: %.4f" % sensehat.get_accelerometer_raw( )['z'] )
print time.strftime( "%Y-%m-%d %I:%M:%S %p %Z" , time.localtime( ) )
class Host(object):
def __init__(self,
rotation = 0,
low_light = False,
calibration = {},
smoothing = 0,
register_services = True,
environmental_publishing = False,
environmental_publishing_rate = 1,
imu_publishing = True,
#imu_publishing_mode = "get_gyroscope_rpy",
imu_publishing_mode = "get_sensor_msg",
imu_publishing_config = "1|1|1",
imu_publishing_rate = 0.01,
stick_publishing = False,
stick_sampling = 0.2):
"""Constructor method"""
#IMU_FRAME = rospy.get_param('~imu_frame', 'imu_link')
IMU_FRAME="imu_frame"
# Init sensor
self.sense = SenseHat()
self.sense.clear(0,0,0)
self.sense.set_rotation(rotation)
self.sense.low_light = low_light
self.measures = {
'humidity': self.sense.get_humidity,
'temperature_from_humidity': self.sense.get_temperature_from_humidity,
'temperature_from_pressure': self.sense.get_temperature_from_pressure,
'pressure': self.sense.get_pressure,
'compass': self.sense.get_compass,
}
# Init parameters
self.imu_publishing = imu_publishing
self.imu_publishing_mode = imu_publishing_mode
self.imu_publishing_rate = imu_publishing_rate
self.history = {}
for measure in self.measures:
self.history[measure] = collections.deque([], maxlen = smoothing if smoothing > 0 else None) if smoothing >= 0 else None
# Init Lock to serialize access to the LED Matrix
self.display_lock = Lock()
# Register publishers
if self.imu_publishing:
self.sense.set_imu_config(*[i=="1" for i in imu_publishing_config.split("|")])
self.imu_pub = rospy.Publisher("imu", Imu, queue_size=10)
rospy.loginfo("sensehat_ros initialized")
def __del__(self):
if self.sense:
self.sense.clear(0,0,0)
rospy.loginfo("sensehat_ros destroyed")
##############################################################################
# Private methods
##############################################################################
def _get_sensor_msg_imu(self, timestamp):
# 1 g-unit is equal to 9.80665 meter/square second. # Linear Acceleration
# 1° × pi/180 = 0.01745rad # Angular velocity # Gyro
gunit_to_mps_squ = 9.80665
msg = Imu()
msg.header.stamp = timestamp
msg.header.frame_id = IMU_FRAME
gyr = self.sense.get_gyroscope_raw()
acc = self.sense.get_accelerometer_raw()
msg.orientation.x = 0.0
msg.orientation.y = 0.0
msg.orientation.z = 0.0
msg.orientation.w = 0.0
msg.orientation_covariance = [-1, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0]
#msg.angular_velocity.x = gyr['x'] * np.pi
msg.angular_velocity.x = round( gyr['x'] * ( np.pi / 180 ), 8 ) * (-1) # Inverted x axis
msg.angular_velocity.y = round( gyr['y'] * ( np.pi / 180 ), 8 )
msg.angular_velocity.z = round( gyr['z'] * ( np.pi / 180 ), 8 )
msg.angular_velocity_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg.linear_acceleration.x = round((acc['x'] * gunit_to_mps_squ), 8 ) * (-1) # Inverted x axis
msg.linear_acceleration.y = round((acc['y'] * gunit_to_mps_squ), 8 )
msg.linear_acceleration.z = round((acc['z'] * gunit_to_mps_squ), 8 )
#msg.linear_acceleration.x = np.radians( acc['x'] )
#msg.linear_acceleration.y = np.radians( acc['y'] )
#msg.linear_acceleration.z = np.radians( acc['z'] )
msg.linear_acceleration_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
return msg
def _publish_sensor_msg_imu(self, event):
imu = self._get_sensor_msg_imu(rospy.Time.now())
rospy.logdebug( "Publishing IMU" )
br = tf2_ros.TransformBroadcaster()
self.imu_pub.publish(imu)
##############################################################################
# Run
##############################################################################
def run(self):
if self.imu_publishing:
# Start publishing events
rospy.Timer(rospy.Duration(self.imu_publishing_rate), self._publish_sensor_msg_imu)
rospy.loginfo("sensehat_ros publishing Imu")
rospy.spin()
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()
print(start)
if real_temp < 26.7 or real_temp > 18.3 or real_humidity < 70 or real_humidity > 40:
sense.set_pixels(happy_face)
if real_temp > 26.7 or real_temp < 18.3 or real_humidity > 70 or real_humidity < 40:
sense.set_pixels(sad_face)
acceleration = sense.get_accelerometer_raw()
AcX = acceleration["x"]
AcY = acceleration["y"]
AcZ = acceleration["z"]
gyroscope = sense.get_gyroscope_raw()
GyX = gyroscope["x"]
GyY = gyroscope["y"]
GyZ = gyroscope["z"]
if start == 54:
start = 0
magnetometer = sense.get_compass_raw()
MaX = magnetometer["x"]
MaY = magnetometer["y"]
MaZ = magnetometer["z"]
magnetometer_equation = sqrt(MaX * MaX + MaY * MaY + MaZ * MaZ)
"Time Stamp (Accelerometer), X (Degrees), Y (Degrees), Z (Degrees)\n")
# Get a start time for the test
start = time.time()
# Activate the gyro, disables the compass and accelerometer
sense.set_imu_config(False, True, False)
# Loop to write a bunch of the current pressure readings for the duration
# of the test
while time.time() - start < lengthOfTest:
# Retrieve the pressure from the SenseHat
pressure = sense.get_pressure() / 68.9476
# Retrieve the raw GYRO readings. This function returns a dictionary indexed with strings x, y, z.
# The values are floats representing rotational intensity of the axis in RADIANS PER SECOND.
rawGYRO = sense.get_gyroscope_raw()
gyro = sense.get_gyroscope()
rawAccel = sense.get_accelerometer_raw()
accel = sense.get_accelerometer()
now = time.time() - start
file.write(
str(now) + "," + str(pressure) + ",," + str(now) + "," +
str(rawGYRO.get("x")) + "," + str(rawGYRO.get("y")) + "," +
str(rawGYRO.get("z")) + ",," + str(now) + "," +
str(gyro.get("pitch")) + "," + str(gyro.get("roll")) + "," +
str(gyro.get("yaw")) + ",," + str(now) + "," + str(rawAccel.get("x")) +
"," + str(rawAccel.get("y")) + "," + str(rawAccel.get("z")) + ",," +
str(now) + "," + str(accel.get("pitch")) + "," +
str(accel.get("roll")) + "," + str(accel.get("yaw")) + '\n')
if GPIO.input(LEFT) == False:
Back() #If the left button (from the 4 button diamond shape arangement) is pressed the Back() function will run
if GPIO.input(RIGHT) == False:
Forwards() #If the right button is pressed then the Forwards() function will run
if GPIO.input(UP) == False:
Plus() #If the up button (the one at the top) is pressed then the Plus() function will run
if GPIO.input(DOWN) == False:
Less() #If the down button (the one at the bottom) is pressed then the Less() function will run
if GPIO.input(A) == False:
Prev() #If button A (the left button of the pair of buttons beneath the first 4) is pressed then the Prev() function will run
if GPIO.input(B) == False:
Next(1) #If button B (to the right of button A) is pressed then the Next(1) function will run
#shake routines to call prev and next
pitch, roll, yaw = sense.get_gyroscope_raw().values()
if yaw > threshold:
pitch = round(pitch, 1) #This sets the variable 'pitch' to the gyroscope measurement of pitch rounded to one place
roll = round(roll, 1) #This sets the variable 'roll' to the gyroscope measurement of roll rounded to one place
yaw = round(yaw, 1) #This sets the variable 'yaw' to the gyroscope measurement of yaw rounded to one place
print ("%s %s %s" % (pitch, roll, yaw))
sense.set_pixels(less)
Less() #If the MP3 Music Player is tilted right from a 90 degrees position with the screen facing the user the Less() function will run
if yaw < -threshold:
pitch = round(pitch, 1)
roll = round(roll, 1)
yaw = round(yaw, 1)
print ("%s %s %s" % (pitch, roll, yaw))
sense.set_pixels(plus)
Plus() #If the MP3 Music Player is tilted left from a 90 degrees position with the screen facing the user the Plus() function will run
#Could be used to replicate other functionality
start = time.time()
current = time.time()
i = 0
video = 0
#SET MAX LOG DURATION IN SECONDS
while (current-start) < 5:
current = time.time()
t = sense.get_temperature()
p = sense.get_pressure()
h = sense.get_humidity()
pitch, roll, yaw = sense.get_orientation().values()
xc, yc, zc = sense.get_compass_raw().values()
xg, yg, zg = sense.get_gyroscope_raw().values()
xa, ya, za = sense.get_accelerometer_raw().values()
f = open('./hat-log/hat.csv', 'a', os.O_NONBLOCK)
line = "%d, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n" % (1000*time.time(),t,p,h,pitch,roll,yaw,xc,yc,zc,xg,yg,zg,xa,ya,za)
f.write(line)
f.flush
f.close()
#set za threshold to the number of g you would expect at launch
if video == 0 and (za > 1.1 or za < -1.1):
video = 1
call(["./video", ""])
#print i
i = i+1
start = time.time()
current = time.time()
i = 0
video = 0
#SET MAX LOG DURATION IN SECONDS
while (current - start) < 5:
current = time.time()
t = sense.get_temperature()
p = sense.get_pressure()
h = sense.get_humidity()
pitch, roll, yaw = sense.get_orientation().values()
xc, yc, zc = sense.get_compass_raw().values()
xg, yg, zg = sense.get_gyroscope_raw().values()
xa, ya, za = sense.get_accelerometer_raw().values()
f = open('./hat-log/hat.csv', 'a', os.O_NONBLOCK)
line = "%d, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n" % (
1000 * time.time(), t, p, h, pitch, roll, yaw, xc, yc, zc, xg, yg, zg,
xa, ya, za)
f.write(line)
f.flush
f.close()
#set za threshold to the number of g you would expect at launch
if video == 0 and (za > 1.1 or za < -1.1):
video = 1
call(["./video", ""])
sense = SenseHat()
sense.set_imu_config(True, True, True)
# Python SenseHat API documentation:
# https://pythonhosted.org/sense-hat/api
# Roll is: angular x.
# Pitch is: angular y.
# Yaw is: angular z.
data_imu = {}
data_imu["gyro"] = sense.get_orientation_degrees(
) # Gets orientation (3-ang-axis) in [deg].
data_imu["acc"] = sense.get_accelerometer(
) # Gets orientation from the accelerometer (3-ang-axis) in [deg].
data_imu["gyror"] = sense.get_gyroscope_raw(
) # Gets angular velocity (3-axis) in [rad/s].
data_imu["mag"] = sense.get_compass() # Gets orientation to North in [deg].
data_imu["magr"] = sense.get_compass_raw(
) # Gets magnetic field (3-axis) in [µT].
data_imu["accr"] = sense.get_accelerometer_raw(
) # Gets acceleration (3-axis) in [g]'s.
data_env = {}
data_env["temph"] = sense.get_temperature_from_humidity(
) # Gets temperature from humidity sensor in [ºC].
data_env["tempp"] = sense.get_temperature_from_pressure(
) # Gets temperature from pressure sensor in [ºC].
data_env["pres"] = sense.get_pressure() # Gets pressure in [mbar].
data_env["hum"] = sense.get_humidity() # Gets relative humidity in [%].
cpu_temp = os.popen("vcgencmd measure_temp").readline()
data_env["tempcpu"] = float(cpu_temp.replace("temp=", "").replace("'C\n", ""))
class EasyAHRS:
def __init__(self,senseHatNum=1,timeInit=0):
#####
# Predetermined Mag Calibraton
####
# Sense Hat 1
# xBias = -30.02851629257202
# yBias = 7.201869010925293 +1.4395752679386042
# xSF = 0.6773587707778925
# ySF = 0.7209744740877911
# Sense Hat 2
self.xBias = 2.701296329498291
self.yBias = 19.79104995727539
self.xSF = 0.6955512793747712
self.ySF = 0.7164366008777275
#
# AHRS Smoothing filter time constant
self.tau = 1/200
self.rates = np.array([0.0,0.0,0.0])
self.accel = np.array([0.0,0.0,0.0])
self.mag = np.array([0.0,0.0,0.0])
self.att = np.array([0.0,00.0,0.0])
self.sense = SenseHat()
self.senseHatNum = senseHatNum
if timeInit>0:
self.startTime = timeInit
else:
self.startTime = time.time()
print("initialized {0}".format(self.senseHatNum))
def warmup(self):
print("Warming up ...")
for i in range(1,101):
gyroRaw = self.sense.get_gyroscope_raw()
accelRaw = self.sense.get_accelerometer_raw()
magRaw = self.sense.get_compass_raw()
temp = self.sense.get_temperature()
print("Warm up complete")
def align(self, alignSamples=100):
print("Aligning {0} Samples".format(alignSamples))
for i in range(1,alignSamples+1):
# elapsedTime = time.time() - startTime
gyroRaw = self.sense.get_gyroscope_raw()
accelRaw = self.sense.get_accelerometer_raw()
magRaw = self.sense.get_compass_raw()
temp = self.sense.get_temperature()
magRaw["x"] = self.xSF *(magRaw["x"] - self.xBias)
magRaw["y"] = self.ySF *(magRaw["y"] - self.yBias)
self.accel = self.accel + \
1/alignSamples*np.array([accelRaw["x"], accelRaw["y"],accelRaw["z"]])
self.rates = self.rates + \
1/alignSamples*np.array([gyroRaw["x"], gyroRaw["y"],gyroRaw["z"]])
self.mag = self.mag + \
1/alignSamples*np.array([magRaw["x"], magRaw["y"],magRaw["z"]])
print("Accel: ", self.accel)
print("Gyro: ", self.rates)
print("Mag: ", self.mag)
# Establish initial prich roll from averaged accel measures
self.att[1] = asin(-1*self.accel[0])
print("Pitch {0}".format(self.att[1]))
print("Pitch {0}".format(asin(-1*self.accel[0])))
self.att[0] = atan2(self.accel[1],1.0)
print(self.att)
Ca = self.euler2C(self.att[1],self.att[0],0.0)
print(Ca)
# Establish initial heading from horizontal mag measuremetns
magL = Ca.dot(self.mag.transpose())
self.att[2] = atan2(-1*magL[1],magL[0])
# Initialize Gyro, Accel, and Filtered to Level transitions
C_AL = self.euler2C(self.att[1],self.att[0],self.att[2])
C_GL = self.euler2C(self.att[1],self.att[0],self.att[2])
C_FL = self.euler2C(self.att[1],self.att[0],self.att[2])
# Initial Gyro bias is the average over sample period
self.gyroBias = self.rates
print("Align Attitude:")
print(self.att*degrees(1.0))
print("Align Gyro Bias")
print(self.gyroBias*degrees(1.0))
"""
" c2euler
" This routine converts a C matrix represention to euler representation
" Input: C matrix
" Output: pitch roll yaw
"""
def euler2C(self,pitch,roll,yaw):
C = np.zeros( (3,3) )
cTheta = cos(pitch)
sTheta = sin(pitch)
cPhi = cos(roll)
sPhi = sin(roll)
cPsi = cos(yaw)
sPsi = sin(yaw)
C[0,0] = cTheta*cPsi
C[0,1] = -1*cPhi*sPsi + sPhi*sTheta*cPsi
C[0,2] = sPhi*sPsi + cPhi*sTheta*cPsi
C[1,0] = cTheta*sPsi
C[1,1] = cPhi*cPsi + sPhi*sTheta*sPsi
C[1,2] = -sPhi*cPsi + cPhi*sTheta*sPsi
C[2,0] = -sTheta
C[2,1] = sPhi*cTheta
C[2,2] = cPhi*cTheta
return C
"""
" c2euler
" This routine converts a C matrix represention to euler representation
" Input: C matrix
" Output: pitch roll yaw
"""
def c2euler(self,C):
pitch = -1*asin(C[2,0])
yaw = atan2(C[1,0],C[0,0])
roll = atan2(C[2,1],C[2,2])
return (pitch,roll,yaw)
"""
" ortho_norm
" This routine ortho normalizes a C matrix
" Input: C matrix
" Output: Ortho normed C matrix
"""
def ortho_norm(self,C):
x = C[:,0]
y = C[:,1]
z = C[:,2]
e = x.transpose()
e = e.dot(y)
eScalar = e/2
xOrtho = x - e*y
yOrtho = y - e*x
zOrtho = np.cross(xOrtho,yOrtho)
xNorm = 0.5*(3 - xOrtho.transpose().dot(xOrtho))*xOrtho
yNorm = 0.5*(3 - yOrtho.transpose().dot(yOrtho))*yOrtho
zNorm = 0.5*(3 - zOrtho.transpose().dot(zOrtho))*zOrtho
Con = np.column_stack( (xNorm,yNorm,zNorm) )
return Con
def update(self):
r180 = radians(180)
nr180 = radians(-180)
r360 = radians(360)
deltaTheta = np.array([0, 0 , 0])
accumTheta = np.array([0, 0 , 0])
omega = np.array( [0, 0, 0] )
deltaC = np.ones( (3,3) )
prevTime = time.time() - startTime
# The stuff above should go somewhere else
elapsedTime = time.time() - self.startTime
gyroRaw = sense.get_gyroscope_raw()
accelRaw = sense.get_accelerometer_raw()
magRaw = sense.get_compass_raw()
temp = sense.get_temperature()
# logFile.write("0,{0},{x},{y},{z}\r\n".format(elapsedTime,**gyroRaw)) # rad/sec
# logFile.write("1,{0},{x},{y},{z}\r\n".format(elapsedTime,**accelRaw)) # Gs
# logFile.write("2,{0},{x},{y},{z}\r\n".format(elapsedTime,**magRaw)) # microT
# logFile.write("7,{0},{1}\r\n".format(elapsedTime,temp))
magRaw["x"] = xSF *(magRaw["x"] - xBias)
magRaw["y"] = ySF *(magRaw["y"] - yBias)
deltaTime = elapsedTime - prevTime
# print("0,{0},{x},{y},{z}".format(elapsedTime,**gyroRaw)) # rad/sec
# print("1,{0},{x},{y},{z}".format(elapsedTime,**accelRaw)) # Gs
# print("2,{0},{x},{y},{z}".format(elapsedTime,**magRaw))
# Accumplate Rates
omega = np.array( (gyroRaw["x"], gyroRaw["y"], gyroRaw["z"]) ) - gyroBias
deltaTheta = omega*deltaTime # Basic Integration
accumTheta = accumTheta + deltaTheta
# print("Dt {0} {1} {2}".format(degrees(accumTheta[0]),degrees(accumTheta[1]),degrees(accumTheta[2])))
# Form the deltaC matrix from the angular rotations
# Single taylor series with small angle assumption
deltaC[0,1] = -1*deltaTheta[2]
deltaC[0,2] = deltaTheta[1]
deltaC[1,0] = deltaTheta[2]
deltaC[1,2] = -1*deltaTheta[0]
deltaC[2,0] = -1*deltaTheta[1]
deltaC[2,1] = deltaTheta[0]
# Update the gyro and filter solution
C_GL = C_GL.dot(deltaC)
C_FL = C_FL.dot(deltaC)
# Determine the Accel Solution
accelPitch = asin(-1*accelRaw["x"])
accelRoll = atan2(accelRaw["y"],1.0)
accelMag = np.array([magRaw["x"], magRaw["y"],magRaw["z"]])
C_AL = euler2C(accelPitch,accelRoll,0.0)
# print(Ca)
magL = C_AL.dot(accelMag.transpose())
# print(magL)
yaw = atan2(-1*magL[1],magL[0])
C_AL = euler2C(accelPitch,accelRoll,yaw)
# Update the filtered solution with the data from the
# Accel based solution
(pf,rf,yf) = c2euler(C_FL)
pf = (1-tau)*pf + tau*accelPitch
rf = (1-tau)*rf + tau*accelRoll
ey = yf-yaw
if ey > r180:
ey = ey - r360
elif ey < -r180:
ey = ey + r360
yf = (1-tau)*yf + tau*(yf - ey)
if yf > r180:
yf = yf-r360
elif yf < -r180:
yf = yf+r360
C_FL = euler2C(pf,rf,yf)
# print("Att --> {0} {1} {2}".format(degrees(pf),degrees(rf),degrees(yf)))
prevTime = elapsedTime
# TODO write header
while True:
# Check for end time
now = datetime.now()
if now >= end_time:
logger.info(f'Finished run at {now}')
break
# Main loop
try:
orientation = sh.get_orientation_degrees()
compass = sh.get_compass()
compass_raw = sh.get_compass_raw()
gyro = sh.get_gyroscope()
gyro_raw = sh.get_gyroscope_raw()
accelerometer_raw = sh.get_accelerometer_raw()
camera.capture(debug_capture=True)
util.add_csv_data(csvfile, (
now,
sh.get_humidity(),
sh.get_temperature(),
sh.get_temperature_from_humidity(),
sh.get_temperature_from_pressure(),
sh.get_pressure(),
orientation['roll'],
orientation['pitch'],
orientation['yaw'],
compass,
compass_raw['x'],
def get_gyroscope_raw():
sense = SenseHat()
sense.set_imu_config(False, True, False) # Gyroscope only
return sense.get_gyroscope_raw()
from quat_rotate import qv_mult
import numpy as np
# Get data
sense = SenseHat()
previousTime = -1
ACCELEROMETER_DRIFT_WHEN_STATIONARY = 0.00000000001
samplePeriod = 1 / 256
posX = 0
posY = 0
posZ = 0
while True:
# Get sensor data and the current time
gyro = sense.get_gyroscope_raw() # deg/s
acc = sense.get_accelerometer_raw() # g's (1g = 9.81 m/s^2)
mag = sense.get_compass_raw() # microteslas (uT)
currentTime = time()
# Calculate the magnitude of acceleration
accMagnitude = sqrt((acc['x'] * acc['x']) + (acc['y'] * acc['y']) +
(acc['z'] * acc['z']))
# Use a high-pass filter to remove some noise
filterCutoff = 0.001
butterFilterB, butterFilterA = signal.butter(
1, (2 * filterCutoff) / (1 / samplePeriod), 'highpass')
accMagnitudeFiltered = signal.filtfilt(butterFilterB,
butterFilterA,
[accMagnitude, accMagnitude],
class DataWrite:
def __init__(self):
self.sense = SenseHat()
self.sense.set_imu_config(True, True, True)
def get_data(self):
"""Gets data from environmental sensors and IMU sensors
and formats it for writing to a CSV with time as the first item
"""
# get environmental data from the sensehat
def get_enviro():
"""Gets environmental data and formats it in the form:
pressure, temperature_pressure, temperature_humidity, humidity
"""
# Get readings from each sensor
pressure = self.sense.get_pressure()
temp_press = self.sense.get_temperature_from_pressure()
temp_humid = self.sense.get_temperature_from_humidity()
humidity = self.sense.get_humidity()
# Format the readings
enviro_results = [pressure, temp_press, temp_humid, humidity]
return enviro_results
# get IMU data from the sensehat
def get_imu():
"""Gets IMU data and formats it in the form:
accelX, accelY, accelZ, gyroX, gyroY, gyroZ, compassX, compassY, compassZ, orientationX, orientationY,
orientationZ
"""
# get raw data from IMU sensors
accelraw = self.sense.get_accelerometer_raw()
gyroraw = self.sense.get_gyroscope_raw()
compassraw = self.sense.get_compass_raw()
orientationraw = self.sense.get_orientation_degrees()
# Format raw data into a usable list
imu_results = [
accelraw['x'], accelraw['y'], accelraw['z'], gyroraw['x'],
gyroraw['y'], gyroraw['z'], compassraw['x'], compassraw['y'],
compassraw['z'], orientationraw['pitch'],
orientationraw['roll'], orientationraw['yaw']
]
return imu_results
# Get data from sensors and add time then append together
enviro_res = get_enviro()
imu_res = get_imu()
current_time = datetime.datetime.now().strftime(
"%d-%b-%Y (%H:%M:%S.%f)")
results = [current_time]
results.extend(enviro_res)
results.extend(imu_res)
print(results)
return results
def write_data(self):
"""Writes data to data.csv in append mode as to not delete headers or previous data"""
with open('data.csv', 'a') as f:
writer = csv.writer(f)
writer.writerow(self.get_data())
class CiosRaspberryHat:
#
# Constructor
#
# url string WebsocketのURL
# channel_id string CiosのチャネルID
# access_token string Ciosのアクセストークン
#
def __init__(self, url, channel_id, access_token):
self.url = url
self.channel = channel_id
self.token = access_token
self.ws = 0
self.sense = SenseHat() # SenseHat インスタンス作成
self.sense.set_imu_config(True, True, True) # 加速度センサーの有効化
self.connectCount = 0
self.screen = "top"
#
# connection
#
# WebSocketへのコネクションを貼る
#
def connection(self):
try:
ws_url = self.url + "?" + "channel_id=" + self.channel + "&access_token=" + self.token
print("--------------- WebSocket Connection ---------------")
print("ConnectionURL: " + ws_url)
print("--------------- WebSocket Connection ---------------")
self.sense.show_letter("C", text_colour=[0, 255, 255])
self.ws = create_connection(ws_url)
except:
print("Websocket Connection Error...")
self.sense.show_letter("E", text_colour=[255, 0, 0])
self.errorReConnect()
return "error"
#
# getSensorData
#
# SenseHatからのデータを取得、JSON整形を行う
# return: Json String
#
def getSensorData(self):
try:
humidity = self.sense.get_humidity()
temp = self.sense.get_temperature()
pressure = self.sense.get_pressure()
orientation = self.sense.get_orientation_degrees()
compass = self.sense.get_compass_raw()
gyroscope = self.sense.get_gyroscope_raw()
accelerometer = self.sense.get_accelerometer_raw()
message = {
"humidity": humidity,
"temperature": temp,
"pressure": pressure,
"degrees_p": orientation["pitch"],
"degrees_r": orientation["roll"],
"degrees_y": orientation["yaw"],
"compass_x": compass["x"],
"compass_y": compass["y"],
"compass_z": compass["z"],
"gyroscope_x": gyroscope["x"],
"gyroscope_y": gyroscope["y"],
"gyroscope_z": gyroscope["z"],
"accelerometer_x": accelerometer["x"],
"accelerometer_y": accelerometer["y"],
"accelerometer_z": accelerometer["z"]
}
send_message = json.dumps(message)
return send_message
except:
print("getSensorData Error...")
self.ws.close()
self.sense.show_letter("E", text_colour=[255, 0, 0])
self.errorReConnect()
return "error"
#
# sendMessage
#
# message string 送信するメッセージ（Json形式）
#
def sendMessage(self, message):
try:
print("--------------- WebSocket Send Message ---------------")
print(message)
print("--------------- WebSocket Send Message ---------------")
self.ws.send(message)
except:
print("Websocket Send Error...")
self.ws.close()
self.sense.show_letter("E", text_colour=[255, 0, 0])
self.errorReConnect()
return "error"
#
# Error ReConnect WebSocket
#
# message string 送信するメッセージ（Json形式）
#
def errorReConnect(self):
try:
if self.connectCount > 0: # ErrorCount カウント数以上のエラーで停止
raise
self.connectCount += 1
for v in range(self.connectCount): # Error数に応じて、待機時間を追加
print("Wait ::: " + str(self.connectCount - v) + " sec")
time.sleep(1)
self.connection()
except:
print("Websocket connection Error count : " +
str(self.connectCount) + " Over")
self.sense.show_letter("E", text_colour=[255, 0, 0])
time.sleep(0.5)
self.sense.show_letter("E", text_colour=[0, 255, 0])
time.sleep(0.5)
self.sense.show_letter("E", text_colour=[0, 0, 255])
time.sleep(0.5)
self.sense.show_letter("E", text_colour=[255, 0, 0])
time.sleep(2)
self.sense.clear()
exit()
steps = 0
def sendRequest(url, paramas):
r = requests.post(url, headers={'identifier': getserial()}, data=paramas)
print(r)
while 1:
o = sense.get_orientation()
temp = sense.get_temperature()
humidity = sense.get_humidity()
acceleration = sense.get_accelerometer_raw()
gyrox = sense.get_gyroscope_raw()
x1 = abs(gyrox['x'])
x2 = abs(gyrox['y'])
x3 = abs(gyrox['z'])
x = acceleration['x']
y = acceleration['y']
z = acceleration['z']
if datetime.datetime.now() >= thirty:
# print("request : {0}".format(g))
try:
thread.start_new_thread(sendRequest,
('http://172.16.0.49:4242/informations', {
'temperature': temp,
