mavextra.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 useful extra functions for use by mavlink clients
00004 
00005 Copyright Andrew Tridgell 2011
00006 Released under GNU GPL version 3 or later
00007 '''
00008 
00009 import os, sys
00010 from math import *
00011 
00012 try:
00013     # in case numpy isn't installed
00014     from .quaternion import Quaternion
00015 except:
00016     pass
00017 
00018 try:
00019     # rotmat doesn't work on Python3.2 yet
00020     from .rotmat import Vector3, Matrix3
00021 except Exception:
00022     pass
00023 
00024 
00025 def kmh(mps):
00026     '''convert m/s to Km/h'''
00027     return mps*3.6
00028 
00029 def altitude(SCALED_PRESSURE, ground_pressure=None, ground_temp=None):
00030     '''calculate barometric altitude'''
00031     from . import mavutil
00032     self = mavutil.mavfile_global
00033     if ground_pressure is None:
00034         if self.param('GND_ABS_PRESS', None) is None:
00035             return 0
00036         ground_pressure = self.param('GND_ABS_PRESS', 1)
00037     if ground_temp is None:
00038         ground_temp = self.param('GND_TEMP', 0)
00039     scaling = ground_pressure / (SCALED_PRESSURE.press_abs*100.0)
00040     temp = ground_temp + 273.15
00041     return log(scaling) * temp * 29271.267 * 0.001
00042 
00043 def altitude2(SCALED_PRESSURE, ground_pressure=None, ground_temp=None):
00044     '''calculate barometric altitude'''
00045     from . import mavutil
00046     self = mavutil.mavfile_global
00047     if ground_pressure is None:
00048         if self.param('GND_ABS_PRESS', None) is None:
00049             return 0
00050         ground_pressure = self.param('GND_ABS_PRESS', 1)
00051     if ground_temp is None:
00052         ground_temp = self.param('GND_TEMP', 0)
00053     scaling = SCALED_PRESSURE.press_abs*100.0 / ground_pressure
00054     temp = ground_temp + 273.15
00055     return 153.8462 * temp * (1.0 - exp(0.190259 * log(scaling)))
00056 
00057 def mag_heading(RAW_IMU, ATTITUDE, declination=None, SENSOR_OFFSETS=None, ofs=None):
00058     '''calculate heading from raw magnetometer'''
00059     if declination is None:
00060         import mavutil
00061         declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
00062     mag_x = RAW_IMU.xmag
00063     mag_y = RAW_IMU.ymag
00064     mag_z = RAW_IMU.zmag
00065     if SENSOR_OFFSETS is not None and ofs is not None:
00066         mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
00067         mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
00068         mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
00069 
00070     # go via a DCM matrix to match the APM calculation
00071     dcm_matrix = rotation(ATTITUDE)
00072     cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x)
00073     headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y
00074     headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z)
00075 
00076     heading = degrees(atan2(-headY,headX)) + declination
00077     if heading < 0:
00078         heading += 360
00079     return heading
00080 
00081 def mag_heading_motors(RAW_IMU, ATTITUDE, declination, SENSOR_OFFSETS, ofs, SERVO_OUTPUT_RAW, motor_ofs):
00082     '''calculate heading from raw magnetometer'''
00083     ofs = get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs)
00084 
00085     if declination is None:
00086         import mavutil
00087         declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
00088     mag_x = RAW_IMU.xmag
00089     mag_y = RAW_IMU.ymag
00090     mag_z = RAW_IMU.zmag
00091     if SENSOR_OFFSETS is not None and ofs is not None:
00092         mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
00093         mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
00094         mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
00095 
00096     headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch)
00097     headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll)
00098     heading = degrees(atan2(-headY,headX)) + declination
00099     if heading < 0:
00100         heading += 360
00101     return heading
00102 
00103 def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None):
00104     '''calculate magnetic field strength from raw magnetometer'''
00105     mag_x = RAW_IMU.xmag
00106     mag_y = RAW_IMU.ymag
00107     mag_z = RAW_IMU.zmag
00108     if SENSOR_OFFSETS is not None and ofs is not None:
00109         mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
00110         mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
00111         mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
00112     return sqrt(mag_x**2 + mag_y**2 + mag_z**2)
00113 
00114 def mag_field_df(MAG, ofs=None):
00115     '''calculate magnetic field strength from raw magnetometer (dataflash version)'''
00116     mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
00117     offsets = Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)
00118     if ofs is not None:
00119         mag = (mag - offsets) + Vector3(ofs[0], ofs[1], ofs[2])
00120     return mag.length()
00121 
00122 def get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs):
00123     '''calculate magnetic field strength from raw magnetometer'''
00124     import mavutil
00125     self = mavutil.mavfile_global
00126 
00127     m = SERVO_OUTPUT_RAW
00128     motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw
00129     motor_pwm *= 0.25
00130     rc3_min = self.param('RC3_MIN', 1100)
00131     rc3_max = self.param('RC3_MAX', 1900)
00132     motor = (motor_pwm - rc3_min) / (rc3_max - rc3_min)
00133     if motor > 1.0:
00134         motor = 1.0
00135     if motor < 0.0:
00136         motor = 0.0
00137 
00138     motor_offsets0 = motor_ofs[0] * motor
00139     motor_offsets1 = motor_ofs[1] * motor
00140     motor_offsets2 = motor_ofs[2] * motor
00141     ofs = (ofs[0] + motor_offsets0, ofs[1] + motor_offsets1, ofs[2] + motor_offsets2)
00142 
00143     return ofs
00144 
00145 def mag_field_motors(RAW_IMU, SENSOR_OFFSETS, ofs, SERVO_OUTPUT_RAW, motor_ofs):
00146     '''calculate magnetic field strength from raw magnetometer'''
00147     mag_x = RAW_IMU.xmag
00148     mag_y = RAW_IMU.ymag
00149     mag_z = RAW_IMU.zmag
00150 
00151     ofs = get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs)
00152 
00153     if SENSOR_OFFSETS is not None and ofs is not None:
00154         mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
00155         mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
00156         mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
00157     return sqrt(mag_x**2 + mag_y**2 + mag_z**2)
00158 
00159 def angle_diff(angle1, angle2):
00160     '''show the difference between two angles in degrees'''
00161     ret = angle1 - angle2
00162     if ret > 180:
00163         ret -= 360;
00164     if ret < -180:
00165         ret += 360
00166     return ret
00167 
00168 average_data = {}
00169 
00170 def average(var, key, N):
00171     '''average over N points'''
00172     global average_data
00173     if not key in average_data:
00174         average_data[key] = [var]*N
00175         return var
00176     average_data[key].pop(0)
00177     average_data[key].append(var)
00178     return sum(average_data[key])/N
00179 
00180 derivative_data = {}
00181 
00182 def second_derivative_5(var, key):
00183     '''5 point 2nd derivative'''
00184     global derivative_data
00185     import mavutil
00186     tnow = mavutil.mavfile_global.timestamp
00187 
00188     if not key in derivative_data:
00189         derivative_data[key] = (tnow, [var]*5)
00190         return 0
00191     (last_time, data) = derivative_data[key]
00192     data.pop(0)
00193     data.append(var)
00194     derivative_data[key] = (tnow, data)
00195     h = (tnow - last_time)
00196     # N=5 2nd derivative from
00197     # http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
00198     ret = ((data[4] + data[0]) - 2*data[2]) / (4*h**2)
00199     return ret
00200 
00201 def second_derivative_9(var, key):
00202     '''9 point 2nd derivative'''
00203     global derivative_data
00204     import mavutil
00205     tnow = mavutil.mavfile_global.timestamp
00206 
00207     if not key in derivative_data:
00208         derivative_data[key] = (tnow, [var]*9)
00209         return 0
00210     (last_time, data) = derivative_data[key]
00211     data.pop(0)
00212     data.append(var)
00213     derivative_data[key] = (tnow, data)
00214     h = (tnow - last_time)
00215     # N=5 2nd derivative from
00216     # http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
00217     f = data
00218     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)
00219     return ret
00220 
00221 lowpass_data = {}
00222 
00223 def lowpass(var, key, factor):
00224     '''a simple lowpass filter'''
00225     global lowpass_data
00226     if not key in lowpass_data:
00227         lowpass_data[key] = var
00228     else:
00229         lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var
00230     return lowpass_data[key]
00231 
00232 last_diff = {}
00233 
00234 def diff(var, key):
00235     '''calculate differences between values'''
00236     global last_diff
00237     ret = 0
00238     if not key in last_diff:
00239         last_diff[key] = var
00240         return 0
00241     ret = var - last_diff[key]
00242     last_diff[key] = var
00243     return ret
00244 
00245 last_delta = {}
00246 
00247 def delta(var, key, tusec=None):
00248     '''calculate slope'''
00249     global last_delta
00250     if tusec is not None:
00251         tnow = tusec * 1.0e-6
00252     else:
00253         import mavutil
00254         tnow = mavutil.mavfile_global.timestamp
00255     dv = 0
00256     ret = 0
00257     if key in last_delta:
00258         (last_v, last_t, last_ret) = last_delta[key]
00259         if last_t == tnow:
00260             return last_ret
00261         if tnow == last_t:
00262             ret = 0
00263         else:
00264             ret = (var - last_v) / (tnow - last_t)
00265     last_delta[key] = (var, tnow, ret)
00266     return ret
00267 
00268 def delta_angle(var, key, tusec=None):
00269     '''calculate slope of an angle'''
00270     global last_delta
00271     if tusec is not None:
00272         tnow = tusec * 1.0e-6
00273     else:
00274         import mavutil
00275         tnow = mavutil.mavfile_global.timestamp
00276     dv = 0
00277     ret = 0
00278     if key in last_delta:
00279         (last_v, last_t, last_ret) = last_delta[key]
00280         if last_t == tnow:
00281             return last_ret
00282         if tnow == last_t:
00283             ret = 0
00284         else:
00285             dv = var - last_v
00286             if dv > 180:
00287                 dv -= 360
00288             if dv < -180:
00289                 dv += 360
00290             ret = dv / (tnow - last_t)
00291     last_delta[key] = (var, tnow, ret)
00292     return ret
00293 
00294 def roll_estimate(RAW_IMU,GPS_RAW_INT=None,ATTITUDE=None,SENSOR_OFFSETS=None, ofs=None, mul=None,smooth=0.7):
00295     '''estimate roll from accelerometer'''
00296     rx = RAW_IMU.xacc * 9.81 / 1000.0
00297     ry = RAW_IMU.yacc * 9.81 / 1000.0
00298     rz = RAW_IMU.zacc * 9.81 / 1000.0
00299     if ATTITUDE is not None and GPS_RAW_INT is not None:
00300         ry -= ATTITUDE.yawspeed * GPS_RAW_INT.vel*0.01
00301         rz += ATTITUDE.pitchspeed * GPS_RAW_INT.vel*0.01
00302     if SENSOR_OFFSETS is not None and ofs is not None:
00303         rx += SENSOR_OFFSETS.accel_cal_x
00304         ry += SENSOR_OFFSETS.accel_cal_y
00305         rz += SENSOR_OFFSETS.accel_cal_z
00306         rx -= ofs[0]
00307         ry -= ofs[1]
00308         rz -= ofs[2]
00309         if mul is not None:
00310             rx *= mul[0]
00311             ry *= mul[1]
00312             rz *= mul[2]
00313     return lowpass(degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))),'_roll',smooth)
00314 
00315 def pitch_estimate(RAW_IMU, GPS_RAW_INT=None,ATTITUDE=None, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7):
00316     '''estimate pitch from accelerometer'''
00317     rx = RAW_IMU.xacc * 9.81 / 1000.0
00318     ry = RAW_IMU.yacc * 9.81 / 1000.0
00319     rz = RAW_IMU.zacc * 9.81 / 1000.0
00320     if ATTITUDE is not None and GPS_RAW_INT is not None:
00321         ry -= ATTITUDE.yawspeed * GPS_RAW_INT.vel*0.01
00322         rz += ATTITUDE.pitchspeed * GPS_RAW_INT.vel*0.01
00323     if SENSOR_OFFSETS is not None and ofs is not None:
00324         rx += SENSOR_OFFSETS.accel_cal_x
00325         ry += SENSOR_OFFSETS.accel_cal_y
00326         rz += SENSOR_OFFSETS.accel_cal_z
00327         rx -= ofs[0]
00328         ry -= ofs[1]
00329         rz -= ofs[2]
00330         if mul is not None:
00331             rx *= mul[0]
00332             ry *= mul[1]
00333             rz *= mul[2]
00334     return lowpass(degrees(asin(rx/sqrt(rx**2+ry**2+rz**2))),'_pitch',smooth)
00335 
00336 def rotation(ATTITUDE):
00337     '''return the current DCM rotation matrix'''
00338     r = Matrix3()
00339     r.from_euler(ATTITUDE.roll, ATTITUDE.pitch, ATTITUDE.yaw)
00340     return r
00341 
00342 def mag_rotation(RAW_IMU, inclination, declination):
00343     '''return an attitude rotation matrix that is consistent with the current mag
00344        vector'''
00345     m_body = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag)
00346     m_earth = Vector3(m_body.length(), 0, 0)
00347 
00348     r = Matrix3()
00349     r.from_euler(0, -radians(inclination), radians(declination))
00350     m_earth = r * m_earth
00351 
00352     r.from_two_vectors(m_earth, m_body)
00353     return r
00354 
00355 def mag_yaw(RAW_IMU, inclination, declination):
00356     '''estimate yaw from mag'''
00357     m = mag_rotation(RAW_IMU, inclination, declination)
00358     (r, p, y) = m.to_euler()
00359     y = degrees(y)
00360     if y < 0:
00361         y += 360
00362     return y
00363 
00364 def mag_pitch(RAW_IMU, inclination, declination):
00365     '''estimate pithc from mag'''
00366     m = mag_rotation(RAW_IMU, inclination, declination)
00367     (r, p, y) = m.to_euler()
00368     return degrees(p)
00369 
00370 def mag_roll(RAW_IMU, inclination, declination):
00371     '''estimate roll from mag'''
00372     m = mag_rotation(RAW_IMU, inclination, declination)
00373     (r, p, y) = m.to_euler()
00374     return degrees(r)
00375 
00376 def expected_mag(RAW_IMU, ATTITUDE, inclination, declination):
00377     '''return expected mag vector'''
00378     m_body = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag)
00379     field_strength = m_body.length()
00380 
00381     m = rotation(ATTITUDE)
00382 
00383     r = Matrix3()
00384     r.from_euler(0, -radians(inclination), radians(declination))
00385     m_earth = r * Vector3(field_strength, 0, 0)
00386 
00387     return m.transposed() * m_earth
00388 
00389 def mag_discrepancy(RAW_IMU, ATTITUDE, inclination, declination=None):
00390     '''give the magnitude of the discrepancy between observed and expected magnetic field'''
00391     if declination is None:
00392         import mavutil
00393         declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
00394     expected = expected_mag(RAW_IMU, ATTITUDE, inclination, declination)
00395     mag = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag)
00396     return degrees(expected.angle(mag))
00397 
00398 
00399 def mag_inclination(RAW_IMU, ATTITUDE, declination=None):
00400     '''give the magnitude of the discrepancy between observed and expected magnetic field'''
00401     if declination is None:
00402         import mavutil
00403         declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
00404     r = rotation(ATTITUDE)
00405     mag1 = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag)
00406     mag1 = r * mag1
00407     mag2 = Vector3(cos(radians(declination)), sin(radians(declination)), 0)
00408     inclination = degrees(mag1.angle(mag2))
00409     if RAW_IMU.zmag < 0:
00410         inclination = -inclination
00411     return inclination
00412 
00413 def expected_magx(RAW_IMU, ATTITUDE, inclination, declination):
00414     '''estimate  from mag'''
00415     v = expected_mag(RAW_IMU, ATTITUDE, inclination, declination)
00416     return v.x
00417 
00418 def expected_magy(RAW_IMU, ATTITUDE, inclination, declination):
00419     '''estimate  from mag'''
00420     v = expected_mag(RAW_IMU, ATTITUDE, inclination, declination)
00421     return v.y
00422 
00423 def expected_magz(RAW_IMU, ATTITUDE, inclination, declination):
00424     '''estimate  from mag'''
00425     v = expected_mag(RAW_IMU, ATTITUDE, inclination, declination)
00426     return v.z
00427 
00428 def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7):
00429     '''estimate pitch from accelerometer'''
00430     if hasattr(RAW_IMU, 'xacc'):
00431         rx = RAW_IMU.xacc * 9.81 / 1000.0
00432         ry = RAW_IMU.yacc * 9.81 / 1000.0
00433         rz = RAW_IMU.zacc * 9.81 / 1000.0
00434     else:
00435         rx = RAW_IMU.AccX
00436         ry = RAW_IMU.AccY
00437         rz = RAW_IMU.AccZ
00438     if SENSOR_OFFSETS is not None and ofs is not None:
00439         rx += SENSOR_OFFSETS.accel_cal_x
00440         ry += SENSOR_OFFSETS.accel_cal_y
00441         rz += SENSOR_OFFSETS.accel_cal_z
00442         rx -= ofs[0]
00443         ry -= ofs[1]
00444         rz -= ofs[2]
00445         if mul is not None:
00446             rx *= mul[0]
00447             ry *= mul[1]
00448             rz *= mul[2]
00449     return sqrt(rx**2+ry**2+rz**2)
00450 
00451 
00452 
00453 def pitch_sim(SIMSTATE, GPS_RAW):
00454     '''estimate pitch from SIMSTATE accels'''
00455     xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9)
00456     zacc = SIMSTATE.zacc
00457     zacc += SIMSTATE.ygyro * GPS_RAW.v;
00458     if xacc/zacc >= 1:
00459         return 0
00460     if xacc/zacc <= -1:
00461         return -0
00462     return degrees(-asin(xacc/zacc))
00463 
00464 def distance_two(GPS_RAW1, GPS_RAW2, horizontal=True):
00465     '''distance between two points'''
00466     if hasattr(GPS_RAW1, 'Lat'):
00467         lat1 = radians(GPS_RAW1.Lat)
00468         lat2 = radians(GPS_RAW2.Lat)
00469         lon1 = radians(GPS_RAW1.Lng)
00470         lon2 = radians(GPS_RAW2.Lng)
00471         alt1 = GPS_RAW1.Alt
00472         alt2 = GPS_RAW2.Alt
00473     elif hasattr(GPS_RAW1, 'cog'):
00474         lat1 = radians(GPS_RAW1.lat)*1.0e-7
00475         lat2 = radians(GPS_RAW2.lat)*1.0e-7
00476         lon1 = radians(GPS_RAW1.lon)*1.0e-7
00477         lon2 = radians(GPS_RAW2.lon)*1.0e-7
00478         alt1 = GPS_RAW1.alt*0.001
00479         alt2 = GPS_RAW2.alt*0.001
00480     else:
00481         lat1 = radians(GPS_RAW1.lat)
00482         lat2 = radians(GPS_RAW2.lat)
00483         lon1 = radians(GPS_RAW1.lon)
00484         lon2 = radians(GPS_RAW2.lon)
00485         alt1 = GPS_RAW1.alt*0.001
00486         alt2 = GPS_RAW2.alt*0.001
00487     dLat = lat2 - lat1
00488     dLon = lon2 - lon1
00489 
00490     a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2)
00491     c = 2.0 * atan2(sqrt(a), sqrt(1.0-a))
00492     ground_dist = 6371 * 1000 * c
00493     if horizontal:
00494         return ground_dist
00495     return sqrt(ground_dist**2 + (alt2-alt1)**2)
00496 
00497 
00498 first_fix = None
00499 
00500 def distance_home(GPS_RAW):
00501     '''distance from first fix point'''
00502     global first_fix
00503     if (hasattr(GPS_RAW, 'fix_type') and GPS_RAW.fix_type < 2) or \
00504        (hasattr(GPS_RAW, 'Status')   and GPS_RAW.Status   < 2):
00505         return 0
00506 
00507     if first_fix == None:
00508         first_fix = GPS_RAW
00509         return 0
00510     return distance_two(GPS_RAW, first_fix)
00511 
00512 def sawtooth(ATTITUDE, amplitude=2.0, period=5.0):
00513     '''sawtooth pattern based on uptime'''
00514     mins = (ATTITUDE.usec * 1.0e-6)/60
00515     p = fmod(mins, period*2)
00516     if p < period:
00517         return amplitude * (p/period)
00518     return amplitude * (period - (p-period))/period
00519 
00520 def rate_of_turn(speed, bank):
00521     '''return expected rate of turn in degrees/s for given speed in m/s and
00522        bank angle in degrees'''
00523     if abs(speed) < 2 or abs(bank) > 80:
00524         return 0
00525     ret = degrees(9.81*tan(radians(bank))/speed)
00526     return ret
00527 
00528 def wingloading(bank):
00529     '''return expected wing loading factor for a bank angle in radians'''
00530     return 1.0/cos(bank)
00531 
00532 def airspeed(VFR_HUD, ratio=None, used_ratio=None, offset=None):
00533     '''recompute airspeed with a different ARSPD_RATIO'''
00534     import mavutil
00535     mav = mavutil.mavfile_global
00536     if ratio is None:
00537         ratio = 1.9936 # APM default
00538     if used_ratio is None:
00539         if 'ARSPD_RATIO' in mav.params:
00540             used_ratio = mav.params['ARSPD_RATIO']
00541         else:
00542             print("no ARSPD_RATIO in mav.params")
00543             used_ratio = ratio
00544     if hasattr(VFR_HUD,'airspeed'):
00545         airspeed = VFR_HUD.airspeed
00546     else:
00547         airspeed = VFR_HUD.Airspeed
00548     airspeed_pressure = (airspeed**2) / used_ratio
00549     if offset is not None:
00550         airspeed_pressure += offset
00551         if airspeed_pressure < 0:
00552             airspeed_pressure = 0
00553     airspeed = sqrt(airspeed_pressure * ratio)
00554     return airspeed
00555 
00556 def EAS2TAS(ARSP,GPS,BARO,ground_temp=25):
00557     '''EAS2TAS from ARSP.Temp'''
00558     tempK = ground_temp + 273.15 - 0.0065 * GPS.Alt
00559     return sqrt(1.225 / (BARO.Press / (287.26 * tempK)))
00560 
00561 
00562 def airspeed_ratio(VFR_HUD):
00563     '''recompute airspeed with a different ARSPD_RATIO'''
00564     import mavutil
00565     mav = mavutil.mavfile_global
00566     airspeed_pressure = (VFR_HUD.airspeed**2) / ratio
00567     airspeed = sqrt(airspeed_pressure * ratio)
00568     return airspeed
00569 
00570 def airspeed_voltage(VFR_HUD, ratio=None):
00571     '''back-calculate the voltage the airspeed sensor must have seen'''
00572     import mavutil
00573     mav = mavutil.mavfile_global
00574     if ratio is None:
00575         ratio = 1.9936 # APM default
00576     if 'ARSPD_RATIO' in mav.params:
00577         used_ratio = mav.params['ARSPD_RATIO']
00578     else:
00579         used_ratio = ratio
00580     if 'ARSPD_OFFSET' in mav.params:
00581         offset = mav.params['ARSPD_OFFSET']
00582     else:
00583         return -1
00584     airspeed_pressure = (pow(VFR_HUD.airspeed,2)) / used_ratio
00585     raw = airspeed_pressure + offset
00586     SCALING_OLD_CALIBRATION = 204.8
00587     voltage = 5.0 * raw / 4096
00588     return voltage
00589 
00590 
00591 def earth_rates(ATTITUDE):
00592     '''return angular velocities in earth frame'''
00593     from math import sin, cos, tan, fabs
00594 
00595     p     = ATTITUDE.rollspeed
00596     q     = ATTITUDE.pitchspeed
00597     r     = ATTITUDE.yawspeed
00598     phi   = ATTITUDE.roll
00599     theta = ATTITUDE.pitch
00600     psi   = ATTITUDE.yaw
00601 
00602     phiDot   = p + tan(theta)*(q*sin(phi) + r*cos(phi))
00603     thetaDot = q*cos(phi) - r*sin(phi)
00604     if fabs(cos(theta)) < 1.0e-20:
00605         theta += 1.0e-10
00606     psiDot   = (q*sin(phi) + r*cos(phi))/cos(theta)
00607     return (phiDot, thetaDot, psiDot)
00608 
00609 def roll_rate(ATTITUDE):
00610     '''return roll rate in earth frame'''
00611     (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE)
00612     return phiDot
00613 
00614 def pitch_rate(ATTITUDE):
00615     '''return pitch rate in earth frame'''
00616     (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE)
00617     return thetaDot
00618 
00619 def yaw_rate(ATTITUDE):
00620     '''return yaw rate in earth frame'''
00621     (phiDot, thetaDot, psiDot) = earth_rates(ATTITUDE)
00622     return psiDot
00623 
00624 
00625 def gps_velocity(GLOBAL_POSITION_INT):
00626     '''return GPS velocity vector'''
00627     return Vector3(GLOBAL_POSITION_INT.vx, GLOBAL_POSITION_INT.vy, GLOBAL_POSITION_INT.vz) * 0.01
00628 
00629 
00630 def gps_velocity_old(GPS_RAW_INT):
00631     '''return GPS velocity vector'''
00632     return Vector3(GPS_RAW_INT.vel*0.01*cos(radians(GPS_RAW_INT.cog*0.01)),
00633                    GPS_RAW_INT.vel*0.01*sin(radians(GPS_RAW_INT.cog*0.01)), 0)
00634 
00635 def gps_velocity_body(GPS_RAW_INT, ATTITUDE):
00636     '''return GPS velocity vector in body frame'''
00637     r = rotation(ATTITUDE)
00638     return r.transposed() * Vector3(GPS_RAW_INT.vel*0.01*cos(radians(GPS_RAW_INT.cog*0.01)),
00639                                     GPS_RAW_INT.vel*0.01*sin(radians(GPS_RAW_INT.cog*0.01)),
00640                                     -tan(ATTITUDE.pitch)*GPS_RAW_INT.vel*0.01)
00641 
00642 def earth_accel(RAW_IMU,ATTITUDE):
00643     '''return earth frame acceleration vector'''
00644     r = rotation(ATTITUDE)
00645     accel = Vector3(RAW_IMU.xacc, RAW_IMU.yacc, RAW_IMU.zacc) * 9.81 * 0.001
00646     return r * accel
00647 
00648 def earth_gyro(RAW_IMU,ATTITUDE):
00649     '''return earth frame gyro vector'''
00650     r = rotation(ATTITUDE)
00651     accel = Vector3(degrees(RAW_IMU.xgyro), degrees(RAW_IMU.ygyro), degrees(RAW_IMU.zgyro)) * 0.001
00652     return r * accel
00653 
00654 def airspeed_energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD):
00655     '''return airspeed energy error matching APM internals
00656     This is positive when we are going too slow
00657     '''
00658     aspeed_cm = VFR_HUD.airspeed*100
00659     target_airspeed = NAV_CONTROLLER_OUTPUT.aspd_error + aspeed_cm
00660     airspeed_energy_error = ((target_airspeed*target_airspeed) - (aspeed_cm*aspeed_cm))*0.00005
00661     return airspeed_energy_error
00662 
00663 
00664 def energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD):
00665     '''return energy error matching APM internals
00666     This is positive when we are too low or going too slow
00667     '''
00668     aspeed_energy_error = airspeed_energy_error(NAV_CONTROLLER_OUTPUT, VFR_HUD)
00669     alt_error = NAV_CONTROLLER_OUTPUT.alt_error*100
00670     energy_error = aspeed_energy_error + alt_error*0.098
00671     return energy_error
00672 
00673 def rover_turn_circle(SERVO_OUTPUT_RAW):
00674     '''return turning circle (diameter) in meters for steering_angle in degrees
00675     '''
00676 
00677     # this matches Toms slash
00678     max_wheel_turn = 35
00679     wheelbase      = 0.335
00680     wheeltrack     = 0.296
00681 
00682     steering_angle = max_wheel_turn * (SERVO_OUTPUT_RAW.servo1_raw - 1500) / 400.0
00683     theta = radians(steering_angle)
00684     return (wheeltrack/2) + (wheelbase/sin(theta))
00685 
00686 def rover_yaw_rate(VFR_HUD, SERVO_OUTPUT_RAW):
00687     '''return yaw rate in degrees/second given steering_angle and speed'''
00688     max_wheel_turn=35
00689     speed = VFR_HUD.groundspeed
00690     # assume 1100 to 1900 PWM on steering
00691     steering_angle = max_wheel_turn * (SERVO_OUTPUT_RAW.servo1_raw - 1500) / 400.0
00692     if abs(steering_angle) < 1.0e-6 or abs(speed) < 1.0e-6:
00693         return 0
00694     d = rover_turn_circle(SERVO_OUTPUT_RAW)
00695     c = pi * d
00696     t = c / speed
00697     rate = 360.0 / t
00698     return rate
00699 
00700 def rover_lat_accel(VFR_HUD, SERVO_OUTPUT_RAW):
00701     '''return lateral acceleration in m/s/s'''
00702     speed = VFR_HUD.groundspeed
00703     yaw_rate = rover_yaw_rate(VFR_HUD, SERVO_OUTPUT_RAW)
00704     accel = radians(yaw_rate) * speed
00705     return accel
00706 
00707 
00708 def demix1(servo1, servo2, gain=0.5):
00709     '''de-mix a mixed servo output'''
00710     s1 = servo1 - 1500
00711     s2 = servo2 - 1500
00712     out1 = (s1+s2)*gain
00713     out2 = (s1-s2)*gain
00714     return out1+1500
00715 
00716 def demix2(servo1, servo2, gain=0.5):
00717     '''de-mix a mixed servo output'''
00718     s1 = servo1 - 1500
00719     s2 = servo2 - 1500
00720     out1 = (s1+s2)*gain
00721     out2 = (s1-s2)*gain
00722     return out2+1500
00723 
00724 def mixer(servo1, servo2, mixtype=1, gain=0.5):
00725     '''mix two servos'''
00726     s1 = servo1 - 1500
00727     s2 = servo2 - 1500
00728     v1 = (s1-s2)*gain
00729     v2 = (s1+s2)*gain
00730     if mixtype == 2:
00731         v2 = -v2
00732     elif mixtype == 3:
00733         v1 = -v1
00734     elif mixtype == 4:
00735         v1 = -v1
00736         v2 = -v2
00737     if v1 > 600:
00738         v1 = 600
00739     elif v1 < -600:
00740         v1 = -600
00741     if v2 > 600:
00742         v2 = 600
00743     elif v2 < -600:
00744         v2 = -600
00745     return (1500+v1,1500+v2)
00746 
00747 def mix1(servo1, servo2, mixtype=1, gain=0.5):
00748     '''de-mix a mixed servo output'''
00749     (v1,v2) = mixer(servo1, servo2, mixtype=mixtype, gain=gain)
00750     return v1
00751 
00752 def mix2(servo1, servo2, mixtype=1, gain=0.5):
00753     '''de-mix a mixed servo output'''
00754     (v1,v2) = mixer(servo1, servo2, mixtype=mixtype, gain=gain)
00755     return v2
00756 
00757 def wrap_180(angle):
00758     if angle > 180:
00759         angle -= 360.0
00760     if angle < -180:
00761         angle += 360.0
00762     return angle
00763 
00764 
00765 def wrap_360(angle):
00766     if angle > 360:
00767         angle -= 360.0
00768     if angle < 0:
00769         angle += 360.0
00770     return angle
00771 
00772 class DCM_State(object):
00773     '''DCM state object'''
00774     def __init__(self, roll, pitch, yaw):
00775         self.dcm = Matrix3()
00776         self.dcm2 = Matrix3()
00777         self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw))
00778         self.dcm2.from_euler(radians(roll), radians(pitch), radians(yaw))
00779         self.mag = Vector3()
00780         self.gyro = Vector3()
00781         self.accel = Vector3()
00782         self.gps = None
00783         self.rate = 50.0
00784         self.kp = 0.2
00785         self.kp_yaw = 0.3
00786         self.omega_P = Vector3()
00787         self.omega_P_yaw = Vector3()
00788         self.omega_I = Vector3() # (-0.00199045287445, -0.00653007719666, -0.00714212376624)
00789         self.omega_I_sum = Vector3()
00790         self.omega_I_sum_time = 0
00791         self.omega = Vector3()
00792         self.ra_sum = Vector3()
00793         self.last_delta_angle = Vector3()
00794         self.last_velocity = Vector3()
00795         (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()
00796         (self.roll2, self.pitch2, self.yaw2) = self.dcm2.to_euler()
00797 
00798     def update(self, gyro, accel, mag, GPS):
00799         if self.gyro != gyro or self.accel != accel:
00800             delta_angle = (gyro+self.omega_I) / self.rate
00801             self.dcm.rotate(delta_angle)
00802             correction = self.last_delta_angle % delta_angle
00803             #print (delta_angle - self.last_delta_angle) * 58.0
00804             corrected_delta = delta_angle + 0.0833333 * correction
00805             self.dcm2.rotate(corrected_delta)
00806             self.last_delta_angle = delta_angle
00807 
00808             self.dcm.normalize()
00809             self.dcm2.normalize()
00810 
00811             self.gyro = gyro
00812             self.accel = accel
00813             (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()
00814             (self.roll2, self.pitch2, self.yaw2) = self.dcm2.to_euler()
00815 
00816 dcm_state = None
00817 
00818 def DCM_update(IMU, ATT, MAG, GPS):
00819     '''implement full DCM system'''
00820     global dcm_state
00821     if dcm_state is None:
00822         dcm_state = DCM_State(ATT.Roll, ATT.Pitch, ATT.Yaw)
00823 
00824     mag   = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
00825     gyro  = Vector3(IMU.GyrX, IMU.GyrY, IMU.GyrZ)
00826     accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
00827     accel2 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
00828     dcm_state.update(gyro, accel, mag, GPS)
00829     return dcm_state
00830 
00831 class PX4_State(object):
00832     '''PX4 DCM state object'''
00833     def __init__(self, roll, pitch, yaw, timestamp):
00834         self.dcm = Matrix3()
00835         self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw))
00836         self.gyro = Vector3()
00837         self.accel = Vector3()
00838         self.timestamp = timestamp
00839         (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()
00840 
00841     def update(self, gyro, accel, timestamp):
00842         if self.gyro != gyro or self.accel != accel:
00843             delta_angle = gyro * (timestamp - self.timestamp)
00844             self.timestamp = timestamp
00845             self.dcm.rotate(delta_angle)
00846             self.dcm.normalize()
00847             self.gyro = gyro
00848             self.accel = accel
00849             (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()
00850 
00851 px4_state = None
00852 
00853 def PX4_update(IMU, ATT):
00854     '''implement full DCM using PX4 native SD log data'''
00855     global px4_state
00856     if px4_state is None:
00857         px4_state = PX4_State(degrees(ATT.Roll), degrees(ATT.Pitch), degrees(ATT.Yaw), IMU._timestamp)
00858 
00859     gyro  = Vector3(IMU.GyroX, IMU.GyroY, IMU.GyroZ)
00860     accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
00861     px4_state.update(gyro, accel, IMU._timestamp)
00862     return px4_state
00863 
00864 _downsample_N = 0
00865 
00866 def downsample(N):
00867     '''conditional that is true on every Nth sample'''
00868     global _downsample_N
00869     _downsample_N = (_downsample_N + 1) % N
00870     return _downsample_N == 0
00871 
00872 def armed(HEARTBEAT):
00873     '''return 1 if armed, 0 if not'''
00874     from . import mavutil
00875     if HEARTBEAT.type == mavutil.mavlink.MAV_TYPE_GCS:
00876         self = mavutil.mavfile_global
00877         if self.motors_armed():
00878             return 1
00879         return 0
00880     if HEARTBEAT.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
00881         return 1
00882     return 0
00883 
00884 def rotation_df(ATT):
00885     '''return the current DCM rotation matrix'''
00886     r = Matrix3()
00887     r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw))
00888     return r
00889 
00890 def rotation2(AHRS2):
00891     '''return the current DCM rotation matrix'''
00892     r = Matrix3()
00893     r.from_euler(AHRS2.roll, AHRS2.pitch, AHRS2.yaw)
00894     return r
00895 
00896 def earth_accel2(RAW_IMU,ATTITUDE):
00897     '''return earth frame acceleration vector from AHRS2'''
00898     r = rotation2(ATTITUDE)
00899     accel = Vector3(RAW_IMU.xacc, RAW_IMU.yacc, RAW_IMU.zacc) * 9.81 * 0.001
00900     return r * accel
00901 
00902 def earth_accel_df(IMU,ATT):
00903     '''return earth frame acceleration vector from df log'''
00904     r = rotation_df(ATT)
00905     accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
00906     return r * accel
00907 
00908 def earth_accel2_df(IMU,IMU2,ATT):
00909     '''return earth frame acceleration vector from df log'''
00910     r = rotation_df(ATT)
00911     accel1 = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
00912     accel2 = Vector3(IMU2.AccX, IMU2.AccY, IMU2.AccZ)
00913     accel = 0.5 * (accel1 + accel2)
00914     return r * accel
00915 
00916 def gps_velocity_df(GPS):
00917     '''return GPS velocity vector'''
00918     vx = GPS.Spd * cos(radians(GPS.GCrs))
00919     vy = GPS.Spd * sin(radians(GPS.GCrs))
00920     return Vector3(vx, vy, GPS.VZ)
00921 
00922 def distance_gps2(GPS, GPS2):
00923     '''distance between two points'''
00924     if GPS.TimeMS != GPS2.TimeMS:
00925         # reject messages not time aligned
00926         return None
00927     return distance_two(GPS, GPS2)
00928 
00929 
00930 radius_of_earth = 6378100.0 # in meters
00931 
00932 def wrap_valid_longitude(lon):
00933   ''' wrap a longitude value around to always have a value in the range
00934       [-180, +180) i.e 0 => 0, 1 => 1, -1 => -1, 181 => -179, -181 => 179
00935   '''
00936   return (((lon + 180.0) % 360.0) - 180.0)
00937 
00938 def gps_newpos(lat, lon, bearing, distance):
00939   '''extrapolate latitude/longitude given a heading and distance
00940   thanks to http://www.movable-type.co.uk/scripts/latlong.html
00941   '''
00942   import math
00943   lat1 = math.radians(lat)
00944   lon1 = math.radians(lon)
00945   brng = math.radians(bearing)
00946   dr = distance/radius_of_earth
00947 
00948   lat2 = math.asin(math.sin(lat1)*math.cos(dr) +
00949                    math.cos(lat1)*math.sin(dr)*math.cos(brng))
00950   lon2 = lon1 + math.atan2(math.sin(brng)*math.sin(dr)*math.cos(lat1),
00951                            math.cos(dr)-math.sin(lat1)*math.sin(lat2))
00952   return (math.degrees(lat2), wrap_valid_longitude(math.degrees(lon2)))
00953 
00954 def gps_offset(lat, lon, east, north):
00955   '''return new lat/lon after moving east/north
00956   by the given number of meters'''
00957   import math
00958   bearing = math.degrees(math.atan2(east, north))
00959   distance = math.sqrt(east**2 + north**2)
00960   return gps_newpos(lat, lon, bearing, distance)
00961 
00962 ekf_home = None
00963 
00964 def ekf1_pos(EKF1):
00965   '''calculate EKF position when EKF disabled'''
00966   global ekf_home
00967   from . import mavutil
00968   self = mavutil.mavfile_global
00969   if ekf_home is None:
00970       if not 'GPS' in self.messages or self.messages['GPS'].Status != 3:
00971           return None
00972       ekf_home = self.messages['GPS']
00973       (ekf_home.Lat, ekf_home.Lng) = gps_offset(ekf_home.Lat, ekf_home.Lng, -EKF1.PE, -EKF1.PN)
00974   (lat,lon) = gps_offset(ekf_home.Lat, ekf_home.Lng, EKF1.PE, EKF1.PN)
00975   return (lat, lon)
00976 
00977 def quat_to_euler(q):
00978   '''
00979   Get Euler angles from a quaternion
00980   :param q: quaternion [w, x, y , z]
00981   :returns: euler angles [roll, pitch, yaw]
00982   '''
00983   quat = Quaternion(q)
00984   return quat.euler
00985 
00986 def euler_to_quat(e):
00987   '''
00988   Get quaternion from euler angles
00989   :param e: euler angles [roll, pitch, yaw]
00990   :returns: quaternion [w, x, y , z]
00991   '''
00992   quat = Quaternion(e)
00993   return quat.q
00994 
00995 def rotate_quat(attitude, roll, pitch, yaw):
00996   '''
00997   Returns rotated quaternion
00998   :param attitude: quaternion [w, x, y , z]
00999   :param roll: rotation in rad
01000   :param pitch: rotation in rad
01001   :param yaw: rotation in rad
01002   :returns: quaternion [w, x, y , z]
01003   '''
01004   quat = Quaternion(attitude)
01005   rotation = Quaternion([roll, pitch, yaw])
01006   res = rotation * quat
01007 
01008   return res.q


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57