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)