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