def gimbal_controller(dcm_estimated, dcm_demanded, chan1): '''return the delta to chan1 and chan2''' global PITCH_SCALE, YAW_SCALE quat_est = Quaternion(dcm_estimated) quat_dem = Quaternion(dcm_demanded) quatErr = quat_division(quat_dem, quat_est) if quatErr[0] >= 0.0: scaler = 2.0 else: scaler = -2.0 bf_rates = scaler * Vector3(quatErr[1], quatErr[2], quatErr[3]) yaw_joint_rad = radians((chan1 - ROTATIONS['level'].chan1) * YAW_SCALE) chan1_change = degrees(bf_rates.z) * YAW_SCALE chan2_change = degrees(bf_rates.x * sin(yaw_joint_rad) + bf_rates.y * cos(yaw_joint_rad)) / PITCH_SCALE return (chan1_change, chan2_change)
def EarthRatesToBodyRates(dcm, earth_rates): """Convert the angular velocities from earth frame to body frame. Thanks to James Goppert for the formula all inputs and outputs are in radians returns a gyro vector in body frame, in rad/s . """ from math import sin, cos (phi, theta, psi) = dcm.to_euler() phiDot = earth_rates.x thetaDot = earth_rates.y psiDot = earth_rates.z p = phiDot - psiDot * sin(theta) q = cos(phi) * thetaDot + sin(phi) * psiDot * cos(theta) r = cos(phi) * psiDot * cos(theta) - sin(phi) * thetaDot return Vector3(p, q, r)
def magfit(logfile): '''find best magnetometer rotation fit to a log file''' print("Processing log %s" % filename) mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps) # generate 90 degree rotations rotations = generate_rotations() print("Generated %u rotations" % len(rotations)) count = 0 total_error = [0] * len(rotations) attitude = None gps = None # now gather all the data while True: m = mlog.recv_match() if m is None: break if m.get_type() == "ATTITUDE": attitude = m if m.get_type() == "GPS_RAW_INT": gps = m if m.get_type() == "RAW_IMU": mag = Vector3(m.xmag, m.ymag, m.zmag) if attitude is not None and gps is not None and gps.vel > args.min_speed * 100 and gps.fix_type >= 3: add_errors(mag, attitude, total_error, rotations) count += 1 best_i = 0 best_err = total_error[0] for i in range(len(rotations)): r = rotations[i] print("(%u,%u,%u) err=%.2f" % (r.roll, r.pitch, r.yaw, total_error[i] / count)) if total_error[i] < best_err: best_i = i best_err = total_error[i] r = rotations[best_i] print("Best rotation (%u,%u,%u) err=%.2f" % (r.roll, r.pitch, r.yaw, best_err / count))
def fit_WWW(): from scipy import optimize c = copy.copy(old_corrections) p = [c.offsets.x, c.offsets.y, c.offsets.z, c.scaling] if args.elliptical: p.extend([ c.diag.x, c.diag.y, c.diag.z, c.offdiag.x, c.offdiag.y, c.offdiag.z ]) if args.cmot: p.extend([c.cmot.x, c.cmot.y, c.cmot.z]) ofs = args.max_offset bounds = [(-ofs, ofs), (-ofs, ofs), (-ofs, ofs), (args.min_scale, args.max_scale)] if args.elliptical: for i in range(3): bounds.append((args.min_diag, args.max_diag)) for i in range(3): bounds.append((args.min_offdiag, args.max_offdiag)) if args.cmot: for i in range(3): bounds.append((-args.max_cmot, args.max_cmot)) p = optimize.fmin_slsqp(wmm_error, p, bounds=bounds) p = list(p) c.offsets = Vector3(p.pop(0), p.pop(0), p.pop(0)) c.scaling = p.pop(0) if args.elliptical: c.diag = Vector3(p.pop(0), p.pop(0), p.pop(0)) c.offdiag = Vector3(p.pop(0), p.pop(0), p.pop(0)) else: c.diag = Vector3(1.0, 1.0, 1.0) c.offdiag = Vector3(0.0, 0.0, 0.0) if args.cmot: c.cmot = Vector3(p.pop(0), p.pop(0), p.pop(0)) else: c.cmot = Vector3(0.0, 0.0, 0.0) return c
def mag_error(p, data): cx, cy, cz, r = p errout = 0 # cruse bounds mechanism if r < 0.3: errout = (0.3 - r) * 100 r = 0.3 if r > 2.0: errout = (r - 2.0) * 100 r = 2.0 corr = Vector3(cx, cy, cz) (baseline_field, baseline_throttle) = data[0] ret = [] for d in data: (mag, throttle) = d cmag = mag + corr * math.pow(throttle, r) err = (cmag - baseline_field).length() err += errout ret.append(err) return ret
def get_vicon_pose(self, object_name, segment_name): # get position in mm. Coordinates are in NED vicon_pos = self.vicon.get_segment_global_translation( object_name, segment_name) if vicon_pos is None: # Object is not in view return None, None, None, None vicon_quat = Quaternion( self.vicon.get_segment_global_quaternion(object_name, segment_name)) pos_ned = Vector3(vicon_pos * 0.001) euler = vicon_quat.euler roll, pitch, yaw = euler[0], euler[1], euler[2] yaw = math.radians(mavextra.wrap_360(math.degrees(yaw))) return pos_ned, roll, pitch, yaw
def update_arrows(self): m = self.q.to_rotation_matrix().transposed() sl = 1.1 if self.reference else 1.0 for i in self.xarrows: i.axis = vpy_vec(m * Vector3(sl, 0, 0)) i.up = vpy_vec(m * Vector3(0, 1, 0)) for i in self.yarrows: i.axis = vpy_vec(m * Vector3(0, sl, 0)) i.up = vpy_vec(m * Vector3(1, 0, 0)) for i in self.zarrows: i.axis = vpy_vec(m * Vector3(0, 0, sl)) i.up = vpy_vec(m * Vector3(1, 0, 0)) for i in self.labels: i.xoffset = scene.width * 0.07 i.yoffset = scene.width * 0.1
def velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=0): '''return summed velocity error''' sum = 0 count = 0 for i in range(0, len(vel)-1): dv = vel[i+1] - vel[i] da = Vector3() for idx in range(1+accel_indexes[i]-shift, 1+accel_indexes[i+1]-shift): da += gaccel[idx] dt1 = timestamps[i+1] - timestamps[i] dt2 = (accel_indexes[i+1] - accel_indexes[i]) * imu_dt da *= imu_dt da *= dt1/dt2 #print(accel_indexes[i+1] - accel_indexes[i]) ex = abs(dv.x - da.x) ey = abs(dv.y - da.y) sum += 0.5*(ex+ey) count += 1 if count == 0: return None return sum/count
def CalcRotationVector(self, dx, dy): angle = math.degrees(math.atan2(dy, dx)) # Make angle discrete on multiples of 45 degrees angle = angle + 22.5 - (angle + 22.5) % 45 if angle % 90 == 45: x, y = 1, 1 elif (angle / 90) % 2 == 0: x, y = 1, 0 else: x, y = 0, 1 if abs(angle) > 90: x *= -1 if angle < 0: y *= -1 # NED coordinates from the camera point of view dx, dy, dz = 0, x, y # Get rotation axis by rotating the moving vector -90 degrees on x axis self.rotation_vector = Vector3(dx, dz, -dy)
def add_arrows(self, arrowpos=Vector3(0, 0, 0), labeltext=None): if labeltext is not None: self.labels.append(label(pos=vpy_vec(arrowpos), text=labeltext)) sw = .005 if self.reference else .05 self.xarrows.append( arrow(pos=vpy_vec(arrowpos), color=color.red, opacity=1, shaftwidth=sw)) self.yarrows.append( arrow(pos=vpy_vec(arrowpos), color=color.green, opacity=1, shaftwidth=sw)) self.zarrows.append( arrow(pos=vpy_vec(arrowpos), color=color.blue, opacity=1, shaftwidth=sw)) self.update_arrows()
def BodyRatesToEarthRates(dcm, gyro): """Convert the angular velocities from body frame to earth frame. all inputs and outputs are in radians/s returns a earth rate vector. """ from math import sin, cos, tan, fabs p = gyro.x q = gyro.y r = gyro.z (phi, theta, psi) = dcm.to_euler() phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi)) thetaDot = q * cos(phi) - r * sin(phi) if fabs(cos(theta)) < 1.0e-20: theta += 1.0e-10 psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta) return Vector3(phiDot, thetaDot, psiDot)
def magfit(logfile): '''find best magnetometer offset fit to a log file''' print("Processing log %s" % filename) # open the log file flog = open(filename, mode='r') data = [] data_no_motors = [] mag = None offsets = None # now gather all the data for line in flog: if not line.startswith('COMPASS,'): continue line = line.rstrip() line = line.replace(' ', '') a = line.split(',') ofs = Vector3(float(a[4]), float(a[5]), float(a[6])) if offsets is None: initial_offsets = ofs offsets = ofs motor_ofs = Vector3(float(a[7]), float(a[8]), float(a[9])) mag = Vector3(float(a[1]), float(a[2]), float(a[3])) mag = mag - offsets data.append(mag) data_no_motors.append(mag - motor_ofs) print("Extracted %u data points" % len(data)) print("Current offsets: %s" % initial_offsets) # run the fitting algorithm ofs = initial_offsets for r in range(opts.repeat): ofs = find_offsets(data, ofs) plot_corrected_field('plot.dat', data, ofs) plot_corrected_field('initial.dat', data, initial_offsets) plot_corrected_field('zero.dat', data, Vector3(0, 0, 0)) plot_corrected_field('hand.dat', data, Vector3(-25, -8, -2)) plot_corrected_field('zero-no-motors.dat', data_no_motors, Vector3(0, 0, 0)) print('Loop %u offsets %s' % (r, ofs)) sys.stdout.flush() print("New offsets: %s" % ofs)
def magfit(logfile): '''find best magnetometer offset fit to a log file''' print("Processing log %s" % filename) mlog = mavutil.mavlink_connection(filename) data = [] throttle = 0 # now gather all the data while True: m = mlog.recv_match(condition=args.condition) if m is None: break if m.get_type() == "MAG": mag = Vector3(m.MagX, m.MagY, m.MagZ) data.append((mag, throttle)) #if m.get_type() == "RCOU": # throttle = math.sqrt(m.C1/500.0) if m.get_type() == "CTUN": throttle = m.ThO print("Extracted %u data points" % len(data)) (cmot, r) = fit_data(data) print("Fit : %s r: %s" % (cmot, r)) x = range(len(data)) errors = mag_error((cmot.x, cmot.y, cmot.z, r), data) import matplotlib.pyplot as plt plt.plot(x, errors, 'bo-') x1, x2, y1, y2 = plt.axis() plt.axis((x1, x2, 0, y2)) plt.ylabel('Error') plt.xlabel('Sample') plt.show()
def OnActuationTimer(self, evt): if not self.renderer.vehicle: return dt = evt.GetInterval() * .001 axis, speed = self.angvel angle = speed * dt self.renderer.vehicle.model.rotate(axis, angle) if self.actuation_state == 'attitude': diff = self.desired_quaternion / self.renderer.vehicle.model.quaternion diff = quaternion_to_axis_angle(diff) angle = diff.length() if abs(angle) <= 0.001: self.renderer.vehicle.model.quaternion = self.desired_quaternion self.StopActuation() if self.attitude_callback: self.attitude_callback() self.attitude_callback = None else: speed = angle / dt self.angvel = Vector3(diff.x, diff.y, diff.z), speed * .3 self.Refresh()
def drag(self, velocity, deltat=None): """Return current wind force in Earth frame. The velocity parameter is a Vector3 of the current velocity of the aircraft in earth frame, m/s .""" from math import radians # (m/s, degrees) : wind vector as a magnitude and angle. (speed, direction) = self.current(deltat=deltat) # speed = self.speed # direction = self.direction # Get the wind vector. w = toVec(speed, radians(direction)) obj_speed = velocity.length() # Compute the angle between the object vector and wind vector by taking # the dot product and dividing by the magnitudes. d = w.length() * obj_speed if d == 0: alpha = 0 else: alpha = acos((w * velocity) / d) # Get the relative wind speed and angle from the object. Note that the # relative wind speed includes the velocity of the object; i.e., there # is a headwind equivalent to the object's speed even if there is no # absolute wind. (rel_speed, beta) = apparent_wind(speed, obj_speed, alpha) # Return the vector of the relative wind, relative to the coordinate # system. relWindVec = toVec(rel_speed, beta + atan2(velocity.y, velocity.x)) # Combine them to get the acceleration vector. return Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x)), acc(relWindVec.y, drag_force(self, relWindVec.y)), 0)
def magfit(logfile): '''find best magnetometer offset fit to a log file''' print("Processing log %s" % filename) mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps) data = [] last_t = 0 offsets = Vector3(0,0,0) # now gather all the data while True: m = mlog.recv_match(condition=args.condition) if m is None: break if m.get_type() == "SENSOR_OFFSETS": # update current offsets offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) if m.get_type() == "RAW_IMU": mag = Vector3(m.xmag, m.ymag, m.zmag) # add data point after subtracting the current offsets data.append(mag - offsets + noise()) if m.get_type() == "MAG" and not args.mag2: offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ) mag = Vector3(m.MagX,m.MagY,m.MagZ) data.append(mag - offsets + noise()) if m.get_type() == "MAG2" and args.mag2: offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ) mag = Vector3(m.MagX,m.MagY,m.MagZ) data.append(mag - offsets + noise()) print("Extracted %u data points" % len(data)) print("Current offsets: %s" % offsets) orig_data = data data = select_data(data) # remove initial outliers data.sort(lambda a,b : radius_cmp(a,b,offsets)) data = data[len(data)/16:-len(data)/16] # do an initial fit (offsets, field_strength) = fit_data(data) for count in range(3): # sort the data by the radius data.sort(lambda a,b : radius_cmp(a,b,offsets)) print("Fit %u : %s field_strength=%6.1f to %6.1f" % ( count, offsets, radius(data[0], offsets), radius(data[-1], offsets))) # discard outliers, keep the middle 3/4 data = data[len(data)/8:-len(data)/8] # fit again (offsets, field_strength) = fit_data(data) print("Final : %s field_strength=%6.1f to %6.1f" % ( offsets, radius(data[0], offsets), radius(data[-1], offsets))) if args.plot: plot_data(orig_data, data)
def SetMag(self, x, y, z): self.mag = Vector3(x, y, z)
def magfit(logfile): '''find best magnetometer offset fit to a log file''' print("Processing log %s" % filename) mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps) data = [] last_t = 0 offsets = Vector3(0,0,0) motor_ofs = Vector3(0,0,0) motor = 0.0 # now gather all the data while True: m = mlog.recv_match(condition=args.condition) if m is None: break if m.get_type() == "PARAM_VALUE" and m.param_id == "RC3_MIN": rc3_min = float(m.param_value) if m.get_type() == "SENSOR_OFFSETS": # update current offsets offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) if m.get_type() == "SERVO_OUTPUT_RAW": motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw motor_pwm *= 0.25 rc3_min = mlog.param('RC3_MIN', 1100) rc3_max = mlog.param('RC3_MAX', 1900) motor = (motor_pwm - rc3_min) / (rc3_max - rc3_min) if motor > 1.0: motor = 1.0 if motor < 0.0: motor = 0.0 if m.get_type() == "RAW_IMU": mag = Vector3(m.xmag, m.ymag, m.zmag) # add data point after subtracting the current offsets data.append((mag - offsets + noise(), motor)) print("Extracted %u data points" % len(data)) print("Current offsets: %s" % offsets) data = select_data(data) # do an initial fit with all data (offsets, motor_ofs, field_strength) = fit_data(data) for count in range(3): # sort the data by the radius data.sort(lambda a,b : radius_cmp(a,b,offsets,motor_ofs)) print("Fit %u : %s %s field_strength=%6.1f to %6.1f" % ( count, offsets, motor_ofs, radius(data[0], offsets, motor_ofs), radius(data[-1], offsets, motor_ofs))) # discard outliers, keep the middle 3/4 data = data[len(data)/8:-len(data)/8] # fit again (offsets, motor_ofs, field_strength) = fit_data(data) print("Final : %s %s field_strength=%6.1f to %6.1f" % ( offsets, motor_ofs, radius(data[0], offsets, motor_ofs), radius(data[-1], offsets, motor_ofs))) print("mavgraph.py '%s' 'mag_field(RAW_IMU)' 'mag_field_motors(RAW_IMU,SENSOR_OFFSETS,(%f,%f,%f),SERVO_OUTPUT_RAW,(%f,%f,%f))'" % ( filename, offsets.x,offsets.y,offsets.z, motor_ofs.x, motor_ofs.y, motor_ofs.z))
def noise(): '''a noise vector''' from random import gauss v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1)) v.normalize() return v * args.noise
def magfit(logfile): '''find best magnetometer rotation fit to a log file''' print("Processing log %s" % filename) mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps) last_mag = None last_usec = 0 count = 0 total_error = [0] * len(rotations) AHRS_ORIENTATION = 0 COMPASS_ORIENT = 0 COMPASS_EXTERNAL = 0 last_gyr = None # now gather all the data while True: m = mlog.recv_match() if m is None: break if m.get_type() in ["PARAM_VALUE", "PARM"]: if m.get_type() == "PARM": name = str(m.Name) value = int(m.Value) else: name = str(m.param_id) value = int(m.param_value) if name == "AHRS_ORIENTATION": AHRS_ORIENTATION = value if name == 'COMPASS_ORIENT': COMPASS_ORIENT = value if name == 'COMPASS_EXTERNAL': COMPASS_EXTERNAL = value if m.get_type() in ["RAW_IMU", "MAG", "IMU"]: if m.get_type() == "RAW_IMU": mag = Vector3(m.xmag, m.ymag, m.zmag) gyr = Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001 usec = m.time_usec elif m.get_type() == "IMU": last_gyr = Vector3(m.GyrX, m.GyrY, m.GyrZ) continue elif last_gyr is not None: mag = Vector3(m.MagX, m.MagY, m.MagZ) gyr = last_gyr usec = m.TimeMS * 1000 mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL) if last_mag is not None and gyr.length() > radians( args.min_rotation): add_errors(mag, gyr, last_mag, (usec - last_usec) * 1.0e-6, total_error, rotations) count += 1 last_mag = mag last_usec = usec best_i = 0 best_err = total_error[0] for i in range(len(rotations)): r = rotations[i] if not r.is_90_degrees(): continue if args.verbose: print("%s err=%.2f" % (r, total_error[i] / count)) if total_error[i] < best_err: best_i = i best_err = total_error[i] r = rotations[best_i] print( "Current rotation is AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=%u" % (rotations[AHRS_ORIENTATION], rotations[COMPASS_ORIENT], COMPASS_EXTERNAL)) print("Best rotation is %s err=%.2f from %u points" % (r, best_err / count, count)) print( "Please set AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=1" % (rotations[AHRS_ORIENTATION], r))
def add_noise(self, throttle): """Add noise based on throttle level (from 0..1).""" self.gyro += Vector3(random.gauss(0, 1), random.gauss(0, 1), random.gauss(0, 1)) * throttle * self.gyro_noise self.accel_body += Vector3(random.gauss(0, 1), random.gauss( 0, 1), random.gauss(0, 1)) * throttle * self.accel_noise
def __init__(self): self.offsets = Vector3(0.0, 0.0, 0.0) self.diag = Vector3(1.0, 1.0, 1.0) self.offdiag = Vector3(0.0, 0.0, 0.0) self.cmot = Vector3(0.0, 0.0, 0.0) self.scaling = 1.0
def magfit(logfile): '''find best magnetometer offset fit to a log file''' print("Processing log %s" % logfile) mlog = mavutil.mavlink_connection(logfile) global earth_field, declination global data data = [] ATT = None BAT = None mag_msg = 'MAG%s' % mag_idx count = 0 parameters = {} # get parameters while True: msg = mlog.recv_match(type=['PARM']) if msg is None: break parameters[msg.Name] = msg.Value mlog.rewind() # extract MAG data while True: msg = mlog.recv_match( type=['GPS', mag_msg, 'ATT', 'CTUN', 'BARO', 'BAT'], condition=args.condition) if msg is None: break if msg.get_type() == 'GPS' and msg.Status >= 3 and earth_field is None: earth_field = mavextra.expected_earth_field(msg) (declination, inclination, intensity) = mavextra.get_mag_field_ef(msg.Lat, msg.Lng) print("Earth field: %s strength %.0f declination %.1f degrees" % (earth_field, earth_field.length(), declination)) if msg.get_type() == 'ATT': ATT = msg if msg.get_type() == 'BAT': BAT = msg if msg.get_type() == mag_msg and ATT is not None: if count % args.reduce == 0: data.append((msg, ATT, BAT)) count += 1 old_corrections.offsets = Vector3( parameters.get('COMPASS_OFS%s_X' % mag_idx, 0.0), parameters.get('COMPASS_OFS%s_Y' % mag_idx, 0.0), parameters.get('COMPASS_OFS%s_Z' % mag_idx, 0.0)) old_corrections.diag = Vector3( parameters.get('COMPASS_DIA%s_X' % mag_idx, 1.0), parameters.get('COMPASS_DIA%s_Y' % mag_idx, 1.0), parameters.get('COMPASS_DIA%s_Z' % mag_idx, 1.0)) old_corrections.offdiag = Vector3( parameters.get('COMPASS_ODI%s_X' % mag_idx, 0.0), parameters.get('COMPASS_ODI%s_Y' % mag_idx, 0.0), parameters.get('COMPASS_ODI%s_Z' % mag_idx, 0.0)) if parameters.get('COMPASS_MOTCT', 0) == 2: # only support current based corrections for now old_corrections.cmot = Vector3( parameters.get('COMPASS_MOT%s_X' % mag_idx, 0.0), parameters.get('COMPASS_MOT%s_Y' % mag_idx, 0.0), parameters.get('COMPASS_MOT%s_Z' % mag_idx, 0.0)) old_corrections.scaling = parameters.get('COMPASS_SCALE%s' % mag_idx, None) if old_corrections.scaling is None or old_corrections.scaling < 0.1: force_scale = False old_corrections.scaling = 1.0 else: force_scale = True # remove existing corrections data2 = [] for (MAG, ATT, BAT) in data: if remove_offsets(MAG, BAT, old_corrections): data2.append((MAG, ATT, BAT)) data = data2 print("Extracted %u points" % len(data)) print("Current: %s diag: %s offdiag: %s cmot: %s scale: %.2f" % (old_corrections.offsets, old_corrections.diag, old_corrections.offdiag, old_corrections.cmot, old_corrections.scaling)) # do fit c = fit_WWW() # normalise diagonals to scale factor if force_scale: avgdiag = (c.diag.x + c.diag.y + c.diag.z) / 3.0 calc_scale = c.scaling c.scaling *= avgdiag if c.scaling > args.max_scale: c.scaling = args.max_scale if c.scaling < args.min_scale: c.scaling = args.min_scale scale_change = c.scaling / calc_scale c.diag *= 1.0 / scale_change c.offdiag *= 1.0 / scale_change print("New: %s diag: %s offdiag: %s cmot: %s scale: %.2f" % (c.offsets, c.diag, c.offdiag, c.cmot, c.scaling)) x = [] corrected = {} corrected['Yaw'] = [] expected1 = {} expected2 = {} uncorrected = {} uncorrected['Yaw'] = [] yaw_change1 = [] yaw_change2 = [] for i in range(len(data)): (MAG, ATT, BAT) = data[i] yaw1 = get_yaw(ATT, MAG, BAT, c) corrected['Yaw'].append(yaw1) ef1 = expected_field(ATT, yaw1) cf = correct(MAG, BAT, c) yaw2 = get_yaw(ATT, MAG, BAT, old_corrections) ef2 = expected_field(ATT, yaw2) uncorrected['Yaw'].append(yaw2) uf = correct(MAG, BAT, old_corrections) yaw_change1.append(mavextra.wrap_180(yaw1 - yaw2)) yaw_change2.append(mavextra.wrap_180(yaw1 - ATT.Yaw)) for axis in ['x', 'y', 'z']: if not axis in corrected: corrected[axis] = [] uncorrected[axis] = [] expected1[axis] = [] expected2[axis] = [] corrected[axis].append(getattr(cf, axis)) uncorrected[axis].append(getattr(uf, axis)) expected1[axis].append(getattr(ef1, axis)) expected2[axis].append(getattr(ef2, axis)) x.append(i) c.show_parms() fig, axs = pyplot.subplots(3, 1, sharex=True) for axis in ['x', 'y', 'z']: axs[0].plot(numpy.array(x), numpy.array(uncorrected[axis]), label='Uncorrected %s' % axis.upper()) axs[0].plot(numpy.array(x), numpy.array(expected2[axis]), label='Expected %s' % axis.upper()) axs[0].legend(loc='upper left') axs[0].set_title('Original') axs[0].set_ylabel('Field (mGauss)') axs[1].plot(numpy.array(x), numpy.array(corrected[axis]), label='Corrected %s' % axis.upper()) axs[1].plot(numpy.array(x), numpy.array(expected1[axis]), label='Expected %s' % axis.upper()) axs[1].legend(loc='upper left') axs[1].set_title('Corrected') axs[1].set_ylabel('Field (mGauss)') # show change in yaw estimate from old corrections to new axs[2].plot(numpy.array(x), numpy.array(yaw_change1), label='Mag Yaw Change') axs[2].plot(numpy.array(x), numpy.array(yaw_change2), label='ATT Yaw Change') axs[2].set_title('Yaw Change (degrees)') axs[2].legend(loc='upper left') pyplot.show()
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' if not self.mpstate.map: # don't draw if no map return if m.get_type() != 'GIMBAL_REPORT': return needed = ['ATTITUDE', 'GLOBAL_POSITION_INT'] for n in needed: if not n in self.master.messages: return # clear the camera icon self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView')) gpi = self.master.messages['GLOBAL_POSITION_INT'] att = self.master.messages['ATTITUDE'] vehicle_dcm = Matrix3() vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw) rotmat_copter_gimbal = Matrix3() rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el, m.joint_az) gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal lat = gpi.lat * 1.0e-7 lon = gpi.lon * 1.0e-7 alt = gpi.relative_alt * 1.0e-3 # ground plane ground_plane = Plane() # the position of the camera in the air, remembering its a right # hand coordinate system, so +ve z is down camera_point = Vector3(0, 0, -alt) # get view point of camera when not rotated view_point = Vector3(1, 0, 0) # rotate view_point to form current view vector rot_point = gimbal_dcm * view_point # a line from the camera to the ground line = Line(camera_point, rot_point) # find the intersection with the ground pt = line.plane_intersection(ground_plane, forward_only=True) if pt is None: # its pointing up into the sky return None (view_lat, view_lon) = mp_util.gps_offset(lat, lon, pt.y, pt.x) icon = self.mpstate.map.icon('camera-small-red.png') self.mpstate.map.add_object( mp_slipmap.SlipIcon('gimbalview', (view_lat, view_lon), icon, layer='GimbalView', rotation=0, follow=False))
def gps_lag(logfile): '''work out gps velocity lag times for a log file''' print("Processing log %s" % filename) mlog = mavutil.mavlink_connection(filename) timestamps = [] vel = [] gaccel = [] accel_indexes = [] ATT = None IMU = None dtsum = 0 dtcount = 0 while True: m = mlog.recv_match(type=['GPS','IMU','ATT']) if m is None: break t = m.get_type() if t == 'GPS' and m.Status >= 3 and m.Spd>args.minspeed: v = Vector3(m.Spd*cos(radians(m.GCrs)), m.Spd*sin(radians(m.GCrs)), m.VZ) vel.append(v) timestamps.append(m._timestamp) accel_indexes.append(max(len(gaccel)-1,0)) elif t == 'ATT': ATT = m elif t == 'IMU': if ATT is not None: gaccel.append(earth_accel_df(m, ATT)) if IMU is not None: dt = m._timestamp - IMU._timestamp dtsum += dt dtcount += 1 IMU = m imu_dt = dtsum / dtcount print("Loaded %u samples imu_dt=%.3f" % (len(vel), imu_dt)) besti = -1 besterr = 0 delays = [] errors = [] for i in range(0,100): err = velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=i) if err is None: break errors.append(err) delays.append(i*imu_dt) if besti == -1 or err < besterr: besti = i besterr = err print("Best %u (%.3fs) %f" % (besti, besti*imu_dt, besterr)) if args.plot: import matplotlib.pyplot as plt plt.plot(delays, errors, 'bo-') x1,x2,y1,y2 = plt.axis() plt.axis((x1,x2,0,y2)) plt.ylabel('Error') plt.xlabel('Delay(s)') plt.show()
def SetEuler(self, roll, pitch, yaw, callback=None): self.desired_quaternion = Quaternion((roll, pitch, yaw)) self.attitude_callback = callback self.actuation_state = 'attitude' self.angvel = Vector3(1, 0, 0), 0 self.StartActuation()
def IMUfit(logfile): '''find IMU calibration parameters from a log file''' print("Processing log %s" % logfile) mlog = mavutil.mavlink_connection(logfile) data = IMUData() c = Coefficients() orientation = 0 stop_capture = [False] * 3 if args.tclr: messages = ['PARM', 'TCLR'] else: messages = ['PARM', 'IMU'] while True: msg = mlog.recv_match(type=messages) if msg is None: break if msg.get_type() == 'PARM': # build up the old coefficients so we can remove the impact of # existing coefficients from the data m = re.match("^INS_TCAL(\d)_ENABLE$", msg.Name) if m: imu = int(m.group(1)) - 1 if stop_capture[imu]: continue if msg.Value == 1 and c.enable[imu] == 2: print("TCAL[%u] enabled" % imu) stop_capture[imu] = True continue if msg.Value == 0 and c.enable[imu] == 1: print("TCAL[%u] disabled" % imu) stop_capture[imu] = True continue c.set_enable(imu, msg.Value) m = re.match("^INS_TCAL(\d)_(ACC|GYR)([1-3])_([XYZ])$", msg.Name) if m: imu = int(m.group(1)) - 1 stype = m.group(2) p = int(m.group(3)) axis = m.group(4) if stop_capture[imu]: continue if stype == 'ACC': c.set_acoeff(imu, axis, p, msg.Value / SCALE_FACTOR) if stype == 'GYR': c.set_gcoeff(imu, axis, p, msg.Value / SCALE_FACTOR) m = re.match("^INS_TCAL(\d)_TMIN$", msg.Name) if m: imu = int(m.group(1)) - 1 if stop_capture[imu]: continue c.set_tmin(imu, msg.Value) m = re.match("^INS_TCAL(\d)_TMAX", msg.Name) if m: imu = int(m.group(1)) - 1 if stop_capture[imu]: continue c.set_tmax(imu, msg.Value) m = re.match("^INS_GYR(\d)_CALTEMP", msg.Name) if m: imu = int(m.group(1)) - 1 if stop_capture[imu]: continue c.set_gyro_tcal(imu, msg.Value) m = re.match("^INS_ACC(\d)_CALTEMP", msg.Name) if m: imu = int(m.group(1)) - 1 if stop_capture[imu]: continue c.set_accel_tcal(imu, msg.Value) m = re.match("^INS_(ACC|GYR)(\d?)OFFS_([XYZ])$", msg.Name) if m: stype = m.group(1) if m.group(2) == "": imu = 0 else: imu = int(m.group(2)) - 1 axis = m.group(3) if stop_capture[imu]: continue if stype == 'ACC': c.set_aoffset(imu, axis, msg.Value) if stype == 'GYR': c.set_goffset(imu, axis, msg.Value) if msg.Name == 'AHRS_ORIENTATION': orientation = int(msg.Value) print("Using orientation %d" % orientation) if msg.get_type() == 'TCLR' and args.tclr: imu = msg.I T = msg.Temp if msg.SType == 0: # accel acc = Vector3(msg.X, msg.Y, msg.Z) time = msg.TimeUS * 1.0e-6 data.add_accel(imu, T, time, acc) elif msg.SType == 1: # gyro gyr = Vector3(msg.X, msg.Y, msg.Z) time = msg.TimeUS * 1.0e-6 data.add_gyro(imu, T, time, gyr) if msg.get_type() == 'IMU' and not args.tclr: imu = msg.I if stop_capture[imu]: continue T = msg.T acc = Vector3(msg.AccX, msg.AccY, msg.AccZ) gyr = Vector3(msg.GyrX, msg.GyrY, msg.GyrZ) # invert the board orientation rotation. Corrections are in sensor frame if orientation != 0: acc = acc.rotate_by_inverse_id(orientation) gyr = gyr.rotate_by_inverse_id(orientation) if acc is None or gyr is None: print("Invalid AHRS_ORIENTATION %u" % orientation) sys.exit(1) if c.enable[imu] == 1: acc -= c.correction_accel(imu, T) gyr -= c.correction_gyro(imu, T) time = msg.TimeUS * 1.0e-6 data.add_accel(imu, T, time, acc) data.add_gyro(imu, T, time, gyr) if len(data.IMUs()) == 0: print("No data found") sys.exit(1) print("Loaded %u accel and %u gyro samples" % (len(data.accel[0]['T']), len(data.gyro[0]['T']))) if not args.tclr: # apply moving average filter with 2s width data.Filter(2) clog = c c = Coefficients() calfile = open(args.outfile, "w") for imu in data.IMUs(): tmin = np.amin(data.accel[imu]['T']) tmax = np.amax(data.accel[imu]['T']) c.set_tmin(imu, tmin) c.set_tmax(imu, tmax) for axis in AXES: if args.online: fit = OnlineIMUfit() trel = data.accel[imu]['T'] - TEMP_REF ofs = data.accel_at_temp(imu, axis, clog.atcal[imu]) c.set_accel_poly( imu, axis, fit.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER)) trel = data.gyro[imu]['T'] - TEMP_REF c.set_gyro_poly( imu, axis, fit.polyfit(trel, data.gyro[imu][axis], POLY_ORDER)) else: trel = data.accel[imu]['T'] - TEMP_REF if imu in clog.atcal: ofs = data.accel_at_temp(imu, axis, clog.atcal[imu]) else: ofs = np.mean(data.accel[imu][axis]) c.set_accel_poly( imu, axis, np.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER)) trel = data.gyro[imu]['T'] - TEMP_REF c.set_gyro_poly( imu, axis, np.polyfit(trel, data.gyro[imu][axis], POLY_ORDER)) params = c.param_string(imu) print(params) calfile.write(params) calfile.close() print("Calibration written to %s" % args.outfile) if args.no_graph: return fig, axs = pyplot.subplots(len(data.IMUs()), 1, sharex=True) num_imus = len(data.IMUs()) if num_imus == 1: axs = [axs] for imu in data.IMUs(): scale = math.degrees(1) for axis in AXES: axs[imu].plot(data.gyro[imu]['time'], data.gyro[imu][axis] * scale, label='Uncorrected %s' % axis) for axis in AXES: poly = np.poly1d(c.gcoef[imu][axis]) trel = data.gyro[imu]['T'] - TEMP_REF correction = poly(trel) axs[imu].plot(data.gyro[imu]['time'], (data.gyro[imu][axis] - correction) * scale, label='Corrected %s' % axis) if args.log_parm: for axis in AXES: if clog.enable[imu] == 0.0: print("IMU[%u] disabled in log parms" % imu) continue poly = np.poly1d(clog.gcoef[imu][axis]) correction = poly(data.gyro[imu]['T'] - TEMP_REF) - poly( clog.gtcal[imu] - TEMP_REF) + clog.gofs[imu][axis] axs[imu].plot(data.gyro[imu]['time'], (data.gyro[imu][axis] - correction) * scale, label='Corrected %s (log parm)' % axis) ax2 = axs[imu].twinx() ax2.plot(data.gyro[imu]['time'], data.gyro[imu]['T'], label='Temperature(C)', color='black') ax2.legend(loc='upper right') axs[imu].legend(loc='upper left') axs[imu].set_title('IMU[%u] Gyro (deg/s)' % imu) fig, axs = pyplot.subplots(num_imus, 1, sharex=True) if num_imus == 1: axs = [axs] for imu in data.IMUs(): for axis in AXES: ofs = data.accel_at_temp(imu, axis, clog.atcal.get(imu, TEMP_REF)) axs[imu].plot(data.accel[imu]['time'], data.accel[imu][axis] - ofs, label='Uncorrected %s' % axis) for axis in AXES: poly = np.poly1d(c.acoef[imu][axis]) trel = data.accel[imu]['T'] - TEMP_REF correction = poly(trel) ofs = data.accel_at_temp(imu, axis, clog.atcal.get(imu, TEMP_REF)) axs[imu].plot(data.accel[imu]['time'], (data.accel[imu][axis] - ofs) - correction, label='Corrected %s' % axis) if args.log_parm: for axis in AXES: if clog.enable[imu] == 0.0: print("IMU[%u] disabled in log parms" % imu) continue poly = np.poly1d(clog.acoef[imu][axis]) ofs = data.accel_at_temp(imu, axis, clog.atcal[imu]) correction = poly(data.accel[imu]['T'] - TEMP_REF) - poly(clog.atcal[imu] - TEMP_REF) axs[imu].plot(data.accel[imu]['time'], (data.accel[imu][axis] - ofs) - correction, label='Corrected %s (log parm)' % axis) ax2 = axs[imu].twinx() ax2.plot(data.accel[imu]['time'], data.accel[imu]['T'], label='Temperature(C)', color='black') ax2.legend(loc='upper right') axs[imu].legend(loc='upper left') axs[imu].set_title('IMU[%u] Accel (m/s^2)' % imu) pyplot.show()
def vector_to_AP(vec): '''convert pybullet vector tuple to ArduPilot Vector3''' return Vector3(vec[0], -vec[1], -vec[2])
def toVec(magnitude, angle): """Converts a magnitude and angle (radians) to a vector in the xy plane.""" v = Vector3(magnitude, 0, 0) m = Matrix3() m.from_euler(0, 0, angle) return m.transposed() * v
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/AP_GeodesicGrid.h and https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/AP_GeodesicGrid.cpp as reference for defining the geodesic sections and implementing almost all functions. Those files should be consulted for implementation details. ''' import math from pymavlink.rotmat import Matrix3, Vector3 # The golden number below was obtained from scipy.constants.golden. Let's use # the literal value here as this is the only place that would require scipy # module. g = 1.618033988749895 _first_half = ( (Vector3(-g, 1, 0), Vector3(-1, 0, -g), Vector3(-g, -1, 0)), (Vector3(-1, 0, -g), Vector3(-g, -1, 0), Vector3(0, -g, -1)), (Vector3(-g, -1, 0), Vector3(0, -g, -1), Vector3(0, -g, 1)), (Vector3(-1, 0, -g), Vector3(0, -g, -1), Vector3(1, 0, -g)), (Vector3(0, -g, -1), Vector3(0, -g, 1), Vector3(g, -1, 0)), (Vector3(0, -g, -1), Vector3(1, 0, -g), Vector3(g, -1, 0)), (Vector3(g, -1, 0), Vector3(1, 0, -g), Vector3(g, 1, 0)), (Vector3(1, 0, -g), Vector3(g, 1, 0), Vector3(0, g, -1)), (Vector3(1, 0, -g), Vector3(0, g, -1), Vector3(-1, 0, -g)), (Vector3(0, g, -1), Vector3(-g, 1, 0), Vector3(-1, 0, -g)), ) _second_half = tuple((-a, -b, -c) for a, b, c in _first_half) triangles = _first_half + _second_half radius = math.sqrt(1 + g**2)