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_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_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 attitude_error(attitude, target_roll, target_pitch):
'''return tuple with attitude error as (err_roll, err_pitch)'''
roll = degrees(attitude.roll)
pitch = degrees(attitude.pitch)
if target_roll == 180 and roll < 0:
roll += 360
if target_roll is None:
err_roll = 0
else:
err_roll = abs(util.wrap_180(roll - target_roll))
err_pitch = abs(pitch - target_pitch)
return (err_roll, err_pitch)
def attitude_error(attitude, target_roll, target_pitch):
'''return tuple with attitude error as (err_roll, err_pitch)'''
roll = degrees(attitude.roll)
pitch = degrees(attitude.pitch)
if target_roll == 180 and roll < 0:
roll += 360
if target_roll is None:
err_roll = 0
else:
err_roll = abs(util.wrap_180(roll - target_roll))
err_pitch = abs(pitch - target_pitch)
return (err_roll, err_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()