def set_rotation(conn, rotation, wait=True, timeout=25):
'''set servo rotation'''
if not rotation in ROTATIONS:
util.failure("No rotation %s" % rotation)
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
logger.info("set_rotation: call set_servo -- YAW rotation[%s].chan1=%s PITCH rotation[%s].chan2=%s" % (rotation, ROTATIONS[rotation].chan1, rotation, ROTATIONS[rotation].chan2) )
# start with initial settings from the table
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS[rotation].chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS[rotation].chan2)
if not wait:
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
time.sleep(1)
util.discard_messages(conn.refmav)
if expected_roll == 0 and expected_pitch == 0:
tolerance = ROTATION_LEVEL_TOLERANCE
else:
tolerance = ROTATION_TOLERANCE
# now optimise it
if not optimise_attitude(conn, rotation, tolerance, timeout=timeout):
util.failure("Failed to reach target attitude")
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
def unjam_servos(conn):
'''try to unjam servos with random movement'''
logger.info("Starting unjamming")
conn.discard_messages()
rotations = ROTATIONS.keys()
util.param_set(conn.ref, 'SR0_RAW_SENS', 10)
last_change = time.time()
while True:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
gyro = util.gyro_vector(imu)
logger.info('Gyro: %s' % gyro)
if abs(gyro.x) > 5 or abs(gyro.y) > 5:
logger.info("Unjammed: ", gyro)
break
if time.time() > last_change+0.7:
last_change = time.time()
r1 = int(random.uniform(800, 2100))
r2 = int(random.uniform(800, 2100))
logger.debug("%s %s" % (r1, r2))
util.set_servo(conn.refmav, YAW_CHANNEL, r1)
util.set_servo(conn.refmav, PITCH_CHANNEL, r2)
def unjam_servos(conn):
'''try to unjam servos with random movement'''
logger.info("Starting unjamming")
conn.discard_messages()
rotations = ROTATIONS.keys()
util.param_set(conn.ref, 'SR0_RAW_SENS', 10)
last_change = time.time()
while True:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
gyro = util.gyro_vector(imu)
logger.info('Gyro: %s' % gyro)
if abs(gyro.x) > 5 or abs(gyro.y) > 5:
logger.info("Unjammed: ", gyro)
break
if time.time() > last_change+0.7:
last_change = time.time()
r1 = int(random.uniform(800, 2100))
r2 = int(random.uniform(800, 2100))
logger.debug("%s %s" % (r1, r2))
util.set_servo(conn.refmav, YAW_CHANNEL, r1)
util.set_servo(conn.refmav, PITCH_CHANNEL, r2)
def set_rotation(conn, rotation, wait=True, timeout=25, quick=False):
'''set servo rotation'''
if not rotation in ROTATIONS:
util.failure("No rotation %s" % rotation)
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
logger.info("set_rotation: %s chan1=%u chan2=%u" % (rotation, ROTATIONS[rotation].chan1, ROTATIONS[rotation].chan2) )
# start with initial settings from the table
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS[rotation].chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS[rotation].chan2)
if not wait:
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
time.sleep(1)
util.discard_messages(conn.refmav)
if expected_roll == 0 and expected_pitch == 0:
tolerance = ROTATION_LEVEL_TOLERANCE
else:
tolerance = ROTATION_TOLERANCE
# now optimise it
if not optimise_attitude(conn, rotation, tolerance, timeout=timeout, quick=quick):
util.failure("Failed to reach target attitude")
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
def set_rotation(conn, rotation, wait=True, timeout=25):
'''set servo rotation'''
if not rotation in ROTATIONS:
util.failure("No rotation %s" % rotation)
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
logger.debug("set_rotation: call set_servo -- YAW rotation[%s].chan1=%s PITCH rotation[%s].chan2=%s" % (rotation, ROTATIONS[rotation].chan1, rotation, ROTATIONS[rotation].chan2) )
if ETE == 0:
# start with initial settings from the table
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS[rotation].chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS[rotation].chan2)
elif ETE == 1:
ete = PixETE()
ete.position(ROTATIONS_ETE[rotation].chan2, ROTATIONS_ETE[rotation].chan1)
if not wait:
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
time.sleep(2)
util.discard_messages(conn.refmav)
if expected_roll == 0 and expected_pitch == 0:
tolerance = ROTATION_LEVEL_TOLERANCE
else:
tolerance = ROTATION_TOLERANCE
# now optimise it
if not optimise_attitude(conn, rotation, tolerance, timeout=timeout):
util.failure("Failed to reach target attitude")
return conn.refmav.recv_match(type='ATTITUDE', blocking=True, timeout=3)
def center_servos(conn):
'''center servos at 1500/1500'''
logger.info("Centering servos")
try:
util.set_servo(conn.refmav, YAW_CHANNEL, 1500)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1500)
except Exception as ex:
print("Failed centering servos: %s" % ex)
pass
def optimise_attitude(conn, rotation, tolerance, timeout=25):
'''optimise attitude using servo changes'''
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
chan1 = ROTATIONS[rotation].chan1
chan2 = ROTATIONS[rotation].chan2
attitude = wait_quiescent(conn.refmav)
time_start = time.time()
# we always do at least 2 tries. This means the attitude accuracy
# will tend to improve over time, while not adding excessive time
# per board
tries = 0
while time.time() < time_start+timeout:
#logger.info("============================= BEGIN ROTATIONS try=%s =================" % (tries))
dcm_estimated = Matrix3()
dcm_estimated.from_euler(attitude.roll, attitude.pitch, attitude.yaw)
droll = expected_roll
if droll is None:
droll = attitude.roll
else:
droll = radians(droll)
dpitch = radians(expected_pitch)
dcm_demanded = Matrix3()
dcm_demanded.from_euler(droll, dpitch, attitude.yaw)
(chan1_change, chan2_change) = gimbal_controller(dcm_estimated,
dcm_demanded, chan1)
(err_roll, err_pitch) = attitude_error(attitude, expected_roll, expected_pitch)
# logger.debug("%s offsets: %.2f %.2f chan1=%u chan2=%u" % (rotation, err_roll, err_pitch, chan1, chan2))
logger.info("optimise_attitude: %s err_roll=%.2f err_pitch=%.2f chan1=%u chan2=%u" % (rotation, err_roll, err_pitch, chan1, chan2))
# logger.info("Try %s -- rotation: %s err_roll: %.2f err_pitch: %.2f tolerance %.1f at %s" % (tries, rotation, err_roll, err_pitch, tolerance, time.ctime()))
if (tries > 0 and (abs(err_roll)+abs(err_pitch) < tolerance or
(abs(chan1_change)<1 and abs(chan2_change)<1))):
logger.info("%s converged %.2f %.2f tolerance %.1f at %s" % (rotation, err_roll, err_pitch, tolerance, time.ctime()))
# update optimised rotations to save on convergence time for the next board
ROTATIONS[rotation].chan1 = chan1
ROTATIONS[rotation].chan2 = chan2
logger.info("optimise_attitude: ROTATIONS[%s] chan1:%s chan2:%s" % (rotation, ROTATIONS[rotation].chan1, ROTATIONS[rotation].chan2) )
return True
chan1 += chan1_change
chan2 += chan2_change
if chan1 < 700 or chan1 > 2300 or chan2 < 700 or chan2 > 2300:
logger.debug("servos out of range - failed")
return False
util.set_servo(conn.refmav, YAW_CHANNEL, chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, chan2)
attitude = wait_quiescent(conn.refmav)
tries += 1
logger.error("timed out rotating to %s" % rotation)
return False
def gyro_integrate(conn):
'''test gyros by integrating while rotating to the given rotations'''
conn.ref.send('set streamrate -1\n')
conn.test.send('set streamrate -1\n')
util.param_set(conn.ref, 'SR0_RAW_SENS', 20)
util.param_set(conn.test, 'SR0_RAW_SENS', 20)
logger.info("Starting gyro integration")
wait_quiescent(conn.refmav)
conn.discard_messages()
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1+200)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS['level'].chan2+200)
start_time = time.time()
ref_tstart = None
test_tstart = [None]*3
ref_sum = Vector3()
test_sum = [Vector3(), Vector3(), Vector3()]
msgs = { 'RAW_IMU' : 0, 'SCALED_IMU2' : 1, 'SCALED_IMU3' : 2 }
while time.time() < start_time+20:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
gyro = util.gyro_vector(imu)
tnow = imu.time_usec*1.0e-6
if ref_tstart is not None:
deltat = tnow - ref_tstart
ref_sum += gyro * deltat
ref_tstart = tnow
if time.time() - start_time > 2 and gyro.length() < GYRO_TOLERANCE:
break
imu = conn.testmav.recv_match(type=msgs.keys(), blocking=False)
if imu is not None:
idx = msgs[imu.get_type()]
gyro = util.gyro_vector(imu)
if imu.get_type().startswith("SCALED_IMU"):
tnow = imu.time_boot_ms*1.0e-3
else:
tnow = imu.time_usec*1.0e-6
if test_tstart[idx] is not None:
deltat = tnow - test_tstart[idx]
test_sum[idx] += gyro * deltat
test_tstart[idx] = tnow
logger.debug("Gyro ref sums: %s" % ref_sum)
logger.debug("Gyro test sum1: %s" % test_sum[0])
logger.debug("Gyro test sum2: %s" % test_sum[1])
logger.debug("Gyro test sum3: %s" % test_sum[2])
for idx in range(3):
err = test_sum[idx] - ref_sum
if abs(err.x) > GYRO_SUM_TOLERANCE:
util.failure("X gyro %u error: %.1f" % (idx, err.x))
if abs(err.y) > GYRO_SUM_TOLERANCE:
util.failure("Y gyro %u error: %.1f" % (idx, err.y))
if abs(err.z) > GYRO_SUM_TOLERANCE:
util.failure("Z gyro %u error: %.1f" % (idx, err.z))
def gyro_integrate(conn):
'''test gyros by integrating while rotating to the given rotations'''
conn.ref.send('set streamrate -1\n')
conn.test.send('set streamrate -1\n')
util.param_set(conn.ref, 'SR0_RAW_SENS', 20)
util.param_set(conn.test, 'SR0_RAW_SENS', 20)
logger.info("Starting gyro integration at %s" % time.ctime())
wait_quiescent(conn.refmav)
conn.discard_messages()
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1+200)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS['level'].chan2+200)
start_time = time.time()
ref_tstart = None
test_tstart = [None]*3
ref_sum = Vector3()
test_sum = [Vector3(), Vector3(), Vector3()]
msgs = { 'RAW_IMU' : 0, 'SCALED_IMU2' : 1, 'SCALED_IMU3' : 2 }
while time.time() < start_time+20:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
gyro = util.gyro_vector(imu)
tnow = imu.time_usec*1.0e-6
if ref_tstart is not None:
deltat = tnow - ref_tstart
ref_sum += gyro * deltat
ref_tstart = tnow
if time.time() - start_time > 2 and gyro.length() < GYRO_TOLERANCE:
break
imu = conn.testmav.recv_match(type=msgs.keys(), blocking=False)
if imu is not None:
idx = msgs[imu.get_type()]
gyro = util.gyro_vector(imu)
if imu.get_type().startswith("SCALED_IMU"):
tnow = imu.time_boot_ms*1.0e-3
else:
tnow = imu.time_usec*1.0e-6
if test_tstart[idx] is not None:
deltat = tnow - test_tstart[idx]
test_sum[idx] += gyro * deltat
test_tstart[idx] = tnow
logger.debug("Gyro ref sums: %s" % ref_sum)
logger.debug("Gyro test sum1: %s" % test_sum[0])
logger.debug("Gyro test sum2: %s" % test_sum[1])
logger.debug("Gyro test sum3: %s" % test_sum[2])
for idx in range(3):
err = test_sum[idx] - ref_sum
if abs(err.x) > GYRO_SUM_TOLERANCE:
util.failure("X gyro %u error: %.1f" % (idx, err.x))
if abs(err.y) > GYRO_SUM_TOLERANCE:
util.failure("Y gyro %u error: %.1f" % (idx, err.y))
if abs(err.z) > GYRO_SUM_TOLERANCE:
util.failure("Z gyro %u error: %.1f" % (idx, err.z))
def center_servos(conn):
'''center servos at 1500/1500'''
logger.info("Centering servos")
if ETE == 0:
try:
util.set_servo(conn.refmav, YAW_CHANNEL, 1500)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1500)
print("NO ETE Center")
except Exception as ex:
print("Failed centering servos: %s" % ex)
pass
elif ETE == 1:
#try:
ete = PixETE()
ete.position(0, 0)
time.sleep(2)
print("ETE Center")
def find_pitch_zero(conn):
'''find the pitch zero'''
pitch_min = 900
pitch_max = 2000
best_pitch = None
best_pitch_value = None
for pdelta in [200, 20, 2]:
for pitch in range(pitch_min, pitch_max+pdelta, pdelta):
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch)
wait_quiescent(conn.refmav)
r1, p1, y1 = get_attitude(conn)
print("pitch=%u p=%.1f" % (pitch, p1))
if abs(r1) > 80:
continue
if best_pitch is None or abs(p1) < best_pitch_value:
best_pitch = pitch
best_pitch_value = abs(p1)
print("best_pitch=%u best_pitch_value=%.1f" % (best_pitch, best_pitch_value))
pitch_min = best_pitch-pdelta
pitch_max = best_pitch+pdelta
return best_pitch
def find_pitch_zero(conn):
'''find the pitch zero'''
pitch_min = 900
pitch_max = 2000
best_pitch = None
best_pitch_value = None
for pdelta in [200, 20, 2]:
for pitch in range(pitch_min, pitch_max+pdelta, pdelta):
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch)
wait_quiescent(conn.refmav)
r1, p1, y1 = get_attitude(conn)
print("pitch=%u p=%.1f" % (pitch, p1))
if abs(r1) > 80:
continue
if best_pitch is None or abs(p1) < best_pitch_value:
best_pitch = pitch
best_pitch_value = abs(p1)
print("best_pitch=%u best_pitch_value=%.1f" % (best_pitch, best_pitch_value))
pitch_min = best_pitch-pdelta
pitch_max = best_pitch+pdelta
return best_pitch
def find_yaw_zero(conn):
'''find the yaw zero'''
yaw_min = 1200
yaw_max = 1800
# start in units of 100
best_yaw = None
best_roll_change = None
for ydelta in [200, 20, 2]:
for yaw in range(yaw_min, yaw_max+ydelta, ydelta):
util.set_servo(conn.refmav, YAW_CHANNEL, yaw)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1400)
wait_quiescent(conn.refmav)
r1, p1, y1 = get_attitude(conn)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1500)
wait_quiescent(conn.refmav)
r2, p2, y2 = get_attitude(conn)
rchange = abs(util.wrap_180(r2 - r1))
print("yaw=%u rchange=%.1f" % (yaw, rchange))
if best_yaw is None or rchange < best_roll_change:
best_yaw = yaw
best_roll_change = rchange
print("best_yaw=%u best_roll_change=%.1f" % (best_yaw, best_roll_change))
yaw_min = best_yaw-ydelta
yaw_max = best_yaw+ydelta
return best_yaw
def find_pitch_scale(conn):
'''calibration step 4: find pitch scale'''
global PITCH_SCALE, YAW_SCALE
yaw_zero = ROTATIONS['level'].chan1
pitch_zero = ROTATIONS['level'].chan2
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r1, p1, y1 = get_attitude(conn)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero+100)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r2, p2, y2 = get_attitude(conn)
print(p1, p2)
pitchchange = util.wrap_180(p2 - p1)
PITCH_SCALE = pitchchange / 100.0
print("PITCH_SCALE=%.2f" % PITCH_SCALE)
if abs(PITCH_SCALE) < 0.2:
print("Bad pitch scale")
return False
write_calibration()
return True
def find_yaw_scale(conn):
'''calibration step 3: find yaw scale'''
yaw_zero = ROTATIONS['level'].chan1
pitch_zero = ROTATIONS['level'].chan2
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r1, p1, y1 = get_attitude(conn)
print(r1, p1, y1)
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero+100)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r2, p2, y2 = get_attitude(conn)
print(r2, p2, y2)
print(y1, y2)
yawchange = util.wrap_180(y2 - y1)
YAW_SCALE = yawchange / 100.0
print("YAW_SCALE=%.2f" % YAW_SCALE)
if abs(YAW_SCALE) < 0.2:
print("Bad yaw scale")
return False
write_calibration()
return True
def find_yaw_zero(conn):
'''find the yaw zero'''
yaw_min = 1400
yaw_max = 1600
# start in units of 100
best_yaw = None
best_roll_change = None
for ydelta in [40, 20, 5]:
for yaw in range(yaw_min, yaw_max+ydelta, ydelta):
util.set_servo(conn.refmav, YAW_CHANNEL, yaw)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1400)
wait_quiescent(conn.refmav)
r1, p1, y1 = get_attitude(conn)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1500)
wait_quiescent(conn.refmav)
r2, p2, y2 = get_attitude(conn)
rchange = abs(util.wrap_180(r2 - r1))
print("yaw=%u rchange=%.1f r=%.1f/%.1f p=%.1f/%.1f" % (
yaw, rchange,
r1, r2, p1, p2))
if abs(r1) > 90 or abs(r2) > 90:
continue
if best_yaw is None or rchange < best_roll_change:
best_yaw = yaw
best_roll_change = rchange
print("best_yaw=%u best_roll_change=%.1f (r=%.1f/%.1f p=%.1f/%.1f)" % (
best_yaw, best_roll_change,
r1, r2, p1, p2))
if best_roll_change <= 2.0:
return best_yaw
if best_yaw is not None:
yaw_min = best_yaw-ydelta
yaw_max = best_yaw+ydelta
return best_yaw
def find_yaw_zero(conn):
'''find the yaw zero'''
yaw_min = 1200
yaw_max = 1800
# start in units of 100
best_yaw = None
best_roll_change = None
for ydelta in [200, 20, 2]:
for yaw in range(yaw_min, yaw_max+ydelta, ydelta):
util.set_servo(conn.refmav, YAW_CHANNEL, yaw)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1400)
wait_quiescent(conn.refmav)
r1, p1, y1 = get_attitude(conn)
util.set_servo(conn.refmav, PITCH_CHANNEL, 1500)
wait_quiescent(conn.refmav)
r2, p2, y2 = get_attitude(conn)
rchange = abs(util.wrap_180(r2 - r1))
print("yaw=%u rchange=%.1f" % (yaw, rchange))
if best_yaw is None or rchange < best_roll_change:
best_yaw = yaw
best_roll_change = rchange
print("best_yaw=%u best_roll_change=%.1f" % (best_yaw, best_roll_change))
yaw_min = best_yaw-ydelta
yaw_max = best_yaw+ydelta
return best_yaw
def find_pitch_zero(conn):
'''find the pitch zero'''
pitch_min = 1400
pitch_max = 1600
best_pitch = None
best_pitch_value = None
for pdelta in [40, 20, 5]:
for pitch in range(pitch_min, pitch_max+pdelta, pdelta):
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch)
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1)
wait_quiescent(conn.refmav)
r1, p1, y1 = get_attitude(conn)
print("pitch=%u p=%.1f r=%.1f" % (pitch, p1, r1))
if abs(r1) > 80:
continue
if best_pitch is None or abs(p1) < best_pitch_value:
best_pitch = pitch
best_pitch_value = abs(p1)
print("best_pitch=%u best_pitch_value=%.1f" % (best_pitch, best_pitch_value))
if best_pitch_value <= 2.0:
return best_pitch
pitch_min = best_pitch-pdelta
pitch_max = best_pitch+pdelta
return best_pitch
def calibrate_servos(conn):
'''try to calibrate servos'''
logger.info("Starting calibration")
conn.discard_messages()
# step 1: find yaw zero by finding yaw channel value that minimises roll change on pitch channel change
yaw_zero = find_yaw_zero(conn)
#yaw_zero = 1585
ROTATIONS['level'].chan1 = yaw_zero
# step 2: find pitch zero by finding pitch channel that minimises pitch
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
pitch_zero = find_pitch_zero(conn)
#pitch_zero = 1855
ROTATIONS['level'].chan2 = pitch_zero
# step 3: find yaw scale
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r1, p1, y1 = get_attitude(conn)
print(r1, p1, y1)
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero+100)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r2, p2, y2 = get_attitude(conn)
print(r2, p2, y2)
print(y1, y2)
yawchange = util.wrap_180(y2 - y1)
YAW_SCALE = yawchange / 100.0
print("YAW_SCALE=%.2f" % YAW_SCALE)
# step 3: find pitch scale
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r1, p1, y1 = get_attitude(conn)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero+100)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r2, p2, y2 = get_attitude(conn)
print(p1, p2)
pitchchange = util.wrap_180(p2 - p1)
PITCH_SCALE = pitchchange / 100.0
print("PITCH_SCALE=%.2f" % PITCH_SCALE)
# step 4: optimise each rotation
ROTATION_LEVEL_TOLERANCE = 0
ROTATION_TOLERANCE = 0
for rotation in ['level', 'right', 'left', 'up', 'down', 'back', 'slant']:
print("optimising %s" % rotation)
set_rotation(conn, rotation, wait=True, timeout=60)
# step 5, write calibration.py
write_calibration()
def calibrate_servos(conn):
'''try to calibrate servos'''
logger.info("Starting calibration")
conn.discard_messages()
# step 1: find yaw zero by finding yaw channel value that minimises roll change on pitch channel change
yaw_zero = find_yaw_zero(conn)
#yaw_zero = 1585
ROTATIONS['level'].chan1 = yaw_zero
# step 2: find pitch zero by finding pitch channel that minimises pitch
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
pitch_zero = find_pitch_zero(conn)
#pitch_zero = 1855
ROTATIONS['level'].chan2 = pitch_zero
# step 3: find yaw scale
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r1, p1, y1 = get_attitude(conn)
print(r1, p1, y1)
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero+100)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r2, p2, y2 = get_attitude(conn)
print(r2, p2, y2)
print(y1, y2)
yawchange = util.wrap_180(y2 - y1)
YAW_SCALE = yawchange / 100.0
print("YAW_SCALE=%.2f" % YAW_SCALE)
# step 3: find pitch scale
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero)
time.sleep(1)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r1, p1, y1 = get_attitude(conn)
util.set_servo(conn.refmav, PITCH_CHANNEL, pitch_zero+100)
conn.discard_messages()
wait_quiescent(conn.refmav)
conn.discard_messages()
r2, p2, y2 = get_attitude(conn)
print(p1, p2)
pitchchange = util.wrap_180(p2 - p1)
PITCH_SCALE = pitchchange / 100.0
print("PITCH_SCALE=%.2f" % PITCH_SCALE)
# step 4: optimise each rotation
ROTATION_LEVEL_TOLERANCE = 0
ROTATION_TOLERANCE = 0
for rotation in ['level', 'right', 'left', 'up', 'down', 'back', 'slant']:
print("optimising %s" % rotation)
set_rotation(conn, rotation, wait=True, timeout=60)
# step 5, write calibration.py
write_calibration()
def gyro_integrate(conn):
'''test gyros by integrating while rotating to the given rotations'''
conn.ref.send('set streamrate -1\n')
conn.test.send('set streamrate -1\n')
util.param_set(conn.ref, 'SR0_RAW_SENS', 20)
util.param_set(conn.test, 'SR0_RAW_SENS', 20)
logger.info("Starting gyro integration")
wait_quiescent(conn.refmav)
conn.discard_messages()
if ETE == 0:
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1+200)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS['level'].chan2+200)
if ETE == 1:
ete = PixETE()
ete.position(45, 180)
time.sleep(1)
ete.rollspeed(4000)
ete.position(45, 0)
logger.info("Starting gyro motion")
start_time = time.time()
ref_tstart = None
test_tstart = [None]*3
ref_sum = Vector3()
test_sum = [Vector3(), Vector3(), Vector3()]
msgs = { 'RAW_IMU' : 0, 'SCALED_IMU2' : 1, 'SCALED_IMU3' : 2 }
while time.time() < start_time+20:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
#gyro = util.gyro_vector(imu)
gyro = Vector3(degrees(imu.xgyro*-0.001), degrees(imu.ygyro*0.001), degrees(imu.zgyro*-0.001)) #phil change.... when running this check from back to top, I found that the reference IMU X and Z were reversed.... this hack makes this pass, but I suspect something else is wrong, this should be reverted when that is found.
tnow = imu.time_usec*1.0e-6
if ref_tstart is not None:
deltat = tnow - ref_tstart
ref_sum += gyro * deltat
ref_tstart = tnow
if time.time() - start_time > 2 and gyro.length() < GYRO_TOLERANCE:
break
imu = conn.testmav.recv_match(type=msgs.keys(), blocking=False)
if imu is not None:
idx = msgs[imu.get_type()]
gyro = util.gyro_vector(imu)
if imu.get_type().startswith("SCALED_IMU"):
tnow = imu.time_boot_ms*1.0e-3
else:
tnow = imu.time_usec*1.0e-6
if test_tstart[idx] is not None:
deltat = tnow - test_tstart[idx]
test_sum[idx] += gyro * deltat
test_tstart[idx] = tnow
logger.debug("Gyro ref sums: %s" % ref_sum)
logger.debug("Gyro test sum1: %s" % test_sum[0])
logger.debug("Gyro test sum2: %s" % test_sum[1])
logger.debug("Gyro test sum3: %s" % test_sum[2])
ete.yawspeed(5000)
ete.rollspeed(5000)
ete.position(0, 0)
wait_quiescent(conn.refmav)
for idx in range(3):
err = test_sum[idx] - ref_sum
if abs(err.x) > GYRO_SUM_TOLERANCE:
util.failure("X gyro %u error: %.1f" % (idx, err.x))
if abs(err.y) > GYRO_SUM_TOLERANCE:
util.failure("Y gyro %u error: %.1f" % (idx, err.y))
if abs(err.z) > GYRO_SUM_TOLERANCE:
util.failure("Z gyro %u error: %.1f" % (idx, err.z))
logger.debug("Gyro test finished")
def optimise_attitude(conn, rotation, tolerance, timeout=25, quick=False):
'''optimise attitude using servo changes'''
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
chan1 = ROTATIONS[rotation].chan1
chan2 = ROTATIONS[rotation].chan2
attitude = wait_quiescent(conn.refmav)
time_start = time.time()
# we always do at least 2 tries. This means the attitude accuracy
# will tend to improve over time, while not adding excessive time
# per board
tries = 0
while time.time() < time_start+timeout:
#logger.info("============================= BEGIN ROTATIONS try=%s =================" % (tries))
dcm_estimated = Matrix3()
dcm_estimated.from_euler(attitude.roll, attitude.pitch, attitude.yaw)
droll = expected_roll
if droll is None:
droll = attitude.roll
else:
droll = radians(droll)
dpitch = radians(expected_pitch)
dcm_demanded = Matrix3()
dcm_demanded.from_euler(droll, dpitch, attitude.yaw)
(chan1_change, chan2_change) = gimbal_controller(dcm_estimated,
dcm_demanded, chan1)
(err_roll, err_pitch) = attitude_error(attitude, expected_roll, expected_pitch)
logger.info("optimise_attitude: %s err_roll=%.2f err_pitch=%.2f c1=%u c2=%u" % (rotation, err_roll, err_pitch, chan1, chan2))
if ((abs(err_roll)+abs(err_pitch) > 5*tolerance and
(abs(chan1_change)<1 and abs(chan2_change)<1))):
chan1_change += random.uniform(-20, 20)
chan2_change += random.uniform(-20, 20)
if (tries > 0 and (abs(err_roll)+abs(err_pitch) < tolerance or
(abs(chan1_change)<1 and abs(chan2_change)<1))):
print("roll=%.2f pitch=%.2f expected_roll=%s expected_pitch=%s" % (
degrees(attitude.roll),
degrees(attitude.pitch),
expected_roll, expected_pitch))
logger.info("%s converged %.2f %.2f tolerance %.1f" % (rotation, err_roll, err_pitch, tolerance))
# update optimised rotations to save on convergence time for the next board
ROTATIONS[rotation].chan1 = chan1
ROTATIONS[rotation].chan2 = chan2
return True
chan1 += chan1_change
chan2 += chan2_change
if chan1 < 700:
chan1 += 900
if chan2 < 700:
chan2 += 900
if chan1 > 2300:
chan1 -= 900
if chan2 > 2300:
chan2 -= 900
if chan1 < 700 or chan1 > 2300 or chan2 < 700 or chan2 > 2300:
logger.debug("servos out of range")
return False
util.set_servo(conn.refmav, YAW_CHANNEL, chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, chan2)
attitude = wait_quiescent(conn.refmav, quick=quick)
tries += 1
logger.error("timed out rotating to %s" % rotation)
return False
def calibrate_servos(conn):
'''try to calibrate servos'''
conn.ref.send('gyrocal\n')
conn.ref.expect('Calibrated')
logger.info("Starting calibration")
conn.discard_messages()
# step 1: find yaw zero by finding yaw channel value that minimises roll change on pitch channel change
yaw_zero = find_yaw_zero(conn)
#yaw_zero = ROTATIONS['level'].chan1
ROTATIONS['level'].chan1 = yaw_zero
write_calibration()
# step 2: find pitch zero by finding pitch channel that minimises pitch
util.set_servo(conn.refmav, YAW_CHANNEL, yaw_zero)
time.sleep(1)
pitch_zero = find_pitch_zero(conn)
#pitch_zero = ROTATIONS['level'].chan2
#pitch_zero = 1855
ROTATIONS['level'].chan2 = pitch_zero
write_calibration()
ok = False
for i in range(4):
ok = find_yaw_scale(conn)
if ok:
break
if not ok:
print("Error: ***** Failed to find yaw scale ****")
return
ok = False
for i in range(4):
ok = find_pitch_scale(conn)
if ok:
break
if not ok:
print("Error: ***** Failed to find pitch scale ****")
return
# step 4: optimise each rotation
ROTATION_LEVEL_TOLERANCE = 0
ROTATION_TOLERANCE = 0
guess_rotation_values()
print("trying right check")
try:
set_rotation(conn, 'right', wait=True, timeout=120)
write_calibration()
except Exception:
print("Failed right check - reversing PITCH_SCALE")
global PITCH_SCALE
PITCH_SCALE = -PITCH_SCALE
write_calibration()
for rotation in ['level', 'right', 'left', 'up', 'down', 'back']:
print("optimising %s" % rotation)
set_rotation(conn, rotation, wait=True, timeout=120)
write_calibration()
# step 5, write calibration.py
write_calibration()
