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)
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)
