def main():
logger.debug("initializing imu...")
imu = MPU9250() # initialize mpu9250, the imu
logger.debug("initializing esc...")
esc_hdd = ESC(config['speed_pin_esc'], config['dir_pin1_esc'],
config['dir_pin2_esc'])
esc_hdd.init_sequence() # perform the esc initialization sequence
# do we want to stream sensor data to the web interface
if config['enable_mission_control']:
logger.info("connecting to mission control server...")
mc.connect() # connect to mission control server
logger.info("starting main loop...")
#acc_array = np.zeros(3,1)
#mag_array = np.zeros(3,1)
while True:
# fetch values from the IMU
imu.update_acc_reading() # update acc values
imu.update_mag_reading() # update mag values
acc_meas_raw = [imu.acc_y * -1, imu.acc_x,
imu.acc_z * -1] # fix imu right hand rule
mag_meas_raw = [imu.mag_x, imu.mag_y * -1,
imu.mag_z] # fix imu right hand rule
# generate the necessary vectors for TRIAD
# TODO don't recreate numpy array every time, heavy memory computation
acc_meas = utils.to_unit_vector(acc_meas_raw) # acc unit vector
mag_meas = utils.to_unit_vector(mag_meas_raw) # mag unit vector
#acc_array[:,0] = acc_meas_raw
#mag_array[:,0] = mag_meas_raw
#acc_meas = numpy.linalg.oth(acc_array)
#mag_meas = numpu.linalg.oth(mag_array)
acc_ref = np.array([0.0, 0.0, -1.0]) # acc ref vector
mag_ref = np.array([1.0, 0.0, 0.0]) # mag ref vector
# make sure none of the vectors are zero vectors
if not (np.any(acc_meas) and np.any(mag_meas)):
logger.warn("measurement zero vector computed")
continue
# compute the rotation matrix with the aforemened vectors
rotation = triad.triad(acc_meas, mag_meas, acc_ref, mag_ref)
logger.debug("acc: {} mag: {} rotation: {}".format(
acc_meas, mag_meas, rotation))
# broadcast all data to mission control server
# this is the real-time gui thing
if config['enable_mission_control']:
mc.broadcast(acc_meas_raw, mag_meas_raw, rotation)
# wait a sec before proceeding, this is our
# update frequency.
time.sleep(0.05)
class QuadQav250:
PID_ANGULAR = [[0.006,0.0,0.0],[0.006,0,0],[0,0,0]]
def __init__(self):
try:
self.altimeter = TOF_VL53L1X()
self.thrust = ESC()
self.ahrs = AHRS()
self.angular_pid_ahrs = PidController(QuadQav250.PID_ANGULAR, self.ahrs.get_angular_position, [0,0,0], PidController.t_angular)
self.ahrs.start()
except Exception:
self.shutdown()
print('Failed to initialize')
def shutdown(self):
try:
self.thrust.disarm()
finally:
print('===DISARMED===')
def test(self):
for n in range(100):
pass
#print(self.ahrs.get_angular_position())
def kill(self):
self.shutdown()
def fly(self):
try:
print('============================')
print('STARTING TEST FLIGHT')
self.thrust.spin_test()
base_throttle = [.20,.20,.20,.20]
t0 = time.time()
flying = True
while flying:
if time.time() > t0 + 0.20:
base_throttle = np.add(base_throttle, 0.001)
pid_update = self.angular_pid_ahrs.update()
v_throttle = np.add(base_throttle, pid_update)
self.thrust.set_throttle(v_throttle)
xyz, xyz_dot, dt = self.altimeter.get_altitude()
if xyz[2] > 0.200:
flying = False
print(v_throttle,self.thrust.v_pwm,self.ahrs.xyz)
self.thrust.set_throttle([.2,.2,.2,.2])
time.sleep(0.5)
self.thrust.set_throttle([0,0,0,0])
finally:
self.shutdown()
def main():
logger.debug("Initializing the imu...")
# Initialize the imu.
imu = MPU9250()
# Initialize the speed controller.
logger.debug("initializing esc...")
esc_hdd = ESC(config['speed_pin_esc'], config['dir_pin1_esc'],
config['dir_pin2_esc'])
esc_hdd.init_sequence()
# If enabled, connect to the web interface server.
if config['enable_mission_control']:
logger.info("connecting to mission control server...")
mc.connect() # connect to mission control server
logger.info("starting main loop...")
# acc_array = np.zeros(3,1)
# mag_array = np.zeros(3,1)
while True:
# Fetch and calculate raw measure values from the IMU.
raw_acc_measure, raw_mag_measure = get_updated_sensor_data(imu)
# TODO don't recreate numpy array every time, heavy memory computation
# Generate the necessary vectors for TRIAD.
acc_measure_uvec = utils.to_unit_vector(raw_acc_measure)
mag_measure_uvec = utils.to_unit_vector(raw_mag_measure)
# acc_array[:,0] = acc_meas_raw
# mag_array[:,0] = mag_meas_raw
# acc_meas = numpy.linalg.oth(acc_array)
# mag_meas = numpu.linalg.oth(mag_array)
# Create reference vectors for the accelerometer and magnetometer.
acc_refvec = np.array([0.0, 0.0, -1.0])
mag_refvec = np.array([1.0, 0.0, 0.0])
# Make sure none of the vectors are zero vectors.
if not (np.any(acc_measure_uvec) and np.any(mag_measure_uvec)):
logger.warn("measurement zero vector computed")
continue
# Compute the rotation matrix with the aforementioned vectors.
rotation = triad.triad(acc_measure_uvec, mag_measure_uvec, acc_refvec,
mag_refvec)
logger.debug("acc: {} mag: {} rotation: {}".format(
acc_measure_uvec, mag_measure_uvec, rotation))
# Broadcast the raw measures to the mission control server.
if config['enable_mission_control']:
mc.broadcast(raw_acc_measure, raw_mag_measure, rotation)
# Simulate update frequency through sleeping.
time.sleep(0.05)
def __init__(self):
try:
self.altimeter = TOF_VL53L1X()
self.thrust = ESC()
self.ahrs = AHRS()
self.angular_pid_ahrs = PidController(QuadQav250.PID_ANGULAR, self.ahrs.get_angular_position, [0,0,0], PidController.t_angular)
self.ahrs.start()
except Exception:
self.shutdown()
print('Failed to initialize')
from car import Car
from config import WEB_PORT
from esc import ESC
from led import LED
import esc
import threading
import time
import psutil
#from printf import PRINTF
import os
app = Flask(__name__)
led = LED()
car = Car()
esc = ESC()
#printf = PRINTF()
handle_map = {
'forward': car.forward,
'left': car.left,
'right': car.right,
'pause': car.stop,
'backward': car.backward,
'on': esc.on,
'off': esc.off,
#'on': printf.add,
#'off': printf.dec,
'work': led.work,
}
# PID
rr_pid = PID(p=0.7, i=1, imax=50)
pr_pid = PID(p=0.7, i=1, imax=50)
yr_pid = PID(p=2.7, i=1, imax=50)
rs_pid = PID(p=4.5)
ps_pid = PID(p=4.5)
ys_pid = PID(p=10)
# RC
rc_thr = RC(0) # throttle
rc_rol = RC(1) # roll
rc_pit = RC(2) # pitch
rc_yaw = RC(3) # yaw
# ESC
esc_0 = ESC(0)
esc_1 = ESC(1)
esc_2 = ESC(2)
esc_3 = ESC(3)
esc_4 = ESC(4)
esc_5 = ESC(5)
yaw_target = 0
while True:
# MPU
mpuIntStatus = mpu.getIntStatus()
fifoCount = mpu.getFIFOCount()
if mpuIntStatus < 2 or fifoCount == 1024:
mpu.resetFIFO()
print('FIFO overflow!')
continue
import time
import pwm #Used to enable PWM
from esc import ESC
#You can use your own method to enable PWM, but this just works
pwm.enable();
esc = ESC()
esc.attach("P9_16")
# This example works with aeolian car (double direction) ESC which minimum throttle is 80.
# Try changing values until you found the right value for your ESC
esc.write(80)
#esc.writeMicroseconds(1600)
#Now the ESC es ready, in my case if I use values less than 80 the motor goes rearward
# and if the values are greater than 80 the motor goes fordward
# PID
rr_pid = PID(p=0.7, i=1, imax=50)
pr_pid = PID(p=0.7, i=1, imax=50)
yr_pid = PID(p=2.7, i=1, imax=50)
rs_pid = PID(p=4.5)
ps_pid = PID(p=4.5)
ys_pid = PID(p=10)
# RC
rc_thr = RC(0) # throttle
rc_rol = RC(1) # roll
rc_pit = RC(2) # pitch
rc_yaw = RC(3) # yaw
# ESC
esc_0 = ESC(0)
esc_1 = ESC(1)
esc_2 = ESC(2)
esc_3 = ESC(3)
esc_4 = ESC(4)
esc_5 = ESC(5)
yaw_target = 0
while True:
# MPU
mpuIntStatus = mpu.getIntStatus()
fifoCount = mpu.getFIFOCount()
if mpuIntStatus < 2 or fifoCount == 1024:
mpu.resetFIFO()
print('FIFO overflow!')
continue
