m9g_low_old = m9g_low
mag_raw = np.array([[m9m[0]], [m9m[1]], [m9m[2]]])
mag_correct = np.dot(mag_A_1, mag_raw - mag_b)
psi = np.arctan2(mag_correct[1][0], mag_correct[0][0]) - psi_const
omega = np.array([(m9g[0] - bias_gyro_x), (m9g[1] - bias_gyro_y),
m9g[2] - bias_gyro_z])
yy = np.array([[-m9a_low[0]], [-m9a_low[1]], [-m9a_low[2]], [psi]])
P_new = (I_9 - GG.dot(CC)).dot(P_pre)
x_new = x_pre + GG.dot(yy - h_pre)
angle = -0.0671 * adc.read(4) + 171.29
# print "{:+7.3f}".format(-x_new[:,0][7]*180/np.pi),",","{:+7.3f}".format(x_new[:,0][6]*180/np.pi),",","{:+7.3f}".format(x_new[:,0][8]*180/np.pi)
# print "{:+7.3f}".format(m9a_low[0]),",","{:+7.3f}".format(m9a_low[1]),",","{:+7.3f}".format(m9a_low[2])
print "{:+7.3f}".format(x_new[:, 0][8] * 180 /
np.pi), ",", "{:+7.3f}".format(angle)
#########################################################################################
throt = (float(rcin.read(2)) - 1104) * 80000 / 820
# print throt
omega2_1 = throt
omega2_2 = throt
omega2_3 = throt
omega2_4 = throt
import sys
import time
import navio.adc
import navio.util
navio.util.check_apm()
adc = navio.adc.ADC()
results = [0] * adc.channel_count
while (True):
s = ''
for i in range(0, adc.channel_count):
results[i] = adc.read(i)
s += 'A{0}: {1:6.4f}V '.format(i, results[i] / 1000)
print(s)
time.sleep(0.5)
m9g_low_old = m9g_low
mag_raw = np.array([[m9m[0]], [m9m[1]], [m9m[2]]])
mag_correct = np.dot(mag_A_1, mag_raw - mag_b)
psi = np.arctan2(mag_correct[1][0], mag_correct[0][0]) - psi_const
omega = np.array([(m9g[0] - bias_gyro_x), (m9g[1] - bias_gyro_y),
m9g[2] - bias_gyro_z])
yy = np.array([[-m9a_low[0]], [-m9a_low[1]], [-m9a_low[2]], [psi]])
P_new = (I_9 - GG.dot(CC)).dot(P_pre)
x_new = x_pre + GG.dot(yy - h_pre)
angle = -0.0658 * adc.read(4) + 171.9
# print "{:+7.3f}".format(-x_new[:,0][7]*180/np.pi),",","{:+7.3f}".format(x_new[:,0][6]*180/np.pi),",","{:+7.3f}".format(x_new[:,0][8]*180/np.pi)
# print "{:+7.3f}".format(m9a_low[0]),",","{:+7.3f}".format(m9a_low[1]),",","{:+7.3f}".format(m9a_low[2])
print "{:+7.3f}".format(x_new[:, 0][8] * 180 /
np.pi), ",", "{:+7.3f}".format(angle)
#########################################################################################
elapsed_time = time.time() - start
#print elapsed_time
if elapsed_time < 0.01:
sleep_time = 0.01 - elapsed_time
time.sleep(sleep_time)
#------------------------------------------------#
### Declare global variables here ###
print "Starting main loop: here we go!"
while True:
current_time = timeg*1000.0
if m9m[0] == 0:
led.setColor('Magenta')
else:
led.setColor('Green')
if (current_time - timer_100hz) >=10.0: # 10 ms = 100Hz
#### IMU/Attitude and GPS estimation: DO NOT TOUCH ####
for i in range (0, adc.channel_count):
analog[i] = adc.read(i)*0.001
accels, rates, m9m = imu.getMotion9()
ac_AHRS[0] = -accels[1]
ac_AHRS[1] = -accels[0]
ac_AHRS[2] = accels[2]
gy_AHRS[0] = rates[1]
gy_AHRS[1] = rates[0]
gy_AHRS[2] = -rates[2]
AHRS_data.update_imu(gy_AHRS, ac_AHRS)
roll,pitch,yaw = quat2euler(AHRS_data.quaternion,axes='rxyz')
baro_timer = baro_timer + 1
if (baro_timer == 1): baro.refreshPressure()
elif (baro_timer == 2): baro.readPressure()
elif (baro_timer == 3): baro.refreshTemperature()
elif (baro_timer == 4): baro.readTemperature()
elif (baro_timer == 5):
],
],
dtype=np.float)
#########################################################################################
P_pre = AA.dot(P_old.dot(AA.T)) + QQ
G1 = la.inv(CC.dot(P_pre.dot(CC.T)) + RR)
GG = P_pre.dot((CC.T).dot(G1))
#########################################################################################
h_pre = np.array([
s_the_pre * g, -c_the_pre * s_phi_pre * g, -c_the_pre * c_phi_pre * g
],
dtype=np.float)
m9a, m9g, m9m = imu.getMotion9() #measure
results[5] = adc.read(5) #measure
omega = np.array([
-(m9g[0] - bias_gyro_x), -(m9g[1] - bias_gyro_y), m9g[2] - bias_gyro_z
])
yy = np.array([[m9a[0]], [m9a[1]], [-m9a[2]]])
P_new = (I_9 - GG.dot(CC)).dot(P_pre)
x_new = x_pre + GG.dot(yy - h_pre)
phi = np.arctan(-m9a[1] / m9a[2]) * 180 / np.pi
print "{:+7.3f}".format(
x_new[:, 0][6] * 180 / np.pi), ",", "{:+7.3f}".format(
x_new[:, 0][7] * 180 / np.pi), ",", "{0:.3f}".format(
import sys, time
import numpy as np
import navio.adc
import navio.util
navio.util.check_apm()
adc = navio.adc.ADC()
results = [0] * adc.channel_count
outdata = np.zeros((600, 3))
cnt = 0
while (True):
outdata[cnt, 0] = time.time()
outdata[cnt, 1] = adc.read(4) * 5
outdata[cnt, 2] = adc.read(5) * 5
print "t: " + str(outdata[cnt, 0]) + " Sa: " + str(
outdata[cnt, 1]) + "umol m^-2 s^-1 Sb: " + str(
outdata[cnt, 2]) + "umol m^-2 s^-1"
cnt += 1
if cnt >= 600:
cnt = 0
np.savetxt('DATA/apogee_' + time.strftime('%Y%m%d-%H%M%S') + '.dat',
outdata)
outdata = np.zeros((600, 3))
time.sleep(0.1)
IP = s.getsockname()[0]
except:
IP = '127.0.0.1'
finally:
s.close()
return IP
navio.util.check_apm()
led = navio.leds.Led()
adc = navio.adc.ADC()
# trueIP=trueIP.split(".")
while (True):
# first check if power is above or below minimum
while (adc.read(2) / 100 < 11.8):
led.setColor('Red')
time.sleep(0.1)
led.setColor('Black')
time.sleep(0.1)
# fetch our IP address
trueIP = get_ip()
for i in trueIP:
try:
count = int(i)
for j in range(count):
led.setColor('Green')
time.sleep(0.2)
led.setColor('Black')
time.sleep(0.2)
led.setColor('Yellow')
import navio.pwm
import navio.adc
import navio.util
navio.util.check_apm()
adc = navio.adc.ADC()
results = [0] * adc.channel_count
PWM_OUTPUT = 0
SERVO_MIN = 1.000 #ms
SERVO_MAX = 1.500 #ms
pwm = navio.pwm.PWM(PWM_OUTPUT)
pwm.set_period(400)
#pwm.set_duty_cycle(SERVO_MIN)
#time.sleep(10)
while (True):
results[5] = adc.read(5)
duty=0.001*int(1000+900*results[5]/5000) #ms
pwm.set_duty_cycle(duty)
print duty
time.sleep(0.1)
# pwm.set_duty_cycle(SERVO_MAX)
# time.sleep(0.2)
import sys, time
import numpy as np
import navio.adc
import navio.util
navio.util.check_apm()
adc = navio.adc.ADC()
data = np.zeros(1000)
start_time = time.clock() * 1000.0
for i in range(0, 1000):
analog_val = adc.read(4) * 0.001
data[i] = analog_val
print analog_val
time.sleep(0.25)
end_time = time.clock() * 1000.0
average = np.mean(data)
print end_time - start_time, average
from gpiozero import LED
import navio.util
import navio.adc
from time import sleep
led = LED(17)
power=LED(18)
navio.util.check_apm()
adc=navio.adc.ADC()
while True:
if (adc.read(2)/100<11.8):
led.on()
sleep(0.1)
led.off()
sleep(0.1)
power.off()
else:
power.on();
led.off()
import sys, time
import navio.adc
import navio.util
navio.util.check_apm()
adc = navio.adc.ADC()
results = [0] * adc.channel_count
while (True):
results1 = adc.read(4)
results2 = adc.read(0)
# s += 'A{0}: {1:6.4f}V '.format(i, results[5] / 1000)
# duty=0.001*int(1000+900*results[5]/5000)
print results1 #-0.0658*results1+171.9 , results2
time.sleep(0.1)
