#!/usr/bin/env python ''' useful extra functions for use by mavlink clients Copyright Andrew Tridgell 2011 Released under GNU GPL version 3 or later ''' from __future__ import print_function from __future__ import absolute_import from builtins import object from builtins import sum as builtin_sum from math import * try: # in case numpy isn't installed from .quaternion import Quaternion except: pass try: # rotmat doesn't work on Python3.2 yet from .rotmat import Vector3, Matrix3 except Exception: pass def kmh(mps): '''convert m/s to Km/h''' return mps*3.6 def altitude(SCALED_PRESSURE, ground_pressure=None, ground_temp=None): '''calculate barometric altitude''' from . import mavutil self = mavutil.mavfile_global if ground_pressure is None: if self.param('GND_ABS_PRESS', None) is None: return 0 ground_pressure = self.param('GND_ABS_PRESS', 1) if ground_temp is None: ground_temp = self.param('GND_TEMP', 0) scaling = ground_pressure / (SCALED_PRESSURE.press_abs*100.0) temp = ground_temp + 273.15 return log(scaling) * temp * 29271.267 * 0.001 def altitude2(SCALED_PRESSURE, ground_pressure=None, ground_temp=None): '''calculate barometric altitude''' from . import mavutil self = mavutil.mavfile_global if ground_pressure is None: if self.param('GND_ABS_PRESS', None) is None: return 0 ground_pressure = self.param('GND_ABS_PRESS', 1) if ground_temp is None: ground_temp = self.param('GND_TEMP', 0) scaling = SCALED_PRESSURE.press_abs*100.0 / ground_pressure temp = ground_temp + 273.15 return 153.8462 * temp * (1.0 - exp(0.190259 * log(scaling))) def mag_heading(RAW_IMU, ATTITUDE, declination=None, SENSOR_OFFSETS=None, ofs=None): '''calculate heading from raw magnetometer''' if declination is None: from . import mavutil declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0)) mag_x = RAW_IMU.xmag mag_y = RAW_IMU.ymag mag_z = RAW_IMU.zmag if SENSOR_OFFSETS is not None and ofs is not None: mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z # go via a DCM matrix to match the APM calculation dcm_matrix = rotation(ATTITUDE) cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x) headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z) heading = degrees(atan2(-headY,headX)) + declination if heading < 0: heading += 360 return heading def mag_field_df(MAG, ofs=None, diagonals=(1.0,1.0,1.0), offdiagonals=(0.0,0.0,0.0)): '''calculate magnetic field strength from raw magnetometer for DF ''' mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ) if ofs is not None: mag += Vector3(ofs[0],ofs[1],ofs[2]) - Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ) diagonals = Vector3(diagonals[0], diagonals[1], diagonals[2]) offdiagonals = Vector3(offdiagonals[0], offdiagonals[1], offdiagonals[2]) rot = Matrix3(Vector3(diagonals.x, offdiagonals.x, offdiagonals.y), Vector3(offdiagonals.x, diagonals.y, offdiagonals.z), Vector3(offdiagonals.y, offdiagonals.z, diagonals.z)) mag = rot * mag return mag.length() def mag_heading_df(MAG, ATT, declination=None, ofs=None, diagonals=(1.0,1.0,1.0), offdiagonals=(0.0,0.0,0.0)): '''calculate heading from raw magnetometer''' if declination is None: from pymavlink import mavutil declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0)) mag = Vector3(MAG.MagX,MAG.MagY,MAG.MagZ) if ofs is not None: mag += Vector3(ofs[0],ofs[1],ofs[2]) - Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ) diagonals = Vector3(diagonals[0], diagonals[1], diagonals[2]) offdiagonals = Vector3(offdiagonals[0], offdiagonals[1], offdiagonals[2]) rot = Matrix3(Vector3(diagonals.x, offdiagonals.x, offdiagonals.y), Vector3(offdiagonals.x, diagonals.y, offdiagonals.z), Vector3(offdiagonals.y, offdiagonals.z, diagonals.z)) mag = rot * mag # go via a DCM matrix to match the APM calculation dcm_matrix = rotation_df(ATT) cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x) headY = mag.y * dcm_matrix.c.z - mag.z * dcm_matrix.c.y headX = mag.x * cos_pitch_sq - dcm_matrix.c.x * (mag.y * dcm_matrix.c.y + mag.z * dcm_matrix.c.z) heading = degrees(atan2(-headY,headX)) + declination if heading < 0: heading += 360 return heading def gps_time_to_epoch(week, msec): '''convert GPS week and TOW to a time in seconds since 1970''' epoch = 86400*(10*365 + int((1980-1969)/4) + 1 + 6 - 2) return epoch + 86400*7*week + msec*0.001 - 18 def mag_heading_motors(RAW_IMU, ATTITUDE, declination, SENSOR_OFFSETS, ofs, SERVO_OUTPUT_RAW, motor_ofs): '''calculate heading from raw magnetometer''' ofs = get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs) if declination is None: from . import mavutil declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0)) mag_x = RAW_IMU.xmag mag_y = RAW_IMU.ymag mag_z = RAW_IMU.zmag if SENSOR_OFFSETS is not None and ofs is not None: mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch) headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll) heading = degrees(atan2(-headY,headX)) + declination if heading < 0: heading += 360 return heading def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None): '''calculate magnetic field strength from raw magnetometer''' mag_x = RAW_IMU.xmag mag_y = RAW_IMU.ymag mag_z = RAW_IMU.zmag if SENSOR_OFFSETS is not None and ofs is not None: mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z return sqrt(mag_x**2 + mag_y**2 + mag_z**2) def mag_field_df(MAG, ofs=None): '''calculate magnetic field strength from raw magnetometer (dataflash version)''' mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ) offsets = Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ) if ofs is not None: mag = (mag - offsets) + Vector3(ofs[0], ofs[1], ofs[2]) return mag.length() def get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs): '''calculate magnetic field strength from raw magnetometer''' from . import mavutil self = mavutil.mavfile_global m = SERVO_OUTPUT_RAW motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw motor_pwm *= 0.25 rc3_min = self.param('RC3_MIN', 1100) rc3_max = self.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 motor_offsets0 = motor_ofs[0] * motor motor_offsets1 = motor_ofs[1] * motor motor_offsets2 = motor_ofs[2] * motor ofs = (ofs[0] + motor_offsets0, ofs[1] + motor_offsets1, ofs[2] + motor_offsets2) return ofs def mag_field_motors(RAW_IMU, SENSOR_OFFSETS, ofs, SERVO_OUTPUT_RAW, motor_ofs): '''calculate magnetic field strength from raw magnetometer''' mag_x = RAW_IMU.xmag mag_y = RAW_IMU.ymag mag_z = RAW_IMU.zmag ofs = get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs) if SENSOR_OFFSETS is not None and ofs is not None: mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z return sqrt(mag_x**2 + mag_y**2 + mag_z**2) def angle_diff(angle1, angle2): '''show the difference between two angles in degrees''' ret = angle1 - angle2 if ret > 180: ret -= 360; if ret < -180: ret += 360 return ret average_data = {} def average(var, key, N): '''average over N points''' global average_data if not key in average_data: average_data[key] = [var]*N return var l = average_data[key] l.pop(0) l.append(var) return builtin_sum(l) / N derivative_data = {} def second_derivative_5(var, key): '''5 point 2nd derivative''' global derivative_data from . import mavutil tnow = mavutil.mavfile_global.timestamp if not key in derivative_data: derivative_data[key] = (tnow, [var]*5) return 0 (last_time, data) = derivative_data[key] data.pop(0) data.append(var) derivative_data[key] = (tnow, data) h = (tnow - last_time) # N=5 2nd derivative from # http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ ret = ((data[4] + data[0]) - 2*data[2]) / (4*h**2) return ret def second_derivative_9(var, key): '''9 point 2nd derivative''' global derivative_data from . import mavutil tnow = mavutil.mavfile_global.timestamp if not key in derivative_data: derivative_data[key] = (tnow, [var]*9) return 0 (last_time, data) = derivative_data[key] data.pop(0) data.append(var) derivative_data[key] = (tnow, data) h = (tnow - last_time) # N=5 2nd derivative from # http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ f = data ret = ((f[8] + f[0]) + 4*(f[7] + f[1]) + 4*(f[6]+f[2]) - 4*(f[5]+f[3]) - 10*f[4])/(64*h**2) return ret lowpass_data = {} def lowpass(var, key, factor): '''a simple lowpass filter''' global lowpass_data if not key in lowpass_data: lowpass_data[key] = var else: lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var return lowpass_data[key] def lpalpha(sample_rate_hz, cutoff_hz): '''find alpha for low pass filter''' rc = 1.0 / (2*pi*cutoff_hz) dt = 1.0 / sample_rate_hz return 1.0 - dt/(dt+rc) lowpass_hz_data = {} def lowpassHz(var, key, sample_rate_hz, cutoff_hz): '''a simple lowpass filter with specified frequency''' global lowpass_hz_data alpha = lpalpha(sample_rate_hz, cutoff_hz) if not key in lowpass_hz_data: lowpass_hz_data[key] = var else: lowpass_hz_data[key] = alpha*lowpass_hz_data[key] + (1.0-alpha)*var return lowpass_hz_data[key] last_diff = {} def diff(var, key): '''calculate differences between values''' global last_diff ret = 0 if not key in last_diff: last_diff[key] = var return 0 ret = var - last_diff[key] last_diff[key] = var return ret last_delta = {} def delta(var, key, tusec=None): '''calculate slope''' if isnan(var): return None global last_delta if tusec is not None: tnow = tusec * 1.0e-6 else: from . import mavutil tnow = mavutil.mavfile_global.timestamp ret = 0 if key in last_delta: (last_v, last_t, last_ret) = last_delta[key] if last_t == tnow: return last_ret if tnow == last_t: ret = 0 else: ret = (var - last_v) / (tnow - last_t) last_delta[key] = (var, tnow, ret) return ret last_sum = {} def sum(var, key): '''sum variable''' global last_sum ret = 0 if not key in last_sum: last_sum[key] = 0 last_sum[key] += var return last_sum[key] last_integral = {} def integral(var, key, timeus): '''integrate variable''' global last_integral ret = 0 if not key in last_integral: last_integral[key] = (0,timeus) (lastsum,lastt) = last_integral[key] dt = (timeus - lastt) * 1.0e-6 dv = var * dt newv = lastsum + dv last_integral[key] = (newv,timeus) return newv def delta_angle(var, key, tusec=None): '''calculate slope of an angle''' global last_delta if tusec is not None: tnow = tusec * 1.0e-6 else: from . import mavutil tnow = mavutil.mavfile_global.timestamp dv = 0 ret = 0 if key in last_delta: (last_v, last_t, last_ret) = last_delta[key] if last_t == tnow: return last_ret if tnow == last_t: ret = 0 else: dv = var - last_v if dv > 180: dv -= 360 if dv < -180: dv += 360 ret = dv / (tnow - last_t) last_delta[key] = (var, tnow, ret) return ret def roll_estimate(RAW_IMU,GPS_RAW_INT=None,ATTITUDE=None,SENSOR_OFFSETS=None, ofs=None, mul=None,smooth=0.7): '''estimate roll from accelerometer''' rx = RAW_IMU.xacc * 9.81 / 1000.0 ry = RAW_IMU.yacc * 9.81 / 1000.0 rz = RAW_IMU.zacc * 9.81 / 1000.0 if ATTITUDE is not None and GPS_RAW_INT is not None: ry -= ATTITUDE.yawspeed * GPS_RAW_INT.vel*0.01 rz += ATTITUDE.pitchspeed * GPS_RAW_INT.vel*0.01 if SENSOR_OFFSETS is not None and ofs is not None: rx += SENSOR_OFFSETS.accel_cal_x ry += SENSOR_OFFSETS.accel_cal_y rz += SENSOR_OFFSETS.accel_cal_z rx -= ofs[0] ry -= ofs[1] rz -= ofs[2] if mul is not None: rx *= mul[0] ry *= mul[1] rz *= mul[2] return lowpass(degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))),'_roll',smooth) def pitch_estimate(RAW_IMU, GPS_RAW_INT=None,ATTITUDE=None, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7): '''estimate pitch from accelerometer''' rx = RAW_IMU.xacc * 9.81 / 1000.0 ry = RAW_IMU.yacc * 9.81 / 1000.0 rz = RAW_IMU.zacc * 9.81 / 1000.0 if ATTITUDE is not None and GPS_RAW_INT is not None: ry -= ATTITUDE.yawspeed * GPS_RAW_INT.vel*0.01 rz += ATTITUDE.pitchspeed * GPS_RAW_INT.vel*0.01 if SENSOR_OFFSETS is not None and ofs is not None: rx += SENSOR_OFFSETS.accel_cal_x ry += SENSOR_OFFSETS.accel_cal_y rz += SENSOR_OFFSETS.accel_cal_z rx -= ofs[0] ry -= ofs[1] rz -= ofs[2] if mul is not None: rx *= mul[0] ry *= mul[1] rz *= mul[2] return lowpass(degrees(asin(rx/sqrt(rx**2+ry**2+rz**2))),'_pitch',smooth) def rotation(ATTITUDE): '''return the current DCM rotation matrix''' r = Matrix3() if hasattr(ATTITUDE, 'roll'): r.from_euler(ATTITUDE.roll, ATTITUDE.pitch, ATTITUDE.yaw) else: r.from_euler(radians(ATTITUDE.Roll), radians(ATTITUDE.Pitch), radians(ATTITUDE.Yaw)) return r def mag_rotation(RAW_IMU, inclination, declination): '''return an attitude rotation matrix that is consistent with the current mag vector''' m_body = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag) m_earth = Vector3(m_body.length(), 0, 0) r = Matrix3() r.from_euler(0, -radians(inclination), radians(declination)) m_earth = r * m_earth r.from_two_vectors(m_earth, m_body) return r def mag_pitch(RAW_IMU, inclination, declination): '''estimate pithc from mag''' m = mag_rotation(RAW_IMU, inclination, declination) (r, p, y) = m.to_euler() return degrees(p) def mag_roll(RAW_IMU, inclination, declination): '''estimate roll from mag''' m = mag_rotation(RAW_IMU, inclination, declination) (r, p, y) = m.to_euler() return degrees(r) def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7): '''estimate pitch from accelerometer''' if hasattr(RAW_IMU, 'xacc'): rx = RAW_IMU.xacc * 9.81 / 1000.0 ry = RAW_IMU.yacc * 9.81 / 1000.0 rz = RAW_IMU.zacc * 9.81 / 1000.0 else: rx = RAW_IMU.AccX ry = RAW_IMU.AccY rz = RAW_IMU.AccZ if SENSOR_OFFSETS is not None and ofs is not None: rx += SENSOR_OFFSETS.accel_cal_x ry += SENSOR_OFFSETS.accel_cal_y rz += SENSOR_OFFSETS.accel_cal_z rx -= ofs[0] ry -= ofs[1] rz -= ofs[2] if mul is not None: rx *= mul[0] ry *= mul[1] rz *= mul[2] return sqrt(rx**2+ry**2+rz**2) def pitch_sim(SIMSTATE, GPS_RAW): '''estimate pitch from SIMSTATE accels''' xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9) zacc = SIMSTATE.zacc zacc += SIMSTATE.ygyro * GPS_RAW.v; if xacc/zacc >= 1: return 0 if xacc/zacc <= -1: return -0 return degrees(-asin(xacc/zacc)) ORGN = None def get_origin(): global ORGN if ORGN is not None: return ORGN from . import mavutil self = mavutil.mavfile_global ret = self.messages.get('ORGN', None) if ret is None: ret = self.messages.get('GPS', None) if ret.Status < 3: return None ORGN = ret return ret # graph distance_two(GPS,XKF1[0]) def get_lat_lon_alt(MSG): '''gets lat and lon in radians and alt in meters from a position msg''' if hasattr(MSG, 'Lat') and hasattr(MSG, 'Lng'): lat = radians(MSG.Lat) lon = radians(MSG.Lng) alt = MSG.Alt elif hasattr(MSG, 'Lat') and hasattr(MSG, 'Lon'): lat = radians(MSG.Lat) lon = radians(MSG.Lon) alt = MSG.Alt elif hasattr(MSG, 'cog'): lat = radians(MSG.lat)*1.0e-7 lon = radians(MSG.lon)*1.0e-7 alt = MSG.alt*0.001 elif hasattr(MSG,'lat') and hasattr(MSG,'lon'): lat = radians(MSG.lat) lon = radians(MSG.lon) alt = MSG.alt*0.001 elif hasattr(MSG,'lat') and hasattr(MSG,'lng'): lat = radians(MSG.lat) lon = radians(MSG.lng) alt = MSG.alt*0.001 elif hasattr(MSG, 'PN'): # origin relative position from EKF (lat,lon,alt) = ekf1_pos(MSG) lat = radians(lat) lon = radians(lon) else: return None return (lat, lon, alt) def _distance_two(MSG1, MSG2, horizontal=True): ''' Return the distance between two points in metres Calculated as the great-circle distance using 'haversine’ formula (Ref: http://www.movable-type.co.uk/scripts/latlong.html) Uses the globally-average earth radius value of 6371km ''' # get_lat_lon_alt returns radians - so no need to convert here (lat1, lon1, alt1) = get_lat_lon_alt(MSG1) (lat2, lon2, alt2) = get_lat_lon_alt(MSG2) dLat = lat2 - lat1 dLon = lon2 - lon1 a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2) c = 2.0 * atan2(sqrt(a), sqrt(1.0-a)) ground_dist = 6371 * 1000 * c if horizontal: return ground_dist return sqrt(ground_dist**2 + (alt2-alt1)**2) def distance_two(MSG1, MSG2, horizontal=True): '''distance between two points''' try: return _distance_two(MSG1, MSG2) except Exception as ex: print(ex) return None first_fix = None def distance_home(GPS_RAW): '''distance from first fix point''' global first_fix if (hasattr(GPS_RAW, 'fix_type') and GPS_RAW.fix_type < 2) or \ (hasattr(GPS_RAW, 'Status') and GPS_RAW.Status < 2): return 0 if first_fix is None: first_fix = GPS_RAW return 0 return distance_two(GPS_RAW, first_fix) def sawtooth(ATTITUDE, amplitude=2.0, period=5.0): '''sawtooth pattern based on uptime''' mins = (ATTITUDE.usec * 1.0e-6)/60 p = fmod(mins, period*2) if p < period: return amplitude * (p/period) return amplitude * (period - (p-period))/period def rate_of_turn(speed, bank): '''return expected rate of turn in degrees/s for given speed in m/s and bank angle in degrees''' if abs(speed) < 2 or abs(bank) > 80: return 0 ret = degrees(9.81*tan(radians(bank))/speed) return ret def wingloading(bank): '''return expected wing loading factor for a bank angle in radians''' return 1.0/cos(bank) def airspeed(VFR_HUD, ratio=None, used_ratio=None, offset=None): '''recompute airspeed with a different ARSPD_RATIO''' from . import mavutil mav = mavutil.mavfile_global if ratio is None: ratio = 1.9936 # APM default if used_ratio is None: if 'ARSPD_RATIO' in mav.params: used_ratio = mav.params['ARSPD_RATIO'] else: print("no ARSPD_RATIO in mav.params") used_ratio = ratio if hasattr(VFR_HUD,'airspeed'): airspeed = VFR_HUD.airspeed else: airspeed = VFR_HUD.Airspeed airspeed_pressure = (airspeed**2) / used_ratio if offset is not None: airspeed_pressure += offset if airspeed_pressure < 0: airspeed_pressure = 0 airspeed = sqrt(airspeed_pressure * ratio) return airspeed def EAS2TAS(ARSP,GPS,BARO,ground_temp=25): '''EAS2TAS from ARSP.Temp''' tempK = ground_temp + 273.15 - 0.0065 * GPS.Alt return sqrt(1.225 / (BARO.Press / (287.26 * tempK))) SSL_AIR_DENSITY = 1.225 C_TO_KELVIN = 273.15 ISA_LAPSE_RATE = 0.0065 ISA_GAS_CONSTANT = 287.26 SSL_AIR_TEMPERATURE = 288.15 SSL_AIR_PRESSURE = 101325.01576 def SimpleAtmosphere(alt_km): REARTH = 6369.0 GMR = 34.163195 # geometric to geopotential altitude h = alt_km*REARTH/(alt_km+REARTH) if (h < 11.0): # Troposphere theta = (SSL_AIR_TEMPERATURE - 6.5 * h) / SSL_AIR_TEMPERATURE delta = pow(theta, GMR / 6.5) else: # Stratosphere theta = 216.65 / SSL_AIR_TEMPERATURE delta = 0.2233611 * exp(-GMR * (h - 11.0) / 216.65) sigma = delta/theta return (sigma, delta, theta) def eas2tas(alt_m, groundtemp=25.0): '''eas2tas from altitude in meters AMSL''' (sigma, delta, theta) = SimpleAtmosphere(alt_m*0.001) pressure = SSL_AIR_PRESSURE * delta tempK = groundtemp + C_TO_KELVIN - ISA_LAPSE_RATE * alt_m eas2tas_squared = SSL_AIR_DENSITY / (pressure / (ISA_GAS_CONSTANT * tempK)) return sqrt(eas2tas_squared) def airspeed_tas(VFR_HUD,GLOBAL_POSITION_INT): '''airspeed as true airspeed from VFR_HUD and GLOBAL_POSITION_INT''' return eas2tas(GLOBAL_POSITION_INT.alt*0.001) * VFR_HUD.airspeed def airspeed_ratio(VFR_HUD): '''recompute airspeed with a different ARSPD_RATIO''' from . import mavutil mav = mavutil.mavfile_global airspeed_pressure = (VFR_HUD.airspeed**2) / ratio airspeed = sqrt(airspeed_pressure * ratio) return airspeed def airspeed_voltage(VFR_HUD, ratio=None): '''back-calculate the voltage the airspeed sensor must have seen''' from . import mavutil mav = mavutil.mavfile_global if ratio is None: ratio = 1.9936 # APM default if 'ARSPD_RATIO' in mav.params: used_ratio = mav.params['ARSPD_RATIO'] else: used_ratio = ratio if 'ARSPD_OFFSET' in mav.params: offset = mav.params['ARSPD_OFFSET'] else: return -1 airspeed_pressure = (pow(VFR_HUD.airspeed,2)) / used_ratio raw = airspeed_pressure + offset SCALING_OLD_CALIBRATION = 204.8 voltage = 5.0 * raw / 4096 return voltage def earth_rates(ATTITUDE): '''return angular velocities in earth frame''' from math import sin, cos, tan, fabs p = ATTITUDE.rollspeed q = ATTITUDE.pitchspeed r = ATTITUDE.yawspeed phi = ATTITUDE.roll theta = ATTITUDE.pitch psi = ATTITUDE.yaw 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 (phiDot, thetaDot, psiDot) def roll_rate(ATTITUDE): '''return roll rate in earth frame''' (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE) return phiDot def pitch_rate(ATTITUDE): '''return pitch rate in earth frame''' (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE) return thetaDot def yaw_rate(ATTITUDE): '''return yaw rate in earth frame''' (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE) return psiDot def gps_velocity(GLOBAL_POSITION_INT): '''return GPS velocity vector''' return Vector3(GLOBAL_POSITION_INT.vx, GLOBAL_POSITION_INT.vy, GLOBAL_POSITION_INT.vz) * 0.01 def gps_velocity_old(GPS_RAW_INT): '''return GPS velocity vector''' return Vector3(GPS_RAW_INT.vel*0.01*cos(radians(GPS_RAW_INT.cog*0.01)), GPS_RAW_INT.vel*0.01*sin(radians(GPS_RAW_INT.cog*0.01)), 0) def gps_velocity_body(GPS_RAW_INT, ATTITUDE): '''return GPS velocity vector in body frame''' r = rotation(ATTITUDE) return r.transposed() * Vector3(GPS_RAW_INT.vel*0.01*cos(radians(GPS_RAW_INT.cog*0.01)), GPS_RAW_INT.vel*0.01*sin(radians(GPS_RAW_INT.cog*0.01)), -tan(ATTITUDE.pitch)*GPS_RAW_INT.vel*0.01) def earth_accel(IMU,ATT): '''return earth frame acceleration vector''' r = rotation(ATT) if hasattr(IMU, 'xacc'): accel = Vector3(IMU.xacc, IMU.yacc, IMU.zacc) * 9.81 * 0.001 else: accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) return r * accel def earth_gyro(IMU,ATT): '''return earth frame gyro vector''' r = rotation(ATT) if hasattr(IMU, 'xgyro'): gyro = Vector3(IMU.xgyro, IMU.ygyro, IMU.zgyro) * degrees(0.001) else: gyro = Vector3(IMU.GyrX, IMU.GyrY, IMU.GyrZ) return r * gyro def airspeed_energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD): '''return airspeed energy error matching APM internals This is positive when we are going too slow ''' aspeed_cm = VFR_HUD.airspeed*100 target_airspeed = NAV_CONTROLLER_OUTPUT.aspd_error + aspeed_cm airspeed_energy_error = ((target_airspeed*target_airspeed) - (aspeed_cm*aspeed_cm))*0.00005 return airspeed_energy_error def energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD): '''return energy error matching APM internals This is positive when we are too low or going too slow ''' aspeed_energy_error = airspeed_energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD) alt_error = NAV_CONTROLLER_OUTPUT.alt_error*100 energy_error = aspeed_energy_error + alt_error*0.098 return energy_error def rover_turn_circle(SERVO_OUTPUT_RAW): '''return turning circle (diameter) in meters for steering_angle in degrees ''' # this matches Toms slash max_wheel_turn = 35 wheelbase = 0.335 wheeltrack = 0.296 steering_angle = max_wheel_turn * (SERVO_OUTPUT_RAW.servo1_raw - 1500) / 400.0 theta = radians(steering_angle) return (wheeltrack/2) + (wheelbase/sin(theta)) def rover_yaw_rate(VFR_HUD, SERVO_OUTPUT_RAW): '''return yaw rate in degrees/second given steering_angle and speed''' max_wheel_turn=35 speed = VFR_HUD.groundspeed # assume 1100 to 1900 PWM on steering steering_angle = max_wheel_turn * (SERVO_OUTPUT_RAW.servo1_raw - 1500) / 400.0 if abs(steering_angle) < 1.0e-6 or abs(speed) < 1.0e-6: return 0 d = rover_turn_circle(SERVO_OUTPUT_RAW) c = pi * d t = c / speed rate = 360.0 / t return rate def rover_lat_accel(VFR_HUD, SERVO_OUTPUT_RAW): '''return lateral acceleration in m/s/s''' speed = VFR_HUD.groundspeed yaw_rate = rover_yaw_rate(VFR_HUD, SERVO_OUTPUT_RAW) accel = radians(yaw_rate) * speed return accel def demix1(servo1, servo2, gain=0.5): '''de-mix a mixed servo output''' s1 = servo1 - 1500 s2 = servo2 - 1500 out1 = (s1+s2)*gain out2 = (s1-s2)*gain return out1+1500 def demix2(servo1, servo2, gain=0.5): '''de-mix a mixed servo output''' s1 = servo1 - 1500 s2 = servo2 - 1500 out1 = (s1+s2)*gain out2 = (s1-s2)*gain return out2+1500 def mixer(servo1, servo2, mixtype=1, gain=0.5): '''mix two servos''' s1 = servo1 - 1500 s2 = servo2 - 1500 v1 = (s1-s2)*gain v2 = (s1+s2)*gain if mixtype == 2: v2 = -v2 elif mixtype == 3: v1 = -v1 elif mixtype == 4: v1 = -v1 v2 = -v2 if v1 > 600: v1 = 600 elif v1 < -600: v1 = -600 if v2 > 600: v2 = 600 elif v2 < -600: v2 = -600 return (1500+v1,1500+v2) def mix1(servo1, servo2, mixtype=1, gain=0.5): '''de-mix a mixed servo output''' (v1,v2) = mixer(servo1, servo2, mixtype=mixtype, gain=gain) return v1 def mix2(servo1, servo2, mixtype=1, gain=0.5): '''de-mix a mixed servo output''' (v1,v2) = mixer(servo1, servo2, mixtype=mixtype, gain=gain) return v2 def wrap_180(angle): if angle > 180: angle -= 360.0 if angle < -180: angle += 360.0 return angle def wrap_360(angle): if angle > 360: angle -= 360.0 if angle < 0: angle += 360.0 return angle class DCM_State(object): '''DCM state object''' def __init__(self, roll, pitch, yaw): self.dcm = Matrix3() self.dcm2 = Matrix3() self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw)) self.dcm2.from_euler(radians(roll), radians(pitch), radians(yaw)) self.mag = Vector3() self.gyro = Vector3() self.accel = Vector3() self.gps = None self.rate = 50.0 self.kp = 0.2 self.kp_yaw = 0.3 self.omega_P = Vector3() self.omega_P_yaw = Vector3() self.omega_I = Vector3() # (-0.00199045287445, -0.00653007719666, -0.00714212376624) self.omega_I_sum = Vector3() self.omega_I_sum_time = 0 self.omega = Vector3() self.ra_sum = Vector3() self.last_delta_angle = Vector3() self.last_velocity = Vector3() (self.roll, self.pitch, self.yaw) = self.dcm.to_euler() (self.roll2, self.pitch2, self.yaw2) = self.dcm2.to_euler() def update(self, gyro, accel, mag, GPS): if self.gyro != gyro or self.accel != accel: delta_angle = old_div((gyro+self.omega_I), self.rate) self.dcm.rotate(delta_angle) correction = self.last_delta_angle % delta_angle #print (delta_angle - self.last_delta_angle) * 58.0 corrected_delta = delta_angle + 0.0833333 * correction self.dcm2.rotate(corrected_delta) self.last_delta_angle = delta_angle self.dcm.normalize() self.dcm2.normalize() self.gyro = gyro self.accel = accel (self.roll, self.pitch, self.yaw) = self.dcm.to_euler() (self.roll2, self.pitch2, self.yaw2) = self.dcm2.to_euler() dcm_state = None def DCM_update(IMU, ATT, MAG, GPS): '''implement full DCM system''' global dcm_state if dcm_state is None: dcm_state = DCM_State(ATT.Roll, ATT.Pitch, ATT.Yaw) mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ) gyro = Vector3(IMU.GyrX, IMU.GyrY, IMU.GyrZ) accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) accel2 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) dcm_state.update(gyro, accel, mag, GPS) return dcm_state class PX4_State(object): '''PX4 DCM state object''' def __init__(self, roll, pitch, yaw, timestamp): self.dcm = Matrix3() self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw)) self.gyro = Vector3() self.accel = Vector3() self.timestamp = timestamp (self.roll, self.pitch, self.yaw) = self.dcm.to_euler() def update(self, gyro, accel, timestamp): if self.gyro != gyro or self.accel != accel: delta_angle = gyro * (timestamp - self.timestamp) self.timestamp = timestamp self.dcm.rotate(delta_angle) self.dcm.normalize() self.gyro = gyro self.accel = accel (self.roll, self.pitch, self.yaw) = self.dcm.to_euler() px4_state = None def PX4_update(IMU, ATT): '''implement full DCM using PX4 native SD log data''' global px4_state if px4_state is None: px4_state = PX4_State(degrees(ATT.Roll), degrees(ATT.Pitch), degrees(ATT.Yaw), IMU._timestamp) gyro = Vector3(IMU.GyroX, IMU.GyroY, IMU.GyroZ) accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) px4_state.update(gyro, accel, IMU._timestamp) return px4_state _downsample_N = 0 def downsample(N): '''conditional that is true on every Nth sample''' global _downsample_N _downsample_N = (_downsample_N + 1) % N return _downsample_N == 0 def armed(HEARTBEAT): '''return 1 if armed, 0 if not''' from . import mavutil if HEARTBEAT.type == mavutil.mavlink.MAV_TYPE_GCS: self = mavutil.mavfile_global if self.motors_armed(): return 1 return 0 if HEARTBEAT.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED: return 1 return 0 def rotation_df(ATT): '''return the current DCM rotation matrix''' r = Matrix3() r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw)) return r def rotation2(AHRS2): '''return the current DCM rotation matrix''' r = Matrix3() r.from_euler(AHRS2.roll, AHRS2.pitch, AHRS2.yaw) return r def earth_accel2(RAW_IMU,ATTITUDE): '''return earth frame acceleration vector from AHRS2''' r = rotation2(ATTITUDE) accel = Vector3(RAW_IMU.xacc, RAW_IMU.yacc, RAW_IMU.zacc) * 9.81 * 0.001 return r * accel def earth_accel_df(IMU,ATT): '''return earth frame acceleration vector from df log''' r = rotation_df(ATT) accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) return r * accel def earth_accel2_df(IMU,IMU2,ATT): '''return earth frame acceleration vector from df log''' r = rotation_df(ATT) accel1 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) accel2 = Vector3(IMU2.AccX, IMU2.AccY, IMU2.AccZ) accel = 0.5 * (accel1 + accel2) return r * accel def gps_velocity_df(GPS): '''return GPS velocity vector''' vx = GPS.Spd * cos(radians(GPS.GCrs)) vy = GPS.Spd * sin(radians(GPS.GCrs)) return Vector3(vx, vy, GPS.VZ) def distance_gps2(GPS, GPS2): '''distance between two points''' if GPS.TimeMS != GPS2.TimeMS: # reject messages not time aligned return None return distance_two(GPS, GPS2) # SI base unit for 1 Rgeo / equatorial radius radius_of_earth = 6378100.0 # in meters def wrap_valid_longitude(lon): ''' wrap a longitude value around to always have a value in the range [-180, +180) i.e 0 => 0, 1 => 1, -1 => -1, 181 => -179, -181 => 179 ''' return (((lon + 180.0) % 360.0) - 180.0) def gps_newpos(lat, lon, bearing, distance): '''extrapolate latitude/longitude given a heading and distance thanks to http://www.movable-type.co.uk/scripts/latlong.html ''' import math lat1 = math.radians(lat) lon1 = math.radians(lon) brng = math.radians(bearing) dr = distance/radius_of_earth lat2 = math.asin(math.sin(lat1)*math.cos(dr) + math.cos(lat1)*math.sin(dr)*math.cos(brng)) lon2 = lon1 + math.atan2(math.sin(brng)*math.sin(dr)*math.cos(lat1), math.cos(dr)-math.sin(lat1)*math.sin(lat2)) return (math.degrees(lat2), wrap_valid_longitude(math.degrees(lon2))) def gps_offset(lat, lon, east, north): '''return new lat/lon after moving east/north by the given number of meters''' import math bearing = math.degrees(math.atan2(east, north)) distance = math.sqrt(east**2 + north**2) return gps_newpos(lat, lon, bearing, distance) ekf_origin = None def ekf1_pos(EKF1): '''calculate EKF position when EKF disabled''' global ekf_origin from . import mavutil self = mavutil.mavfile_global if getattr(EKF1,'C',0) != 0: return None if ekf_origin is None: if not 'ORGN' in self.messages: return None ekf_origin = self.messages['ORGN'] (ekf_origin.Lat, ekf_origin.Lng) = (ekf_origin.Lat, ekf_origin.Lng) (lat,lon) = gps_offset(ekf_origin.Lat, ekf_origin.Lng, EKF1.PE, EKF1.PN) alt = ekf_origin.Alt - EKF1.PD return (lat, lon, alt) def quat_to_euler(q): ''' Get Euler angles from a quaternion :param q: quaternion [w, x, y , z] :returns: euler angles [roll, pitch, yaw] ''' quat = Quaternion(q) return quat.euler def euler_to_quat(e): ''' Get quaternion from euler angles :param e: euler angles [roll, pitch, yaw] :returns: quaternion [w, x, y , z] ''' quat = Quaternion(e) return quat.q def rotate_quat(attitude, roll, pitch, yaw): ''' Returns rotated quaternion :param attitude: quaternion [w, x, y , z] :param roll: rotation in rad :param pitch: rotation in rad :param yaw: rotation in rad :returns: quaternion [w, x, y , z] ''' quat = Quaternion(attitude) rotation = Quaternion([roll, pitch, yaw]) res = rotation * quat return res.q def qroll(MSG): '''return quaternion roll in degrees''' q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4]) return degrees(q.euler[0]) def qpitch(MSG): '''return quaternion pitch in degrees''' q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4]) return degrees(q.euler[1]) def qyaw(MSG): '''return quaternion yaw in degrees''' q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4]) return degrees(q.euler[2]) def euler_rotated(MSG, roll, pitch, yaw): '''return eulers in radians from quaternion for view at given attitude in euler radians''' rot_view = Matrix3() rot_view.from_euler(roll, pitch, yaw) q = Quaternion([MSG.Q1,MSG.Q2,MSG.Q3,MSG.Q4]) dcm = (rot_view * q.dcm.transposed()).transposed() return dcm.to_euler() def euler_p90(MSG): '''return eulers in radians from quaternion for view at pitch 90''' return euler_rotated(MSG, 0, radians(90), 0); def qroll_p90(MSG): '''return quaternion roll in degrees for view at pitch 90''' return degrees(euler_p90(MSG)[0]) def qpitch_p90(MSG): '''return quaternion roll in degrees for view at pitch 90''' return degrees(euler_p90(MSG)[1]) def qyaw_p90(MSG): '''return quaternion roll in degrees for view at pitch 90''' return degrees(euler_p90(MSG)[2]) def rotation_df(ATT): '''return the current DCM rotation matrix''' r = Matrix3() r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw)) return r def rotation2(AHRS2): '''return the current DCM rotation matrix''' r = Matrix3() r.from_euler(AHRS2.roll, AHRS2.pitch, AHRS2.yaw) return r def earth_accel2(RAW_IMU,ATTITUDE): '''return earth frame acceleration vector from AHRS2''' r = rotation2(ATTITUDE) accel = Vector3(RAW_IMU.xacc, RAW_IMU.yacc, RAW_IMU.zacc) * 9.81 * 0.001 return r * accel def earth_accel_df(IMU,ATT): '''return earth frame acceleration vector from df log''' r = rotation_df(ATT) accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) return r * accel def earth_accel2_df(IMU,IMU2,ATT): '''return earth frame acceleration vector from df log''' r = rotation_df(ATT) accel1 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ) accel2 = Vector3(IMU2.AccX, IMU2.AccY, IMU2.AccZ) accel = 0.5 * (accel1 + accel2) return r * accel def gps_velocity_df(GPS): '''return GPS velocity vector''' vx = GPS.Spd * cos(radians(GPS.GCrs)) vy = GPS.Spd * sin(radians(GPS.GCrs)) return Vector3(vx, vy, GPS.VZ) def armed(HEARTBEAT): '''return 1 if armed, 0 if not''' from pymavlink import mavutil if HEARTBEAT.type == mavutil.mavlink.MAV_TYPE_GCS: self = mavutil.mavfile_global if self.motors_armed(): return 1 return 0 if HEARTBEAT.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED: return 1 return 0 def mode(HEARTBEAT): '''return flight mode number''' from pymavlink import mavutil if HEARTBEAT.type == mavutil.mavlink.MAV_TYPE_GCS: return None return HEARTBEAT.custom_mode ''' magnetic field tables for estimating earths mag field updated 2019-12-20 ''' # set to the sampling in degrees for the table below SAMPLING_RES = 10.0 SAMPLING_MIN_LAT = -90.0 SAMPLING_MAX_LAT = 90.0 SAMPLING_MIN_LON = -180.0 SAMPLING_MAX_LON = 180.0 declination_table = [ [148.83402,138.83401,128.83401,118.83402,108.83402,98.83402,88.83402,78.83402,68.83402,58.83402,48.83402,38.83402,28.83402,18.83402,8.83402,-1.16598,-11.16598,-21.16598,-31.16598,-41.16598,-51.16598,-61.16598,-71.16598,-81.16598,-91.16598,-101.16598,-111.16598,-121.16598,-131.16598,-141.16598,-151.16598,-161.16598,-171.16598,178.83402,168.83402,158.83402,148.83402], [129.09306,116.89412,105.78898,95.63017,86.23504,77.42476,69.04348,60.96591,53.09841,45.37592,37.75568,30.20867,22.70998,15.23027,7.73037,0.16098,-7.53238,-15.40109,-23.48496,-31.80703,-40.37375,-49.18062,-58.22152,-67.49946,-77.03640,-86.88079,-97.11212,-107.84156,-119.20626,-131.35191,-144.39481,-158.35719,-173.08769,171.78274,156.78187,142.43310,129.09306], [85.81367,77.83602,71.40003,65.88433,60.88263,56.08204,51.22755,46.12774,40.67419,34.85457,28.74909,22.50583,16.29363,10.23812,4.36029,-1.45095,-7.40778,-13.74178,-20.61213,-28.04922,-35.95530,-44.15322,-52.45486,-60.71951,-68.88732,-76.99183,-85.16746,-93.67473,-102.97950,-113.96429,-128.46293,-150.36073,175.55488,138.00554,112.28051,96.48319,85.81367], [48.22805,46.81921,45.24300,43.71189,42.27245,40.80022,39.00177,36.49549,32.95972,28.26545,22.54379,16.18846,9.77818,3.88971,-1.16066,-5.50500,-9.68832,-14.40052,-20.13990,-26.99385,-34.63319,-42.50584,-50.08478,-57.00760,-63.07978,-68.20773,-72.30336,-75.14845,-76.14220,-73.56875,-61.46573,-19.66028,28.39616,43.98783,48.35027,49.03265,48.22805], [31.42931,31.60530,31.28145,30.79047,30.38024,30.16777,29.97487,29.25597,27.27948,23.46202,17.66378,10.37176,2.68673,-4.06559,-9.01317,-12.19989,-14.47861,-17.04830,-21.00672,-26.83158,-33.98769,-41.32654,-47.85214,-52.97945,-56.36644,-57.72068,-56.62201,-52.29959,-43.64045,-30.08348,-13.41611,2.35315,14.41455,22.53506,27.54777,30.28028,31.42931], [22.67985,23.21169,23.25494,22.99742,22.63545,22.42369,22.46877,22.43187,21.42571,18.33186,12.46440,4.21692,-4.75068,-12.30560,-17.22721,-19.74039,-20.77499,-21.16946,-22.14607,-25.39336,-31.01102,-37.19196,-42.26227,-45.34969,-45.95366,-43.77299,-38.67388,-30.72261,-20.87969,-11.16390,-2.84711,4.17466,10.15622,15.07307,18.83144,21.33469,22.67985], [17.07334,17.59062,17.77934,17.69577,17.34929,16.88674,16.56217,16.38656,15.63204,12.86517,6.93562,-1.75751,-10.98436,-18.20308,-22.41588,-24.27993,-24.67590,-23.50719,-21.00605,-19.72369,-21.87960,-26.21214,-30.23195,-32.25069,-31.49417,-28.07385,-22.58093,-15.67611,-8.73630,-3.38117,0.43868,3.90117,7.49078,10.92679,13.85484,15.93237,17.07334], [13.37426,13.67217,13.78744,13.78998,13.54222,12.98780,12.38852,11.96065,11.09372,8.25499,2.21162,-6.34723,-14.82596,-20.89032,-23.89434,-24.45752,-23.26867,-20.14461,-15.21492,-10.62227,-9.09952,-11.18829,-14.91638,-17.58833,-17.75073,-15.65321,-12.03643,-7.38772,-2.92537,-0.15156,1.30857,3.04660,5.56513,8.30463,10.76538,12.51642,13.37426], [11.10969,11.11944,11.01236,10.99728,10.84097,10.32286,9.70494,9.19687,8.09157,4.94277,-1.12864,-9.05484,-16.32980,-21.04376,-22.55109,-21.19496,-17.89373,-13.44411,-8.58084,-4.24867,-1.62827,-1.86509,-4.55045,-7.42469,-8.64775,-8.12536,-6.32716,-3.48850,-0.64489,0.72027,0.97976,1.90952,4.01623,6.51266,8.80749,10.44185,11.10969], [9.88349,9.72040,9.41289,9.38439,9.33291,8.89921,8.31537,7.68011,6.17272,2.60695,-3.33977,-10.37899,-16.38823,-19.72660,-19.70115,-16.80477,-12.41185,-7.91607,-4.15177,-1.10381,1.22204,1.82879,0.23517,-2.13784,-3.62813,-3.94630,-3.34494,-1.85961,-0.22644,0.26762,-0.08803,0.44077,2.38763,4.89878,7.32401,9.15291,9.88349], [9.12464,9.18177,8.94457,9.04553,9.19618,8.89744,8.21223,7.14260,4.96349,0.86055,-4.97931,-11.14357,-15.85640,-17.78532,-16.57645,-13.08478,-8.67898,-4.59243,-1.60427,0.55144,2.39028,3.23916,2.31654,0.46434,-0.91732,-1.53003,-1.62589,-1.18564,-0.62403,-0.86624,-1.66562,-1.50145,0.21812,2.79294,5.56329,7.91823,9.12464], [8.06290,8.93172,9.29767,9.81684,10.30693,10.20311,9.30721,7.53634,4.41412,-0.45099,-6.41113,-11.85029,-15.25565,-15.88209,-13.96742,-10.49677,-6.46933,-2.75504,-0.08743,1.66921,3.12874,3.96903,3.45981,2.05876,0.88578,0.20951,-0.29117,-0.68669,-1.18101,-2.27487,-3.63054,-3.95562,-2.62221,-0.08793,3.01927,6.01454,8.06290], [6.31041,8.41617,9.93058,11.21483,12.13996,12.23034,11.14011,8.66524,4.49928,-1.34733,-7.71989,-12.71272,-15.08305,-14.75641,-12.45343,-9.09705,-5.35725,-1.83470,0.82823,2.57001,3.86519,4.69588,4.60243,3.76104,2.87601,2.13967,1.24833,0.02788,-1.62859,-3.80231,-5.90990,-6.79346,-5.87079,-3.45535,-0.15720,3.34006,6.31041], [4.26294,7.59975,10.43666,12.70491,14.16020,14.43398,13.18770,10.10768,4.90586,-2.08386,-9.15208,-14.09196,-15.93051,-15.08268,-12.51517,-9.09089,-5.35759,-1.77919,1.14514,3.24292,4.77489,5.90082,6.46683,6.42854,5.95440,5.04613,3.49019,1.14215,-1.96091,-5.46021,-8.43365,-9.79880,-9.13330,-6.77311,-3.34752,0.50970,4.26294], [2.57464,6.81988,10.72127,13.95370,16.10049,16.72460,15.39548,11.64478,5.15172,-3.36781,-11.49173,-16.72206,-18.40831,-17.31430,-14.50140,-10.79977,-6.74992,-2.76545,0.78143,3.70100,6.08013,8.07892,9.67963,10.67456,10.77058,9.65347,7.07600,3.02043,-2.09809,-7.27683,-11.16806,-12.81861,-12.13043,-9.62141,-5.97603,-1.77704,2.57464], [1.43503,6.27921,10.88019,14.88261,17.80063,19.02158,17.77297,13.12636,4.53718,-6.54055,-16.27050,-21.85552,-23.31365,-21.85455,-18.61101,-14.36797,-9.64057,-4.79964,-0.13239,4.18385,8.10605,11.64519,14.70734,16.97990,17.92977,16.88820,13.23164,6.81146,-1.34665,-8.97876,-14.01257,-15.81005,-14.85926,-12.00033,-7.99733,-3.40265,1.43502], [0.13094,5.45479,10.54962,15.08235,18.55436,20.17754,18.67574,12.20150,-0.36226,-15.49225,-26.46004,-31.19380,-31.33251,-28.66356,-24.33699,-19.03627,-13.18344,-7.06335,-0.88475,5.19490,11.05071,16.55252,21.50321,25.56424,28.16988,28.41286,24.95220,16.43585,3.63861,-8.67079,-16.18909,-18.69045,-17.68753,-14.54612,-10.18137,-5.16795,0.13094], [-4.14830,1.12345,5.96040,9.84415,11.94596,10.80871,4.02869,-10.36405,-27.94912,-39.79617,-44.16477,-43.57950,-40.12657,-34.99189,-28.83083,-22.02403,-14.80922,-7.34799,0.23881,7.85050,15.39197,22.75942,29.82156,36.38924,42.15868,46.59144,48.63821,46.09967,34.72041,12.13936,-8.98865,-18.65098,-20.47841,-18.40133,-14.39834,-9.45818,-4.14830], [-169.79948,-159.79948,-149.79948,-139.79948,-129.79948,-119.79948,-109.79948,-99.79948,-89.79948,-79.79948,-69.79948,-59.79948,-49.79948,-39.79948,-29.79948,-19.79948,-9.79948,0.20052,10.20052,20.20052,30.20052,40.20052,50.20052,60.20052,70.20052,80.20052,90.20052,100.20052,110.20052,120.20052,130.20052,140.20052,150.20052,160.20052,170.20052,-179.79948,-169.79948] ] inclination_table = [ [-72.02070,-72.02071,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02070,-72.02071,-72.02070,-72.02071], [-78.23208,-77.46612,-76.54604,-75.51344,-74.40408,-73.25043,-72.08420,-70.93779,-69.84384,-68.83332,-67.93241,-67.15960,-66.52407,-66.02648,-65.66205,-65.42519,-65.31392,-65.33255,-65.49161,-65.80511,-66.28626,-66.94289,-67.77395,-68.76776,-69.90201,-71.14489,-72.45664,-73.79090,-75.09549,-76.31306,-77.38240,-78.24183,-78.83644,-79.12876,-79.10879,-78.79618,-78.23208], [-80.80007,-78.97830,-77.14756,-75.29021,-73.38193,-71.40769,-69.37743,-67.33857,-65.37896,-63.61481,-62.16302,-61.10460,-60.45316,-60.14557,-60.06470,-60.08698,-60.13217,-60.19219,-60.32958,-60.65151,-61.27183,-62.27481,-63.69257,-65.50302,-67.64474,-70.03800,-72.60073,-75.25455,-77.92173,-80.51496,-82.91263,-84.88736,-85.95658,-85.63251,-84.30522,-82.60297,-80.80007], [-77.45516,-75.43606,-73.49604,-71.57699,-69.59653,-67.45749,-65.08505,-62.48902,-59.81600,-57.35291,-55.46368,-54.46054,-54.44855,-55.23685,-56.40404,-57.49090,-58.18917,-58.42457,-58.34278,-58.24952,-58.51231,-59.42525,-61.10217,-63.46936,-66.34503,-69.53469,-72.88656,-76.29320,-79.66218,-82.88130,-85.73520,-87.38955,-86.36194,-84.15583,-81.83294,-79.58616,-77.45517], [-71.58214,-69.61565,-67.71488,-65.86479,-64.01505,-62.04510,-59.77482,-57.07400,-54.02405,-51.03212,-48.80491,-48.09823,-49.26412,-51.92460,-55.16562,-58.07793,-60.10267,-61.01513,-60.84749,-59.97822,-59.15799,-59.18575,-60.45940,-62.86069,-65.97595,-69.36736,-72.71868,-75.80268,-78.35851,-80.06262,-80.67983,-80.25871,-79.08901,-77.46953,-75.59576,-73.59787,-71.58215], [-64.39807,-62.40221,-60.40960,-58.42549,-56.48151,-54.55795,-52.49863,-50.03954,-47.03044,-43.80319,-41.39118,-41.16169,-43.78688,-48.51954,-53.83971,-58.60413,-62.35170,-64.81766,-65.60605,-64.62579,-62.64650,-61.08659,-61.05364,-62.68316,-65.34905,-68.24163,-70.78236,-72.62117,-73.51797,-73.52965,-73.02048,-72.24528,-71.21594,-69.88980,-68.25886,-66.38361,-64.39807], [-55.02826,-52.87120,-50.69073,-48.44731,-46.19513,-44.04849,-42.00465,-39.74123,-36.79801,-33.33110,-30.77621,-31.26765,-35.68330,-42.58861,-49.78570,-56.05256,-61.22645,-65.24992,-67.52021,-67.43115,-65.26549,-62.35447,-60.45735,-60.49007,-61.96843,-63.81210,-65.26064,-65.90429,-65.53604,-64.51130,-63.52424,-62.77292,-61.94296,-60.76846,-59.16172,-57.17787,-55.02826], [-42.23934,-39.72248,-37.30606,-34.84704,-32.29950,-29.84715,-27.62882,-25.25073,-22.02346,-18.13398,-15.56621,-17.09131,-23.52381,-32.78684,-42.11183,-49.91489,-55.93881,-60.34177,-62.85919,-63.04596,-60.93631,-57.41244,-54.21948,-52.82417,-53.10885,-54.02972,-54.81946,-54.92826,-53.94911,-52.38029,-51.27875,-50.79557,-50.20937,-49.05107,-47.23568,-44.84941,-42.23934], [-25.31616,-22.26353,-19.64432,-17.13765,-14.48794,-11.90452,-9.56551,-6.93532,-3.35246,0.62534,2.69744,0.20162,-7.49683,-18.49333,-29.80260,-38.99555,-45.22087,-48.81124,-50.28975,-49.88122,-47.49942,-43.57896,-39.80201,-37.81589,-37.58213,-38.12733,-38.79073,-38.91636,-37.84560,-36.15384,-35.28930,-35.35164,-35.11620,-33.90633,-31.71407,-28.67497,-25.31616], [-5.22365,-1.68002,0.98130,3.28459,5.73141,8.13291,10.33797,12.92226,16.28019,19.52644,20.64919,17.75076,10.26968,-0.68527,-12.39654,-21.81573,-27.55757,-29.98547,-30.24089,-29.18412,-26.62224,-22.51726,-18.50023,-16.36665,-16.02599,-16.47452,-17.18522,-17.56335,-16.82114,-15.49276,-15.21405,-16.04947,-16.38744,-15.33409,-12.92178,-9.31548,-5.22365], [14.65727,18.24457,20.73560,22.68750,24.74090,26.83478,28.82717,31.06453,33.64927,35.73650,35.91607,33.06931,26.82267,17.82545,8.14938,0.35249,-4.22224,-5.65846,-5.08244,-3.67265,-1.30171,2.36894,6.00997,7.96495,8.31670,8.01335,7.45026,6.96852,7.19717,7.76654,7.31456,5.77666,4.68883,5.07867,7.05547,10.50432,14.65727], [31.00203,34.03206,36.22486,37.93041,39.71929,41.67672,43.64451,45.63057,47.51149,48.64179,48.13476,45.46014,40.64785,34.34474,27.90343,22.77471,19.78522,19.08118,19.96093,21.39191,23.28877,25.96005,28.61507,30.09860,30.42073,30.29938,30.04062,29.74069,29.65970,29.53545,28.53162,26.62276,24.91817,24.37385,25.31100,27.72542,31.00203], [43.33608,45.45851,47.28808,48.92364,50.69187,52.68920,54.75852,56.70372,58.24993,58.87748,58.03709,55.61656,52.01961,47.93440,44.13769,41.24459,39.61639,39.39065,40.22833,41.47069,42.89022,44.59331,46.23881,47.24171,47.57154,47.63935,47.66249,47.62919,47.50096,47.00810,45.71114,43.68014,41.65440,40.38784,40.27252,41.37670,43.33608], [53.10131,54.36161,55.82606,57.45873,59.32146,61.39014,63.50628,65.43734,66.85905,67.31408,66.44780,64.38450,61.68268,58.96721,56.68844,55.08044,54.25566,54.24982,54.89128,55.82780,56.83438,57.87845,58.85920,59.59271,60.06042,60.40887,60.71534,60.90716,60.81057,60.15911,58.74748,56.73559,54.64618,53.02448,52.20550,52.28132,53.10131], [61.90363,62.59082,63.73000,65.25974,67.10373,69.13404,71.16923,72.97878,74.24745,74.58969,73.79370,72.08739,69.99806,68.01780,66.44167,65.38391,64.86230,64.84287,65.21557,65.80372,66.46017,67.12970,67.80720,68.49179,69.18909,69.89166,70.52514,70.92481,70.86509,70.14456,68.73300,66.86155,64.92449,63.29260,62.20331,61.74779,61.90363], [70.57319,70.97549,71.81891,73.05700,74.60513,76.33503,78.07584,79.60158,80.60606,80.76720,79.99878,78.58008,76.92858,75.37566,74.11340,73.22087,72.70518,72.52979,72.62677,72.91213,73.31657,73.81370,74.42101,75.17239,76.07666,77.07532,78.01176,78.63384,78.66938,77.98598,76.70086,75.10445,73.50638,72.14451,71.16426,70.63412,70.57319], [78.82351,79.06440,79.60559,80.41612,81.44382,82.60991,83.79747,84.82323,85.41014,85.29919,84.52961,83.39228,82.15604,80.99185,79.99787,79.22370,78.68527,78.37555,78.27394,78.35635,78.60546,79.01788,79.60326,80.37283,81.31875,82.38760,83.44917,84.26774,84.54142,84.12292,83.18704,82.03917,80.91872,79.97342,79.28584,78.89722,78.82351], [85.92404,85.99807,86.21370,86.55320,86.98729,87.46743,87.90712,88.15645,88.05609,87.61718,86.98684,86.28678,85.58976,84.94098,84.37090,83.90045,83.54357,83.30879,83.20077,83.22179,83.37274,83.65354,84.06234,84.59369,85.23581,85.96691,86.74942,87.51716,88.14109,88.39226,88.15089,87.63861,87.08412,86.59723,86.22810,86.00055,85.92404], [88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420,88.21420] ] intensity_table = [ [0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507,0.54507], [0.60562,0.59923,0.59133,0.58213,0.57184,0.56070,0.54892,0.53679,0.52458,0.51262,0.50121,0.49067,0.48128,0.47330,0.46697,0.46249,0.46006,0.45986,0.46203,0.46665,0.47372,0.48312,0.49460,0.50778,0.52217,0.53717,0.55217,0.56652,0.57964,0.59102,0.60027,0.60715,0.61152,0.61342,0.61294,0.61026,0.60562], [0.62993,0.61663,0.60163,0.58513,0.56721,0.54792,0.52741,0.50597,0.48417,0.46274,0.44252,0.42424,0.40840,0.39524,0.38483,0.37722,0.37262,0.37149,0.37444,0.38211,0.39496,0.41303,0.43591,0.46271,0.49220,0.52288,0.55321,0.58160,0.60660,0.62705,0.64217,0.65166,0.65569,0.65477,0.64966,0.64113,0.62993], [0.61859,0.59954,0.57951,0.55859,0.53652,0.51281,0.48703,0.45921,0.43010,0.40125,0.37460,0.35184,0.33383,0.32034,0.31043,0.30313,0.29816,0.29624,0.29887,0.30789,0.32478,0.35003,0.38290,0.42162,0.46385,0.50716,0.54918,0.58755,0.62009,0.64502,0.66138,0.66918,0.66921,0.66276,0.65127,0.63614,0.61859], [0.58434,0.56134,0.53820,0.51511,0.49174,0.46715,0.44010,0.40984,0.37699,0.34382,0.31364,0.28967,0.27359,0.26453,0.25966,0.25610,0.25262,0.24997,0.25053,0.25779,0.27518,0.30435,0.34417,0.39141,0.44206,0.49253,0.54000,0.58194,0.61593,0.64015,0.65395,0.65794,0.65353,0.64245,0.62626,0.60641,0.58434], [0.53921,0.51448,0.48995,0.46595,0.44250,0.41888,0.39359,0.36521,0.33366,0.30097,0.27115,0.24895,0.23726,0.23477,0.23677,0.23885,0.23920,0.23803,0.23691,0.23986,0.25296,0.28060,0.32245,0.37391,0.42855,0.48085,0.52753,0.56643,0.59562,0.61429,0.62316,0.62341,0.61626,0.60311,0.58508,0.56327,0.53921], [0.48785,0.46379,0.43988,0.41636,0.39367,0.37182,0.34998,0.32674,0.30099,0.27346,0.24780,0.22959,0.22251,0.22507,0.23198,0.23916,0.24558,0.25057,0.25278,0.25389,0.26060,0.28058,0.31692,0.36595,0.41928,0.46891,0.51065,0.54231,0.56238,0.57214,0.57493,0.57236,0.56426,0.55095,0.53309,0.51149,0.48785], [0.43209,0.41089,0.38995,0.36941,0.34979,0.33155,0.31470,0.29826,0.28044,0.26067,0.24164,0.22828,0.22418,0.22862,0.23770,0.24867,0.26122,0.27397,0.28296,0.28650,0.28895,0.29863,0.32271,0.36133,0.40653,0.44908,0.48374,0.50731,0.51774,0.51826,0.51530,0.51075,0.50252,0.48985,0.47321,0.45336,0.43209], [0.37890,0.36283,0.34739,0.33271,0.31922,0.30724,0.29696,0.28780,0.27809,0.26658,0.25432,0.24439,0.23994,0.24247,0.25117,0.26398,0.27930,0.29517,0.30771,0.31403,0.31546,0.31823,0.33068,0.35590,0.38846,0.42051,0.44683,0.46322,0.46680,0.46142,0.45465,0.44832,0.43950,0.42721,0.41228,0.39568,0.37890], [0.34114,0.33192,0.32348,0.31616,0.31061,0.30672,0.30410,0.30225,0.29980,0.29492,0.28700,0.27755,0.26956,0.26653,0.27083,0.28123,0.29446,0.30791,0.31935,0.32678,0.32987,0.33216,0.33985,0.35562,0.37648,0.39779,0.41572,0.42632,0.42691,0.42012,0.41134,0.40209,0.39087,0.37775,0.36429,0.35181,0.34114], [0.32818,0.32516,0.32319,0.32283,0.32517,0.32980,0.33535,0.34062,0.34397,0.34296,0.33615,0.32475,0.31211,0.30265,0.30010,0.30443,0.31265,0.32241,0.33228,0.34067,0.34700,0.35327,0.36224,0.37416,0.38773,0.40158,0.41356,0.42060,0.42076,0.41455,0.40362,0.38928,0.37303,0.35700,0.34334,0.33369,0.32818], [0.33981,0.34000,0.34266,0.34809,0.35721,0.36937,0.38240,0.39400,0.40180,0.40293,0.39568,0.38157,0.36502,0.35110,0.34343,0.34233,0.34605,0.35337,0.36304,0.37282,0.38187,0.39163,0.40273,0.41394,0.42487,0.43601,0.44625,0.45294,0.45399,0.44803,0.43420,0.41393,0.39124,0.37028,0.35389,0.34377,0.33981], [0.37236,0.37289,0.37826,0.38816,0.40237,0.41956,0.43724,0.45274,0.46333,0.46592,0.45864,0.44316,0.42447,0.40808,0.39741,0.39283,0.39339,0.39860,0.40735,0.41725,0.42701,0.43745,0.44902,0.46095,0.47308,0.48570,0.49764,0.50631,0.50895,0.50305,0.48719,0.46316,0.43591,0.41060,0.39068,0.37789,0.37236], [0.42245,0.42215,0.42846,0.44064,0.45724,0.47601,0.49435,0.50993,0.52038,0.52304,0.51631,0.50148,0.48288,0.46550,0.45264,0.44515,0.44269,0.44488,0.45083,0.45875,0.46742,0.47714,0.48867,0.50225,0.51759,0.53373,0.54863,0.55948,0.56339,0.55795,0.54232,0.51859,0.49131,0.46537,0.44426,0.42977,0.42245], [0.48319,0.48226,0.48758,0.49842,0.51308,0.52913,0.54416,0.55622,0.56358,0.56459,0.55836,0.54575,0.52954,0.51323,0.49956,0.48985,0.48445,0.48324,0.48569,0.49085,0.49796,0.50710,0.51894,0.53390,0.55144,0.56981,0.58634,0.59810,0.60259,0.59827,0.58527,0.56571,0.54318,0.52144,0.50331,0.49032,0.48319], [0.53929,0.53799,0.54070,0.54691,0.55552,0.56497,0.57363,0.58010,0.58328,0.58236,0.57702,0.56775,0.55586,0.54317,0.53137,0.52173,0.51505,0.51167,0.51159,0.51458,0.52039,0.52900,0.54059,0.55506,0.57159,0.58845,0.60334,0.61392,0.61846,0.61626,0.60790,0.59512,0.58032,0.56589,0.55363,0.54459,0.53929], [0.57278,0.57090,0.57068,0.57187,0.57403,0.57656,0.57878,0.58007,0.57991,0.57795,0.57409,0.56849,0.56162,0.55414,0.54681,0.54040,0.53554,0.53271,0.53223,0.53423,0.53873,0.54564,0.55475,0.56559,0.57735,0.58891,0.59900,0.60643,0.61040,0.61066,0.60758,0.60203,0.59514,0.58801,0.58156,0.57637,0.57278], [0.57900,0.57728,0.57584,0.57463,0.57360,0.57263,0.57160,0.57039,0.56890,0.56707,0.56489,0.56241,0.55976,0.55710,0.55465,0.55263,0.55127,0.55074,0.55121,0.55273,0.55530,0.55883,0.56314,0.56798,0.57300,0.57785,0.58216,0.58562,0.58802,0.58928,0.58943,0.58865,0.58716,0.58523,0.58309,0.58097,0.57900], [0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830,0.56830] ] def interpolate_table(table, latitude_deg, longitude_deg): '''interpolate inside a table for a given lat/lon in degrees''' # round down to nearest sampling resolution min_lat = int(floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES) min_lon = int(floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES) # find index of nearest low sampling point min_lat_index = int(floor(-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES) min_lon_index = int(floor(-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES) # calculate intensity data_sw = table[min_lat_index][min_lon_index] data_se = table[min_lat_index][min_lon_index + 1] data_ne = table[min_lat_index + 1][min_lon_index + 1] data_nw = table[min_lat_index + 1][min_lon_index] # perform bilinear interpolation on the four grid corners data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min return value earth_declination = None ''' calculate magnetic field intensity and orientation, interpolating in tables returns array [declination_deg, inclination_deg, intensity] or None ''' def get_mag_field_ef(latitude_deg, longitude_deg): # limit to table bounds if latitude_deg < SAMPLING_MIN_LAT: return None if latitude_deg >= SAMPLING_MAX_LAT: return None if longitude_deg < SAMPLING_MIN_LON: return None if longitude_deg >= SAMPLING_MAX_LON: return None intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg) declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg) inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg) global earth_declination earth_declination = declination_deg return [declination_deg, inclination_deg, intensity_gauss] def expected_earth_field_lat_lon(lat, lon): '''return expected magnetic field for a location''' field_var = get_mag_field_ef(lat, lon) mag_ef = Vector3(field_var[2]*1000.0, 0.0, 0.0) R = Matrix3() R.from_euler(0.0, -radians(field_var[1]), radians(field_var[0])) mag_ef = R * mag_ef earth_field = mag_ef return earth_field def get_lat_lon_status(GPS): '''get lat, lon and status from a GPS message''' if hasattr(GPS,'fix_type'): gps_status = GPS.fix_type lat = GPS.lat*1.0e-7 lon = GPS.lon*1.0e-7 else: gps_status = GPS.Status lat = GPS.Lat lon = GPS.Lng return (lat, lon, gps_status) def expected_earth_field(GPS): '''return expected magnetic field for a location''' (lat, lon, gps_status) = get_lat_lon_status(GPS) if gps_status < 3: return Vector3(0,0,0) return expected_earth_field_lat_lon(lat, lon) def expected_mag(GPS,ATT,roll_adjust=0,pitch_adjust=0,yaw_adjust=0): '''return expected magnetic field for a location and attitude''' earth_field = expected_earth_field(GPS) if earth_field is None: return Vector3(0,0,0) if hasattr(ATT,'roll'): roll = degrees(ATT.roll)+roll_adjust pitch = degrees(ATT.pitch)+pitch_adjust yaw = degrees(ATT.yaw)+yaw_adjust else: roll = ATT.Roll+roll_adjust pitch = ATT.Pitch+pitch_adjust yaw = ATT.Yaw+yaw_adjust rot = Matrix3() rot.from_euler(radians(roll), radians(pitch), radians(yaw)) field = rot.transposed() * earth_field return field def expected_mag_latlon(lat,lon,ATT,roll_adjust=0,pitch_adjust=0,yaw_adjust=0): '''return expected magnetic field for a location and attitude''' earth_field = expected_earth_field_lat_lon(lat,lon) if earth_field is None: return Vector3(0,0,0) if hasattr(ATT,'roll'): roll = degrees(ATT.roll)+roll_adjust pitch = degrees(ATT.pitch)+pitch_adjust yaw = degrees(ATT.yaw)+yaw_adjust else: roll = ATT.Roll+roll_adjust pitch = ATT.Pitch+pitch_adjust yaw = ATT.Yaw+yaw_adjust rot = Matrix3() rot.from_euler(radians(roll), radians(pitch), radians(yaw)) field = rot.transposed() * earth_field return field def mag_yaw(GPS,ATT,MAG): '''calculate heading from raw magnetometer''' ef = expected_earth_field(GPS) mag_x = MAG.MagX mag_y = MAG.MagY mag_z = MAG.MagZ # go via a DCM matrix to match the APM calculation dcm_matrix = rotation_df(ATT) cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x) headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z) # we need the declination too (lat, lon, gps_status) = get_lat_lon_status(GPS) field = get_mag_field_ef(lat, lon) declination = field[0] heading = degrees(atan2(-headY,headX)) + declination if heading < 0: heading += 360 return heading def expected_mag_yaw(GPS,ATT,MAG,roll_adjust=0,pitch_adjust=0,yaw_adjust=0): '''return expected magnetic field for a location and attitude''' earth_field = expected_earth_field(GPS) roll = ATT.Roll+roll_adjust pitch = ATT.Pitch+pitch_adjust yaw = mag_yaw(GPS,ATT,MAG) rot = Matrix3() rot.from_euler(radians(roll), radians(pitch), radians(yaw)) field = rot.transposed() * earth_field return field def earth_field_error(GPS,NKF2): '''return vector error in earth field estimate''' earth_field = expected_earth_field(GPS) if earth_field is None: return Vector3(0,0,0) ef = Vector3(NKF2.MN,NKF2.ME,NKF2.MD) ret = ef - earth_field return ret def distance_home_df(GPS,ORGN): '''distance from home origin''' return distance_two(GPS_RAW, first_fix) def airspeed_estimate(GLOBAL_POSITION_INT,WIND): '''estimate airspeed''' wind = WIND gpi = GLOBAL_POSITION_INT from pymavlink.rotmat import Vector3 import math wind3d = Vector3(wind.speed*math.cos(math.radians(wind.direction)), wind.speed*math.sin(math.radians(wind.direction)), 0) ground = Vector3(gpi.vx*0.01, gpi.vy*0.01, 0) airspeed = (ground + wind3d).length() return airspeed def distance_from(GPS_RAW1, lat, lon): ''' Return the distance from a given location in meters Calculated as the great-circle distance using 'haversine’ formula (Ref: http://www.movable-type.co.uk/scripts/latlong.html) Uses the globally-average earth radius value of 6371km ''' if hasattr(GPS_RAW1, 'Lat'): lat1 = radians(GPS_RAW1.Lat) lon1 = radians(GPS_RAW1.Lng) elif hasattr(GPS_RAW1, 'cog'): lat1 = radians(GPS_RAW1.lat)*1.0e-7 lon1 = radians(GPS_RAW1.lon)*1.0e-7 else: lat1 = radians(GPS_RAW1.lat) lon1 = radians(GPS_RAW1.lon) lat2 = radians(lat) lon2 = radians(lon) dLat = lat2 - lat1 dLon = lon2 - lon1 a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2) c = 2.0 * atan2(sqrt(a), sqrt(1.0-a)) ground_dist = 6371 * 1000 * c return ground_dist def distance_lat_lon(lat1, lon1, lat2, lon2): ''' Return the distance between two points in metres Calculated as the great-circle distance using 'haversine’ formula (Ref: http://www.movable-type.co.uk/scripts/latlong.html) Uses the globally-average earth radius value of 6371km ''' lat1 = radians(lat1) lon1 = radians(lon1) lat2 = radians(lat2) lon2 = radians(lon2) dLat = lat2 - lat1 dLon = lon2 - lon1 a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2) c = 2.0 * atan2(sqrt(a), sqrt(1.0-a)) ground_dist = 6371 * 1000 * c return ground_dist def constrain(v, minv, maxv): if v < minv: v = minv if v > maxv: v = maxv return v def sim_body_rates(SIM): '''return body frame rates from simulator attitudes''' rollRate = delta(SIM.Roll,'sbr',SIM.TimeUS) pitchRate = delta(SIM.Pitch,'sbp',SIM.TimeUS) yawRate = delta(SIM.Yaw,'sby',SIM.TimeUS) phi = radians(SIM.Roll) theta = radians(SIM.Pitch) phiDot = radians(rollRate) thetaDot = radians(pitchRate) psiDot = radians(yawRate) 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 reset_state_data(): '''reset state data, used on log rewind''' global average_data global derivative_data global lowpass_data global last_diff global last_delta global first_fix global dcm_state global last_sum global last_integral average_data.clear() derivative_data.clear() lowpass_data.clear() last_delta.clear() last_sum.clear() last_integral.clear() first_fix = None dcm_state = None # terrain functions, using MAVProxy elevation module EleModel = None def terrain_height(lat,lon): '''get terrain height''' global EleModel if EleModel is None: from MAVProxy.modules.mavproxy_map.mp_elevation import ElevationModel EleModel = ElevationModel("srtm",offline=1) return EleModel.GetElevation(lat,lon) def terrain_margin_lat_lon(lat1,lon1,alt1,lat2,lon2,alt2): ''' return minimum height above terrain on path between two positions (AMSL) ''' distance = distance_lat_lon(lat1, lon1, lat2, lon2) steps = distance / 10 dlat = (lat2-lat1) / steps dlon = (lon2-lon1) / steps dalt = (alt2-alt1) / steps min_margin = None for i in range(max(1,int(steps))): talt = terrain_height(lat1,lon1) margin = alt1 - talt if min_margin is None or margin < min_margin: min_margin = margin lat1 += dlat lon1 += dlon alt1 += dalt return min_margin def terrain_margin(TERR,lat,lon,antenna_height): ''' return minimum height above terrain on path between two positions (AMSL) ''' alt = terrain_height(lat,lon)+antenna_height return terrain_margin_lat_lon(TERR.Lat,TERR.Lng,TERR.CHeight+TERR.TerrH,lat,lon,alt) def radio_margin(TERR,lat,lon,antenna_height): ''' return how much height we could lose and still have line of sight from an antenna at antenna_height above ground at lat/lon ''' ant_alt = terrain_height(lat,lon)+antenna_height if hasattr(TERR,'CHeight'): alt = TERR.CHeight+TERR.TerrH else: # allow for GPS messages alt = TERR.Alt low = -alt high = 0 while high > low+1: test = 0.5*(low+high) m = terrain_margin_lat_lon(TERR.Lat,TERR.Lng,alt+test,lat,lon,ant_alt) if m > 0: high = test elif m < 0: low = test else: low = test high = test return -high def mm_curr(RCOU,BAT,PWM_MIN,PWM_MAX,Mfirst,Mlast): ''' motor model to predict current draw given PWM to VTOL motors returned value should be proportional to expected total current draw ''' total_curr = 0.0 voltage = BAT.Volt for m in range(Mfirst,Mlast+1): pwm = getattr(RCOU,'C%u'%m,None) if pwm is None: return 0.0 command = voltage*max(pwm - PWM_MIN,0)/(PWM_MAX-PWM_MIN) total_curr += command**2 return total_curr def RotateMag(MAG,rotation): '''rotate a MAG message by rotation enumeration''' v = Vector3(MAG.MagX,MAG.MagY,MAG.MagZ) return v.rotate_by_id(rotation)