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.
def pymavlink.mavextra.airspeed_ratio | ( | VFR_HUD | ) |
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.
def pymavlink.mavextra.distance_home | ( | GPS_RAW | ) |
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.
def pymavlink.mavextra.downsample | ( | N | ) |
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.
def pymavlink.mavextra.ekf1_pos | ( | EKF1 | ) |
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.
def pymavlink.mavextra.euler_to_quat | ( | e | ) |
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.
def pymavlink.mavextra.gps_velocity_df | ( | GPS | ) |
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.
def pymavlink.mavextra.quat_to_euler | ( | q | ) |
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.
def pymavlink.mavextra.rotation2 | ( | AHRS2 | ) |
return the current DCM rotation matrix
Definition at line 890 of file mavextra.py.
def pymavlink.mavextra.rotation_df | ( | ATT | ) |
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.
def pymavlink.mavextra.wingloading | ( | bank | ) |
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.
def pymavlink.mavextra.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
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.
Definition at line 864 of file mavextra.py.
dictionary pymavlink::mavextra::average_data = {} |
Definition at line 168 of file mavextra.py.
Definition at line 816 of file mavextra.py.
dictionary pymavlink::mavextra::derivative_data = {} |
Definition at line 180 of file mavextra.py.
Definition at line 962 of file mavextra.py.
Definition at line 498 of file mavextra.py.
dictionary pymavlink::mavextra::last_delta = {} |
Definition at line 245 of file mavextra.py.
dictionary pymavlink::mavextra::last_diff = {} |
Definition at line 232 of file mavextra.py.
dictionary pymavlink::mavextra::lowpass_data = {} |
Definition at line 221 of file mavextra.py.
Definition at line 851 of file mavextra.py.
float pymavlink::mavextra::radius_of_earth = 6378100.0 |
Definition at line 930 of file mavextra.py.