Classes | Functions | Variables
pymavlink::mavextra Namespace Reference

Classes

class  DCM_State
class  PX4_State

Functions

def airspeed
def airspeed_energy_error
def airspeed_ratio
def airspeed_voltage
def altitude
def altitude2
def angle_diff
def armed
def average
def DCM_update
def delta
def delta_angle
def demix1
def demix2
def diff
def distance_gps2
def distance_home
def distance_two
def downsample
def earth_accel
def earth_accel2
def earth_accel2_df
def earth_accel_df
def earth_gyro
def earth_rates
def EAS2TAS
def ekf1_pos
def energy_error
def euler_to_quat
def expected_mag
def expected_magx
def expected_magy
def expected_magz
def get_motor_offsets
def gps_newpos
def gps_offset
def gps_velocity
def gps_velocity_body
def gps_velocity_df
def gps_velocity_old
def gravity
def kmh
def lowpass
def mag_discrepancy
def mag_field
def mag_field_df
def mag_field_motors
def mag_heading
def mag_heading_motors
def mag_inclination
def mag_pitch
def mag_roll
def mag_rotation
def mag_yaw
def mix1
def mix2
def mixer
def pitch_estimate
def pitch_rate
def pitch_sim
def PX4_update
def quat_to_euler
def rate_of_turn
def roll_estimate
def roll_rate
def rotate_quat
def rotation
def rotation2
def rotation_df
def rover_lat_accel
def rover_turn_circle
def rover_yaw_rate
def sawtooth
def second_derivative_5
def second_derivative_9
def wingloading
def wrap_180
def wrap_360
def wrap_valid_longitude
def yaw_rate

Variables

int _downsample_N = 0
dictionary average_data = {}
 dcm_state = None
dictionary derivative_data = {}
 ekf_home = None
 first_fix = None
dictionary last_delta = {}
dictionary last_diff = {}
dictionary lowpass_data = {}
 px4_state = None
float radius_of_earth = 6378100.0

Function Documentation

def pymavlink.mavextra.airspeed (   VFR_HUD,
  ratio = None,
  used_ratio = None,
  offset = None 
)
recompute airspeed with a different ARSPD_RATIO

Definition at line 527 of file mavextra.py.

def pymavlink.mavextra.airspeed_energy_error (   NAV_CONTROLLER_OUTPUT,
  VFR_HUD 
)
return airspeed energy error matching APM internals
This is positive when we are going too slow

Definition at line 649 of file mavextra.py.

recompute airspeed with a different ARSPD_RATIO

Definition at line 557 of file mavextra.py.

def pymavlink.mavextra.airspeed_voltage (   VFR_HUD,
  ratio = None 
)
back-calculate the voltage the airspeed sensor must have seen

Definition at line 565 of file mavextra.py.

def pymavlink.mavextra.altitude (   SCALED_PRESSURE,
  ground_pressure = None,
  ground_temp = None 
)
calculate barometric altitude

Definition at line 24 of file mavextra.py.

def pymavlink.mavextra.altitude2 (   SCALED_PRESSURE,
  ground_pressure = None,
  ground_temp = None 
)
calculate barometric altitude

Definition at line 38 of file mavextra.py.

def pymavlink.mavextra.angle_diff (   angle1,
  angle2 
)
show the difference between two angles in degrees

Definition at line 154 of file mavextra.py.

def pymavlink.mavextra.armed (   HEARTBEAT)
return 1 if armed, 0 if not

Definition at line 867 of file mavextra.py.

def pymavlink.mavextra.average (   var,
  key,
  N 
)
average over N points

Definition at line 165 of file mavextra.py.

def pymavlink.mavextra.DCM_update (   IMU,
  ATT,
  MAG,
  GPS 
)
implement full DCM system

Definition at line 813 of file mavextra.py.

def pymavlink.mavextra.delta (   var,
  key,
  tusec = None 
)
calculate slope

Definition at line 242 of file mavextra.py.

def pymavlink.mavextra.delta_angle (   var,
  key,
  tusec = None 
)
calculate slope of an angle

Definition at line 263 of file mavextra.py.

def pymavlink.mavextra.demix1 (   servo1,
  servo2,
  gain = 0.5 
)
de-mix a mixed servo output

Definition at line 703 of file mavextra.py.

def pymavlink.mavextra.demix2 (   servo1,
  servo2,
  gain = 0.5 
)
de-mix a mixed servo output

Definition at line 711 of file mavextra.py.

def pymavlink.mavextra.diff (   var,
  key 
)
calculate differences between values

Definition at line 229 of file mavextra.py.

def pymavlink.mavextra.distance_gps2 (   GPS,
  GPS2 
)
distance between two points

Definition at line 917 of file mavextra.py.

distance from first fix point

Definition at line 495 of file mavextra.py.

def pymavlink.mavextra.distance_two (   GPS_RAW1,
  GPS_RAW2,
  horizontal = True 
)
distance between two points

Definition at line 459 of file mavextra.py.

conditional that is true on every Nth sample

Definition at line 861 of file mavextra.py.

def pymavlink.mavextra.earth_accel (   RAW_IMU,
  ATTITUDE 
)
return earth frame acceleration vector

Definition at line 637 of file mavextra.py.

def pymavlink.mavextra.earth_accel2 (   RAW_IMU,
  ATTITUDE 
)
return earth frame acceleration vector from AHRS2

Definition at line 891 of file mavextra.py.

def pymavlink.mavextra.earth_accel2_df (   IMU,
  IMU2,
  ATT 
)
return earth frame acceleration vector from df log

Definition at line 903 of file mavextra.py.

def pymavlink.mavextra.earth_accel_df (   IMU,
  ATT 
)
return earth frame acceleration vector from df log

Definition at line 897 of file mavextra.py.

def pymavlink.mavextra.earth_gyro (   RAW_IMU,
  ATTITUDE 
)
return earth frame gyro vector

Definition at line 643 of file mavextra.py.

def pymavlink.mavextra.earth_rates (   ATTITUDE)
return angular velocities in earth frame

Definition at line 586 of file mavextra.py.

def pymavlink.mavextra.EAS2TAS (   ARSP,
  GPS,
  BARO,
  ground_temp = 25 
)
EAS2TAS from ARSP.Temp

Definition at line 551 of file mavextra.py.

calculate EKF position when EKF disabled

Definition at line 959 of file mavextra.py.

def pymavlink.mavextra.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

Definition at line 659 of file mavextra.py.

Get quaternion from euler angles
:param e: euler angles [roll, pitch, yaw]
:returns: quaternion [w, x, y , z]

Definition at line 981 of file mavextra.py.

def pymavlink.mavextra.expected_mag (   RAW_IMU,
  ATTITUDE,
  inclination,
  declination 
)
return expected mag vector

Definition at line 371 of file mavextra.py.

def pymavlink.mavextra.expected_magx (   RAW_IMU,
  ATTITUDE,
  inclination,
  declination 
)
estimate  from mag

Definition at line 408 of file mavextra.py.

def pymavlink.mavextra.expected_magy (   RAW_IMU,
  ATTITUDE,
  inclination,
  declination 
)
estimate  from mag

Definition at line 413 of file mavextra.py.

def pymavlink.mavextra.expected_magz (   RAW_IMU,
  ATTITUDE,
  inclination,
  declination 
)
estimate  from mag

Definition at line 418 of file mavextra.py.

def pymavlink.mavextra.get_motor_offsets (   SERVO_OUTPUT_RAW,
  ofs,
  motor_ofs 
)
calculate magnetic field strength from raw magnetometer

Definition at line 117 of file mavextra.py.

def pymavlink.mavextra.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

Definition at line 933 of file mavextra.py.

def pymavlink.mavextra.gps_offset (   lat,
  lon,
  east,
  north 
)
return new lat/lon after moving east/north
by the given number of meters

Definition at line 949 of file mavextra.py.

def pymavlink.mavextra.gps_velocity (   GLOBAL_POSITION_INT)
return GPS velocity vector

Definition at line 620 of file mavextra.py.

def pymavlink.mavextra.gps_velocity_body (   GPS_RAW_INT,
  ATTITUDE 
)
return GPS velocity vector in body frame

Definition at line 630 of file mavextra.py.

return GPS velocity vector

Definition at line 911 of file mavextra.py.

def pymavlink.mavextra.gps_velocity_old (   GPS_RAW_INT)
return GPS velocity vector

Definition at line 625 of file mavextra.py.

def pymavlink.mavextra.gravity (   RAW_IMU,
  SENSOR_OFFSETS = None,
  ofs = None,
  mul = None,
  smooth = 0.7 
)
estimate pitch from accelerometer

Definition at line 423 of file mavextra.py.

def pymavlink.mavextra.kmh (   mps)
convert m/s to Km/h

Definition at line 20 of file mavextra.py.

def pymavlink.mavextra.lowpass (   var,
  key,
  factor 
)
a simple lowpass filter

Definition at line 218 of file mavextra.py.

def pymavlink.mavextra.mag_discrepancy (   RAW_IMU,
  ATTITUDE,
  inclination,
  declination = None 
)
give the magnitude of the discrepancy between observed and expected magnetic field

Definition at line 384 of file mavextra.py.

def pymavlink.mavextra.mag_field (   RAW_IMU,
  SENSOR_OFFSETS = None,
  ofs = None 
)
calculate magnetic field strength from raw magnetometer

Definition at line 98 of file mavextra.py.

def pymavlink.mavextra.mag_field_df (   MAG,
  ofs = None 
)
calculate magnetic field strength from raw magnetometer (dataflash version)

Definition at line 109 of file mavextra.py.

def pymavlink.mavextra.mag_field_motors (   RAW_IMU,
  SENSOR_OFFSETS,
  ofs,
  SERVO_OUTPUT_RAW,
  motor_ofs 
)
calculate magnetic field strength from raw magnetometer

Definition at line 140 of file mavextra.py.

def pymavlink.mavextra.mag_heading (   RAW_IMU,
  ATTITUDE,
  declination = None,
  SENSOR_OFFSETS = None,
  ofs = None 
)
calculate heading from raw magnetometer

Definition at line 52 of file mavextra.py.

def pymavlink.mavextra.mag_heading_motors (   RAW_IMU,
  ATTITUDE,
  declination,
  SENSOR_OFFSETS,
  ofs,
  SERVO_OUTPUT_RAW,
  motor_ofs 
)
calculate heading from raw magnetometer

Definition at line 76 of file mavextra.py.

def pymavlink.mavextra.mag_inclination (   RAW_IMU,
  ATTITUDE,
  declination = None 
)
give the magnitude of the discrepancy between observed and expected magnetic field

Definition at line 394 of file mavextra.py.

def pymavlink.mavextra.mag_pitch (   RAW_IMU,
  inclination,
  declination 
)
estimate pithc from mag

Definition at line 359 of file mavextra.py.

def pymavlink.mavextra.mag_roll (   RAW_IMU,
  inclination,
  declination 
)
estimate roll from mag

Definition at line 365 of file mavextra.py.

def pymavlink.mavextra.mag_rotation (   RAW_IMU,
  inclination,
  declination 
)
return an attitude rotation matrix that is consistent with the current mag
   vector

Definition at line 337 of file mavextra.py.

def pymavlink.mavextra.mag_yaw (   RAW_IMU,
  inclination,
  declination 
)
estimate yaw from mag

Definition at line 350 of file mavextra.py.

def pymavlink.mavextra.mix1 (   servo1,
  servo2,
  mixtype = 1,
  gain = 0.5 
)
de-mix a mixed servo output

Definition at line 742 of file mavextra.py.

def pymavlink.mavextra.mix2 (   servo1,
  servo2,
  mixtype = 1,
  gain = 0.5 
)
de-mix a mixed servo output

Definition at line 747 of file mavextra.py.

def pymavlink.mavextra.mixer (   servo1,
  servo2,
  mixtype = 1,
  gain = 0.5 
)
mix two servos

Definition at line 719 of file mavextra.py.

def pymavlink.mavextra.pitch_estimate (   RAW_IMU,
  GPS_RAW_INT = None,
  ATTITUDE = None,
  SENSOR_OFFSETS = None,
  ofs = None,
  mul = None,
  smooth = 0.7 
)
estimate pitch from accelerometer

Definition at line 310 of file mavextra.py.

def pymavlink.mavextra.pitch_rate (   ATTITUDE)
return pitch rate in earth frame

Definition at line 609 of file mavextra.py.

def pymavlink.mavextra.pitch_sim (   SIMSTATE,
  GPS_RAW 
)
estimate pitch from SIMSTATE accels

Definition at line 448 of file mavextra.py.

def pymavlink.mavextra.PX4_update (   IMU,
  ATT 
)
implement full DCM using PX4 native SD log data

Definition at line 848 of file mavextra.py.

Get Euler angles from a quaternion
:param q: quaternion [w, x, y , z]
:returns: euler angles [roll, pitch, yaw]

Definition at line 972 of file mavextra.py.

def pymavlink.mavextra.rate_of_turn (   speed,
  bank 
)
return expected rate of turn in degrees/s for given speed in m/s and
   bank angle in degrees

Definition at line 515 of file mavextra.py.

def pymavlink.mavextra.roll_estimate (   RAW_IMU,
  GPS_RAW_INT = None,
  ATTITUDE = None,
  SENSOR_OFFSETS = None,
  ofs = None,
  mul = None,
  smooth = 0.7 
)
estimate roll from accelerometer

Definition at line 289 of file mavextra.py.

def pymavlink.mavextra.roll_rate (   ATTITUDE)
return roll rate in earth frame

Definition at line 604 of file mavextra.py.

def pymavlink.mavextra.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]

Definition at line 990 of file mavextra.py.

def pymavlink.mavextra.rotation (   ATTITUDE)
return the current DCM rotation matrix

Definition at line 331 of file mavextra.py.

return the current DCM rotation matrix

Definition at line 885 of file mavextra.py.

return the current DCM rotation matrix

Definition at line 879 of file mavextra.py.

def pymavlink.mavextra.rover_lat_accel (   VFR_HUD,
  SERVO_OUTPUT_RAW 
)
return lateral acceleration in m/s/s

Definition at line 695 of file mavextra.py.

def pymavlink.mavextra.rover_turn_circle (   SERVO_OUTPUT_RAW)
return turning circle (diameter) in meters for steering_angle in degrees

Definition at line 668 of file mavextra.py.

def pymavlink.mavextra.rover_yaw_rate (   VFR_HUD,
  SERVO_OUTPUT_RAW 
)
return yaw rate in degrees/second given steering_angle and speed

Definition at line 681 of file mavextra.py.

def pymavlink.mavextra.sawtooth (   ATTITUDE,
  amplitude = 2.0,
  period = 5.0 
)
sawtooth pattern based on uptime

Definition at line 507 of file mavextra.py.

def pymavlink.mavextra.second_derivative_5 (   var,
  key 
)
5 point 2nd derivative

Definition at line 177 of file mavextra.py.

def pymavlink.mavextra.second_derivative_9 (   var,
  key 
)
9 point 2nd derivative

Definition at line 196 of file mavextra.py.

return expected wing loading factor for a bank angle in radians

Definition at line 523 of file mavextra.py.

def pymavlink.mavextra.wrap_180 (   angle)

Definition at line 752 of file mavextra.py.

def pymavlink.mavextra.wrap_360 (   angle)

Definition at line 760 of file mavextra.py.

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

Definition at line 927 of file mavextra.py.

def pymavlink.mavextra.yaw_rate (   ATTITUDE)
return yaw rate in earth frame

Definition at line 614 of file mavextra.py.


Variable Documentation

Definition at line 859 of file mavextra.py.

Definition at line 163 of file mavextra.py.

Definition at line 811 of file mavextra.py.

Definition at line 175 of file mavextra.py.

Definition at line 957 of file mavextra.py.

Definition at line 493 of file mavextra.py.

Definition at line 240 of file mavextra.py.

Definition at line 227 of file mavextra.py.

Definition at line 216 of file mavextra.py.

Definition at line 846 of file mavextra.py.

Definition at line 925 of file mavextra.py.



mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:18