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 532 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 654 of file mavextra.py.

recompute airspeed with a different ARSPD_RATIO

Definition at line 562 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 570 of file mavextra.py.

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

Definition at line 29 of file mavextra.py.

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

Definition at line 43 of file mavextra.py.

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

Definition at line 159 of file mavextra.py.

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

Definition at line 872 of file mavextra.py.

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

Definition at line 170 of file mavextra.py.

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

Definition at line 818 of file mavextra.py.

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

Definition at line 247 of file mavextra.py.

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

Definition at line 268 of file mavextra.py.

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

Definition at line 708 of file mavextra.py.

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

Definition at line 716 of file mavextra.py.

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

Definition at line 234 of file mavextra.py.

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

Definition at line 922 of file mavextra.py.

distance from first fix point

Definition at line 500 of file mavextra.py.

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

Definition at line 464 of file mavextra.py.

conditional that is true on every Nth sample

Definition at line 866 of file mavextra.py.

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

Definition at line 642 of file mavextra.py.

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

Definition at line 896 of file mavextra.py.

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

Definition at line 908 of file mavextra.py.

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

Definition at line 902 of file mavextra.py.

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

Definition at line 648 of file mavextra.py.

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

Definition at line 591 of file mavextra.py.

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

Definition at line 556 of file mavextra.py.

calculate EKF position when EKF disabled

Definition at line 964 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 664 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 986 of file mavextra.py.

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

Definition at line 376 of file mavextra.py.

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

Definition at line 413 of file mavextra.py.

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

Definition at line 418 of file mavextra.py.

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

Definition at line 423 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 122 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 938 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 954 of file mavextra.py.

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

Definition at line 625 of file mavextra.py.

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

Definition at line 635 of file mavextra.py.

return GPS velocity vector

Definition at line 916 of file mavextra.py.

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

Definition at line 630 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 428 of file mavextra.py.

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

Definition at line 25 of file mavextra.py.

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

Definition at line 223 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 389 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 103 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 114 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 145 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 57 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 81 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 399 of file mavextra.py.

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

Definition at line 364 of file mavextra.py.

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

Definition at line 370 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 342 of file mavextra.py.

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

Definition at line 355 of file mavextra.py.

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

Definition at line 747 of file mavextra.py.

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

Definition at line 752 of file mavextra.py.

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

Definition at line 724 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 315 of file mavextra.py.

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

Definition at line 614 of file mavextra.py.

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

Definition at line 453 of file mavextra.py.

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

Definition at line 853 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 977 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 520 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 294 of file mavextra.py.

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

Definition at line 609 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 995 of file mavextra.py.

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

Definition at line 336 of file mavextra.py.

return the current DCM rotation matrix

Definition at line 890 of file mavextra.py.

return the current DCM rotation matrix

Definition at line 884 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 700 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 673 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 686 of file mavextra.py.

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

Definition at line 512 of file mavextra.py.

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

Definition at line 182 of file mavextra.py.

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

Definition at line 201 of file mavextra.py.

return expected wing loading factor for a bank angle in radians

Definition at line 528 of file mavextra.py.

def pymavlink.mavextra.wrap_180 (   angle)

Definition at line 757 of file mavextra.py.

def pymavlink.mavextra.wrap_360 (   angle)

Definition at line 765 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 932 of file mavextra.py.

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

Definition at line 619 of file mavextra.py.


Variable Documentation

Definition at line 864 of file mavextra.py.

Definition at line 168 of file mavextra.py.

Definition at line 816 of file mavextra.py.

Definition at line 180 of file mavextra.py.

Definition at line 962 of file mavextra.py.

Definition at line 498 of file mavextra.py.

Definition at line 245 of file mavextra.py.

Definition at line 232 of file mavextra.py.

Definition at line 221 of file mavextra.py.

Definition at line 851 of file mavextra.py.

Definition at line 930 of file mavextra.py.



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