def imu_degrees():
imu = GY80()
imu.update()
# w, x, y, z = imu.current_orientation_quaternion_mag_acc_only()
w, x, y, z = imu._current_gyro_only_q
yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z)
return(yaw * 180.0 / pi + 180, pitch * 180.0 / pi + 180, roll * 180.0 / pi + 180)
def imu_degrees():
imu = GY80()
imu.update()
# w, x, y, z = imu.current_orientation_quaternion_mag_acc_only()
w, x, y, z = imu._current_gyro_only_q
yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z)
return (yaw * 180.0 / pi + 180, pitch * 180.0 / pi + 180,
roll * 180.0 / pi + 180)
def current_orientation_euler_angles_mag_acc_only(self):
"""Current orientation using yaw, pitch, roll (radians) using sensor's frame."""
return quaternion_to_euler_angles(*self.current_orientation_quaternion_mag_acc_only())
def current_orientation_euler_angles_mag_acc_only(self):
"""Current orientation using yaw, pitch, roll (radians) using sensor's frame."""
return quaternion_to_euler_angles(
*self.current_orientation_quaternion_mag_acc_only())
g = sqrt(x * x + y * y + z * z)
print("Magnitude of acceleration %0.2fg (%0.2f %0.2f %0.2f)" %
(g, x, y, z))
if abs(g - 1) > 0.3:
sys.stderr.write("Not starting from rest, acceleration %0.2f\n" % g)
sys.exit(1)
print("Starting q by acc/mag (%0.2f, %0.2f, %0.2f, %0.2f)" % imu._q_start)
try:
while True:
print()
imu.update()
#w, x, y, z = imu.current_orientation_quaternion_hybrid()
w, x, y, z = imu._current_hybrid_orientation_q
#print("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z))
yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z)
print("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f), "
"yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" %
(w, x, y, z, yaw * 180.0 / pi, pitch * 180.0 / pi,
roll * 180.0 / pi))
w, x, y, z = imu._current_gyro_only_q
#print("Gyro-only quaternion (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z))
yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z)
print("Gyro-only quaternion (%0.2f, %0.2f, %0.2f, %0.2f), "
"yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" %
(w, x, y, z, yaw * 180.0 / pi, pitch * 180.0 / pi,
roll * 180.0 / pi))
w, x, y, z = imu.current_orientation_quaternion_mag_acc_only()
#print("Accel/Comp quaternion (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z))
x, y, z = imu.read_accel()
g = sqrt(x*x + y*y + z*z)
print("Magnitude of acceleration %0.2fg (%0.2f %0.2f %0.2f)" % (g, x, y, z))
if abs(g - 1) > 0.3:
sys.stderr.write("Not starting from rest, acceleration %0.2f\n" % g)
sys.exit(1)
print("Starting q by acc/mag (%0.2f, %0.2f, %0.2f, %0.2f)" % imu._q_start)
try:
while True:
print()
imu.update()
#w, x, y, z = imu.current_orientation_quaternion_hybrid()
w, x, y, z = imu._current_hybrid_orientation_q
#print("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z))
yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z)
print("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f), "
"yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" % (w, x, y, z,
yaw * 180.0 / pi,
pitch * 180.0 / pi,
roll * 180.0 / pi))
w, x, y, z = imu._current_gyro_only_q
#print("Gyro-only quaternion (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z))
yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z)
print("Gyro-only quaternion (%0.2f, %0.2f, %0.2f, %0.2f), "
"yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" % (w, x, y, z,
yaw * 180.0 / pi,
pitch * 180.0 / pi,
roll * 180.0 / pi))
def acct1(null):
"""
#LED_Button
print("#####")
print bn
GPIO.output(led,GPIO.HIGH)
print("#####")
###
"""
imu = GY80()
imu.update()
time.sleep(5)
#w, x, y, z = imu.current_orientation_quaternion_hybrid()
w, x, y, z = imu._current_hybrid_orientation_q
#print("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z))
yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z)
roll = roll * 180.0 / 3.14
pitch = pitch * 180.0 / 3.14
print roll
print pitch
if (-10 < roll < 10 and 70 < pitch < 80):
b = 2
print("khodro be samte rast vazhegon shode")
elif (-30 < roll < 0 and -70 < pitch < 90):
b = 3
print("khodro be samte chap vazhegon shode")
elif (80 < roll < 110 and 0 < pitch < 15):
b = 4
print("khodro be aghab vazhegoon shode ast")
elif (-90 < roll < -110 and -5 < pitch < 10):
b = 5
print("khodro be jelo vazhegoon shode ast")
else:
b = 1
print("tanha zarbe")
#if( a=1 or b=2 or =b=3 or b=4 )
# print("accident occured")
##GPS Read
print("$$$$$$")
port.write("AT+CGPSINF=0" + '\r\n')
rcv = port.read(120)
print rcv
str1 = rcv.split(",")
#print str1[1],str1[2]
## HTTP Post
sens1 = adxl345.read_scaled_accel_x()
sens2 = adxl345.read_scaled_accel_y()
sens3 = adxl345.read_scaled_accel_z()
#v average
avr = 0
for i in range(9):
avr = avr + (mylist[i]) / 9
port.write("AT+HTTPPARA=\"URL\", \"" + url + '&field1=' + str1[1] +
'&field2=' + str1[2] + '&field3=' + str(b) + '&field4=' +
str(a) + '&field4=' + str(avr) + "\"\r\n")
# port.write("AT+HTTPPARA=\"URL\", \"" + url +'&field5='+str(sens2) + "\"\r\n")
# port.write("AT+HTTPPARA=\"URL\", \"" + url +'&field6='+str(sens3) + "\"\r\n")
rcv = port.read(500)
print rcv
port.write("AT+HTTPACTION=0\r\n")
rcv = port.read(120)
print rcv
rcv = port.read(120)
print rcv
time.sleep(5)
rcv = port.write("AT+HTTPTERM\r\n")
rcv = port.read(500)
print rcv
print("zarbe az jelo")
time.sleep(5)
