mirror of https://github.com/ArduPilot/ardupilot
mavlink: added auto-detection of mavlink protocol version
This commit is contained in:
parent
9cadd8c1db
commit
830e5997d2
|
@ -103,16 +103,9 @@ parser.add_option("--skip", type='string', default='', help='list of steps to sk
|
||||||
parser.add_option("--list", action='store_true', default=False, help='list the available steps')
|
parser.add_option("--list", action='store_true', default=False, help='list the available steps')
|
||||||
parser.add_option("--viewerip", default=None, help='IP address to send MAVLink and fg packets to')
|
parser.add_option("--viewerip", default=None, help='IP address to send MAVLink and fg packets to')
|
||||||
parser.add_option("--experimental", default=False, action='store_true', help='enable experimental tests')
|
parser.add_option("--experimental", default=False, action='store_true', help='enable experimental tests')
|
||||||
parser.add_option("--mav09", action='store_true', default=False, help="Use MAVLink protocol 0.9")
|
|
||||||
|
|
||||||
opts, args = parser.parse_args()
|
opts, args = parser.parse_args()
|
||||||
|
|
||||||
if not opts.mav09:
|
|
||||||
os.environ['MAVLINK10'] = '1'
|
|
||||||
import mavlinkv10 as mavlink
|
|
||||||
else:
|
|
||||||
import mavlink as mavlink
|
|
||||||
|
|
||||||
import arducopter, arduplane
|
import arducopter, arduplane
|
||||||
|
|
||||||
steps = [
|
steps = [
|
||||||
|
|
|
@ -17,10 +17,10 @@ def altitude(SCALED_PRESSURE):
|
||||||
'''calculate barometric altitude'''
|
'''calculate barometric altitude'''
|
||||||
import mavutil
|
import mavutil
|
||||||
self = mavutil.mavfile_global
|
self = mavutil.mavfile_global
|
||||||
if self.ground_pressure is None or self.ground_temperature is None:
|
if self.param('GND_ABS_PRESS', None) is None:
|
||||||
return 0
|
return 0
|
||||||
scaling = self.ground_pressure / (SCALED_PRESSURE.press_abs*100.0)
|
scaling = self.param('GND_ABS_PRESS', 1) / (SCALED_PRESSURE.press_abs*100.0)
|
||||||
temp = self.ground_temperature + 273.15
|
temp = self.param('GND_TEMP', 0) + 273.15
|
||||||
return log(scaling) * temp * 29271.267 * 0.001
|
return log(scaling) * temp * 29271.267 * 0.001
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -10,6 +10,21 @@ import struct, array, mavutil, time
|
||||||
|
|
||||||
WIRE_PROTOCOL_VERSION = "0.9"
|
WIRE_PROTOCOL_VERSION = "0.9"
|
||||||
|
|
||||||
|
|
||||||
|
# some base types from mavlink_types.h
|
||||||
|
MAVLINK_TYPE_CHAR = 0
|
||||||
|
MAVLINK_TYPE_UINT8_T = 1
|
||||||
|
MAVLINK_TYPE_INT8_T = 2
|
||||||
|
MAVLINK_TYPE_UINT16_T = 3
|
||||||
|
MAVLINK_TYPE_INT16_T = 4
|
||||||
|
MAVLINK_TYPE_UINT32_T = 5
|
||||||
|
MAVLINK_TYPE_INT32_T = 6
|
||||||
|
MAVLINK_TYPE_UINT64_T = 7
|
||||||
|
MAVLINK_TYPE_INT64_T = 8
|
||||||
|
MAVLINK_TYPE_FLOAT = 9
|
||||||
|
MAVLINK_TYPE_DOUBLE = 10
|
||||||
|
|
||||||
|
|
||||||
class MAVLink_header(object):
|
class MAVLink_header(object):
|
||||||
'''MAVLink message header'''
|
'''MAVLink message header'''
|
||||||
def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
|
def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
|
||||||
|
@ -34,7 +49,9 @@ class MAVLink_message(object):
|
||||||
self._type = name
|
self._type = name
|
||||||
|
|
||||||
def get_msgbuf(self):
|
def get_msgbuf(self):
|
||||||
|
if isinstance(self._msgbuf, str):
|
||||||
return self._msgbuf
|
return self._msgbuf
|
||||||
|
return self._msgbuf.tostring()
|
||||||
|
|
||||||
def get_header(self):
|
def get_header(self):
|
||||||
return self._header
|
return self._header
|
||||||
|
|
|
@ -10,6 +10,21 @@ import struct, array, mavutil, time
|
||||||
|
|
||||||
WIRE_PROTOCOL_VERSION = "1.0"
|
WIRE_PROTOCOL_VERSION = "1.0"
|
||||||
|
|
||||||
|
|
||||||
|
# some base types from mavlink_types.h
|
||||||
|
MAVLINK_TYPE_CHAR = 0
|
||||||
|
MAVLINK_TYPE_UINT8_T = 1
|
||||||
|
MAVLINK_TYPE_INT8_T = 2
|
||||||
|
MAVLINK_TYPE_UINT16_T = 3
|
||||||
|
MAVLINK_TYPE_INT16_T = 4
|
||||||
|
MAVLINK_TYPE_UINT32_T = 5
|
||||||
|
MAVLINK_TYPE_INT32_T = 6
|
||||||
|
MAVLINK_TYPE_UINT64_T = 7
|
||||||
|
MAVLINK_TYPE_INT64_T = 8
|
||||||
|
MAVLINK_TYPE_FLOAT = 9
|
||||||
|
MAVLINK_TYPE_DOUBLE = 10
|
||||||
|
|
||||||
|
|
||||||
class MAVLink_header(object):
|
class MAVLink_header(object):
|
||||||
'''MAVLink message header'''
|
'''MAVLink message header'''
|
||||||
def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
|
def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
|
||||||
|
@ -34,7 +49,9 @@ class MAVLink_message(object):
|
||||||
self._type = name
|
self._type = name
|
||||||
|
|
||||||
def get_msgbuf(self):
|
def get_msgbuf(self):
|
||||||
|
if isinstance(self._msgbuf, str):
|
||||||
return self._msgbuf
|
return self._msgbuf
|
||||||
|
return self._msgbuf.tostring()
|
||||||
|
|
||||||
def get_header(self):
|
def get_header(self):
|
||||||
return self._header
|
return self._header
|
||||||
|
@ -201,7 +218,8 @@ MAV_TYPE_HEXAROTOR = 13 # Hexarotor
|
||||||
MAV_TYPE_OCTOROTOR = 14 # Octorotor
|
MAV_TYPE_OCTOROTOR = 14 # Octorotor
|
||||||
MAV_TYPE_TRICOPTER = 15 # Octorotor
|
MAV_TYPE_TRICOPTER = 15 # Octorotor
|
||||||
MAV_TYPE_FLAPPING_WING = 16 # Flapping wing
|
MAV_TYPE_FLAPPING_WING = 16 # Flapping wing
|
||||||
MAV_TYPE_ENUM_END = 17 #
|
MAV_TYPE_KITE = 17 # Flapping wing
|
||||||
|
MAV_TYPE_ENUM_END = 18 #
|
||||||
|
|
||||||
# MAV_MODE_FLAG
|
# MAV_MODE_FLAG
|
||||||
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 # 0b00000001 Reserved for future use.
|
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 # 0b00000001 Reserved for future use.
|
||||||
|
@ -371,16 +389,6 @@ MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE = 8 # The Y or longitude value is out of rang
|
||||||
MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE = 9 # The Z or altitude value is out of range.
|
MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE = 9 # The Z or altitude value is out of range.
|
||||||
MAV_CMD_ACK_ENUM_END = 10 #
|
MAV_CMD_ACK_ENUM_END = 10 #
|
||||||
|
|
||||||
# MAV_VAR
|
|
||||||
MAV_VAR_FLOAT = 0 # 32 bit float
|
|
||||||
MAV_VAR_UINT8 = 1 # 8 bit unsigned integer
|
|
||||||
MAV_VAR_INT8 = 2 # 8 bit signed integer
|
|
||||||
MAV_VAR_UINT16 = 3 # 16 bit unsigned integer
|
|
||||||
MAV_VAR_INT16 = 4 # 16 bit signed integer
|
|
||||||
MAV_VAR_UINT32 = 5 # 32 bit unsigned integer
|
|
||||||
MAV_VAR_INT32 = 6 # 32 bit signed integer
|
|
||||||
MAV_VAR_ENUM_END = 7 #
|
|
||||||
|
|
||||||
# MAV_RESULT
|
# MAV_RESULT
|
||||||
MAV_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED
|
MAV_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED
|
||||||
MAV_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED
|
MAV_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED
|
||||||
|
@ -494,6 +502,7 @@ MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT = 59
|
||||||
MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT = 60
|
MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT = 60
|
||||||
MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST = 61
|
MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST = 61
|
||||||
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT = 62
|
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT = 62
|
||||||
|
MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST = 63
|
||||||
MAVLINK_MSG_ID_STATE_CORRECTION = 64
|
MAVLINK_MSG_ID_STATE_CORRECTION = 64
|
||||||
MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66
|
MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66
|
||||||
MAVLINK_MSG_ID_DATA_STREAM = 67
|
MAVLINK_MSG_ID_DATA_STREAM = 67
|
||||||
|
@ -1732,19 +1741,20 @@ class MAVLink_set_quad_motors_setpoint_message(MAVLink_message):
|
||||||
|
|
||||||
class MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message(MAVLink_message):
|
class MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message(MAVLink_message):
|
||||||
'''
|
'''
|
||||||
|
Setpoint for up to four quadrotors in a group / wing
|
||||||
'''
|
'''
|
||||||
def __init__(self, target_systems, roll, pitch, yaw, thrust):
|
def __init__(self, group, mode, roll, pitch, yaw, thrust):
|
||||||
MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, 'SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST')
|
MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, 'SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST')
|
||||||
self._fieldnames = ['target_systems', 'roll', 'pitch', 'yaw', 'thrust']
|
self._fieldnames = ['group', 'mode', 'roll', 'pitch', 'yaw', 'thrust']
|
||||||
self.target_systems = target_systems
|
self.group = group
|
||||||
|
self.mode = mode
|
||||||
self.roll = roll
|
self.roll = roll
|
||||||
self.pitch = pitch
|
self.pitch = pitch
|
||||||
self.yaw = yaw
|
self.yaw = yaw
|
||||||
self.thrust = thrust
|
self.thrust = thrust
|
||||||
|
|
||||||
def pack(self, mav):
|
def pack(self, mav):
|
||||||
return MAVLink_message.pack(self, mav, 200, struct.pack('<6h6h6h6H6s', self.roll, self.pitch, self.yaw, self.thrust, self.target_systems))
|
return MAVLink_message.pack(self, mav, 240, struct.pack('<4h4h4h4HBB', self.roll, self.pitch, self.yaw, self.thrust, self.group, self.mode))
|
||||||
|
|
||||||
class MAVLink_nav_controller_output_message(MAVLink_message):
|
class MAVLink_nav_controller_output_message(MAVLink_message):
|
||||||
'''
|
'''
|
||||||
|
@ -1768,6 +1778,26 @@ class MAVLink_nav_controller_output_message(MAVLink_message):
|
||||||
def pack(self, mav):
|
def pack(self, mav):
|
||||||
return MAVLink_message.pack(self, mav, 183, struct.pack('<fffffhhH', self.nav_roll, self.nav_pitch, self.alt_error, self.aspd_error, self.xtrack_error, self.nav_bearing, self.target_bearing, self.wp_dist))
|
return MAVLink_message.pack(self, mav, 183, struct.pack('<fffffhhH', self.nav_roll, self.nav_pitch, self.alt_error, self.aspd_error, self.xtrack_error, self.nav_bearing, self.target_bearing, self.wp_dist))
|
||||||
|
|
||||||
|
class MAVLink_set_quad_swarm_led_roll_pitch_yaw_thrust_message(MAVLink_message):
|
||||||
|
'''
|
||||||
|
Setpoint for up to four quadrotors in a group / wing
|
||||||
|
'''
|
||||||
|
def __init__(self, group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust):
|
||||||
|
MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, 'SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST')
|
||||||
|
self._fieldnames = ['group', 'mode', 'led_red', 'led_blue', 'led_green', 'roll', 'pitch', 'yaw', 'thrust']
|
||||||
|
self.group = group
|
||||||
|
self.mode = mode
|
||||||
|
self.led_red = led_red
|
||||||
|
self.led_blue = led_blue
|
||||||
|
self.led_green = led_green
|
||||||
|
self.roll = roll
|
||||||
|
self.pitch = pitch
|
||||||
|
self.yaw = yaw
|
||||||
|
self.thrust = thrust
|
||||||
|
|
||||||
|
def pack(self, mav):
|
||||||
|
return MAVLink_message.pack(self, mav, 130, struct.pack('<4h4h4h4HBB4s4s4s', self.roll, self.pitch, self.yaw, self.thrust, self.group, self.mode, self.led_red, self.led_blue, self.led_green))
|
||||||
|
|
||||||
class MAVLink_state_correction_message(MAVLink_message):
|
class MAVLink_state_correction_message(MAVLink_message):
|
||||||
'''
|
'''
|
||||||
Corrects the systems state by adding an error correction term
|
Corrects the systems state by adding an error correction term
|
||||||
|
@ -2275,8 +2305,9 @@ mavlink_map = {
|
||||||
MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT : ( '<Iffff', MAVLink_roll_pitch_yaw_thrust_setpoint_message, [0, 1, 2, 3, 4], 239 ),
|
MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT : ( '<Iffff', MAVLink_roll_pitch_yaw_thrust_setpoint_message, [0, 1, 2, 3, 4], 239 ),
|
||||||
MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT : ( '<Iffff', MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message, [0, 1, 2, 3, 4], 238 ),
|
MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT : ( '<Iffff', MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message, [0, 1, 2, 3, 4], 238 ),
|
||||||
MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT : ( '<HHHHB', MAVLink_set_quad_motors_setpoint_message, [4, 0, 1, 2, 3], 30 ),
|
MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT : ( '<HHHHB', MAVLink_set_quad_motors_setpoint_message, [4, 0, 1, 2, 3], 30 ),
|
||||||
MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST : ( '<6h6h6h6H6s', MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message, [4, 0, 1, 2, 3], 200 ),
|
MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST : ( '<4h4h4h4HBB', MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message, [4, 5, 0, 1, 2, 3], 240 ),
|
||||||
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT : ( '<fffffhhH', MAVLink_nav_controller_output_message, [0, 1, 5, 6, 7, 2, 3, 4], 183 ),
|
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT : ( '<fffffhhH', MAVLink_nav_controller_output_message, [0, 1, 5, 6, 7, 2, 3, 4], 183 ),
|
||||||
|
MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST : ( '<4h4h4h4HBB4s4s4s', MAVLink_set_quad_swarm_led_roll_pitch_yaw_thrust_message, [4, 5, 6, 7, 8, 0, 1, 2, 3], 130 ),
|
||||||
MAVLINK_MSG_ID_STATE_CORRECTION : ( '<fffffffff', MAVLink_state_correction_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 130 ),
|
MAVLINK_MSG_ID_STATE_CORRECTION : ( '<fffffffff', MAVLink_state_correction_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 130 ),
|
||||||
MAVLINK_MSG_ID_REQUEST_DATA_STREAM : ( '<HBBBB', MAVLink_request_data_stream_message, [1, 2, 3, 0, 4], 148 ),
|
MAVLINK_MSG_ID_REQUEST_DATA_STREAM : ( '<HBBBB', MAVLink_request_data_stream_message, [1, 2, 3, 0, 4], 148 ),
|
||||||
MAVLINK_MSG_ID_DATA_STREAM : ( '<HBB', MAVLink_data_stream_message, [1, 0, 2], 21 ),
|
MAVLINK_MSG_ID_DATA_STREAM : ( '<HBB', MAVLink_data_stream_message, [1, 0, 2], 21 ),
|
||||||
|
@ -3275,8 +3306,8 @@ class MAVLink(object):
|
||||||
|
|
||||||
target_system : System ID (uint8_t)
|
target_system : System ID (uint8_t)
|
||||||
target_component : Component ID (uint8_t)
|
target_component : Component ID (uint8_t)
|
||||||
param_id : Onboard parameter id (char)
|
param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char)
|
||||||
param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t)
|
param_index : Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) (int16_t)
|
||||||
|
|
||||||
'''
|
'''
|
||||||
msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index)
|
msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index)
|
||||||
|
@ -3297,8 +3328,8 @@ class MAVLink(object):
|
||||||
|
|
||||||
target_system : System ID (uint8_t)
|
target_system : System ID (uint8_t)
|
||||||
target_component : Component ID (uint8_t)
|
target_component : Component ID (uint8_t)
|
||||||
param_id : Onboard parameter id (char)
|
param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char)
|
||||||
param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t)
|
param_index : Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) (int16_t)
|
||||||
|
|
||||||
'''
|
'''
|
||||||
return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index))
|
return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index))
|
||||||
|
@ -3334,9 +3365,9 @@ class MAVLink(object):
|
||||||
keep track of received parameters and allows him to
|
keep track of received parameters and allows him to
|
||||||
re-request missing parameters after a loss or timeout.
|
re-request missing parameters after a loss or timeout.
|
||||||
|
|
||||||
param_id : Onboard parameter id (char)
|
param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char)
|
||||||
param_value : Onboard parameter value (float)
|
param_value : Onboard parameter value (float)
|
||||||
param_type : Onboard parameter type: see MAV_VAR enum (uint8_t)
|
param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t)
|
||||||
param_count : Total number of onboard parameters (uint16_t)
|
param_count : Total number of onboard parameters (uint16_t)
|
||||||
param_index : Index of this onboard parameter (uint16_t)
|
param_index : Index of this onboard parameter (uint16_t)
|
||||||
|
|
||||||
|
@ -3352,9 +3383,9 @@ class MAVLink(object):
|
||||||
keep track of received parameters and allows him to
|
keep track of received parameters and allows him to
|
||||||
re-request missing parameters after a loss or timeout.
|
re-request missing parameters after a loss or timeout.
|
||||||
|
|
||||||
param_id : Onboard parameter id (char)
|
param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char)
|
||||||
param_value : Onboard parameter value (float)
|
param_value : Onboard parameter value (float)
|
||||||
param_type : Onboard parameter type: see MAV_VAR enum (uint8_t)
|
param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t)
|
||||||
param_count : Total number of onboard parameters (uint16_t)
|
param_count : Total number of onboard parameters (uint16_t)
|
||||||
param_index : Index of this onboard parameter (uint16_t)
|
param_index : Index of this onboard parameter (uint16_t)
|
||||||
|
|
||||||
|
@ -3376,9 +3407,9 @@ class MAVLink(object):
|
||||||
|
|
||||||
target_system : System ID (uint8_t)
|
target_system : System ID (uint8_t)
|
||||||
target_component : Component ID (uint8_t)
|
target_component : Component ID (uint8_t)
|
||||||
param_id : Onboard parameter id (char)
|
param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char)
|
||||||
param_value : Onboard parameter value (float)
|
param_value : Onboard parameter value (float)
|
||||||
param_type : Onboard parameter type: see MAV_VAR enum (uint8_t)
|
param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t)
|
||||||
|
|
||||||
'''
|
'''
|
||||||
msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value, param_type)
|
msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value, param_type)
|
||||||
|
@ -3400,9 +3431,9 @@ class MAVLink(object):
|
||||||
|
|
||||||
target_system : System ID (uint8_t)
|
target_system : System ID (uint8_t)
|
||||||
target_component : Component ID (uint8_t)
|
target_component : Component ID (uint8_t)
|
||||||
param_id : Onboard parameter id (char)
|
param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char)
|
||||||
param_value : Onboard parameter value (float)
|
param_value : Onboard parameter value (float)
|
||||||
param_type : Onboard parameter type: see MAV_VAR enum (uint8_t)
|
param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t)
|
||||||
|
|
||||||
'''
|
'''
|
||||||
return self.send(self.param_set_encode(target_system, target_component, param_id, param_value, param_type))
|
return self.send(self.param_set_encode(target_system, target_component, param_id, param_value, param_type))
|
||||||
|
@ -4665,33 +4696,35 @@ class MAVLink(object):
|
||||||
'''
|
'''
|
||||||
return self.send(self.set_quad_motors_setpoint_encode(target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw))
|
return self.send(self.set_quad_motors_setpoint_encode(target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw))
|
||||||
|
|
||||||
def set_quad_swarm_roll_pitch_yaw_thrust_encode(self, target_systems, roll, pitch, yaw, thrust):
|
def set_quad_swarm_roll_pitch_yaw_thrust_encode(self, group, mode, roll, pitch, yaw, thrust):
|
||||||
'''
|
'''
|
||||||
|
Setpoint for up to four quadrotors in a group / wing
|
||||||
|
|
||||||
|
group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t)
|
||||||
target_systems : System IDs for 6 quadrotors: 0..5, the ID's are the MAVLink IDs (uint8_t)
|
mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t)
|
||||||
roll : Desired roll angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t)
|
roll : Desired roll angle in radians +-PI (+-32767) (int16_t)
|
||||||
pitch : Desired pitch angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t)
|
pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t)
|
||||||
yaw : Desired yaw angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t)
|
yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t)
|
||||||
thrust : Collective thrust, scaled to uint16 for 6 quadrotors: 0..5 (uint16_t)
|
thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t)
|
||||||
|
|
||||||
'''
|
'''
|
||||||
msg = MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message(target_systems, roll, pitch, yaw, thrust)
|
msg = MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message(group, mode, roll, pitch, yaw, thrust)
|
||||||
msg.pack(self)
|
msg.pack(self)
|
||||||
return msg
|
return msg
|
||||||
|
|
||||||
def set_quad_swarm_roll_pitch_yaw_thrust_send(self, target_systems, roll, pitch, yaw, thrust):
|
def set_quad_swarm_roll_pitch_yaw_thrust_send(self, group, mode, roll, pitch, yaw, thrust):
|
||||||
'''
|
'''
|
||||||
|
Setpoint for up to four quadrotors in a group / wing
|
||||||
|
|
||||||
|
group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t)
|
||||||
target_systems : System IDs for 6 quadrotors: 0..5, the ID's are the MAVLink IDs (uint8_t)
|
mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t)
|
||||||
roll : Desired roll angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t)
|
roll : Desired roll angle in radians +-PI (+-32767) (int16_t)
|
||||||
pitch : Desired pitch angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t)
|
pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t)
|
||||||
yaw : Desired yaw angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t)
|
yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t)
|
||||||
thrust : Collective thrust, scaled to uint16 for 6 quadrotors: 0..5 (uint16_t)
|
thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t)
|
||||||
|
|
||||||
'''
|
'''
|
||||||
return self.send(self.set_quad_swarm_roll_pitch_yaw_thrust_encode(target_systems, roll, pitch, yaw, thrust))
|
return self.send(self.set_quad_swarm_roll_pitch_yaw_thrust_encode(group, mode, roll, pitch, yaw, thrust))
|
||||||
|
|
||||||
def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
|
def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error):
|
||||||
'''
|
'''
|
||||||
|
@ -4733,6 +4766,42 @@ class MAVLink(object):
|
||||||
'''
|
'''
|
||||||
return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error))
|
return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error))
|
||||||
|
|
||||||
|
def set_quad_swarm_led_roll_pitch_yaw_thrust_encode(self, group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust):
|
||||||
|
'''
|
||||||
|
Setpoint for up to four quadrotors in a group / wing
|
||||||
|
|
||||||
|
group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t)
|
||||||
|
mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t)
|
||||||
|
led_red : RGB red channel (0-255) (uint8_t)
|
||||||
|
led_blue : RGB green channel (0-255) (uint8_t)
|
||||||
|
led_green : RGB blue channel (0-255) (uint8_t)
|
||||||
|
roll : Desired roll angle in radians +-PI (+-32767) (int16_t)
|
||||||
|
pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t)
|
||||||
|
yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t)
|
||||||
|
thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t)
|
||||||
|
|
||||||
|
'''
|
||||||
|
msg = MAVLink_set_quad_swarm_led_roll_pitch_yaw_thrust_message(group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust)
|
||||||
|
msg.pack(self)
|
||||||
|
return msg
|
||||||
|
|
||||||
|
def set_quad_swarm_led_roll_pitch_yaw_thrust_send(self, group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust):
|
||||||
|
'''
|
||||||
|
Setpoint for up to four quadrotors in a group / wing
|
||||||
|
|
||||||
|
group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t)
|
||||||
|
mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t)
|
||||||
|
led_red : RGB red channel (0-255) (uint8_t)
|
||||||
|
led_blue : RGB green channel (0-255) (uint8_t)
|
||||||
|
led_green : RGB blue channel (0-255) (uint8_t)
|
||||||
|
roll : Desired roll angle in radians +-PI (+-32767) (int16_t)
|
||||||
|
pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t)
|
||||||
|
yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t)
|
||||||
|
thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t)
|
||||||
|
|
||||||
|
'''
|
||||||
|
return self.send(self.set_quad_swarm_led_roll_pitch_yaw_thrust_encode(group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust))
|
||||||
|
|
||||||
def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
|
def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr):
|
||||||
'''
|
'''
|
||||||
Corrects the systems state by adding an error correction term to the
|
Corrects the systems state by adding an error correction term to the
|
||||||
|
@ -5530,3 +5599,4 @@ class MAVLink(object):
|
||||||
|
|
||||||
'''
|
'''
|
||||||
return self.send(self.debug_encode(time_boot_ms, ind, value))
|
return self.send(self.debug_encode(time_boot_ms, ind, value))
|
||||||
|
|
||||||
|
|
|
@ -6,7 +6,7 @@ Copyright Andrew Tridgell 2011
|
||||||
Released under GNU GPL version 3 or later
|
Released under GNU GPL version 3 or later
|
||||||
'''
|
'''
|
||||||
|
|
||||||
import socket, math, struct, time, os, fnmatch, array, sys, errno
|
import socket, math, struct, time, os, fnmatch, array, sys, errno, fcntl
|
||||||
from math import *
|
from math import *
|
||||||
from mavextra import *
|
from mavextra import *
|
||||||
|
|
||||||
|
@ -47,8 +47,9 @@ class location(object):
|
||||||
|
|
||||||
class mavfile(object):
|
class mavfile(object):
|
||||||
'''a generic mavlink port'''
|
'''a generic mavlink port'''
|
||||||
def __init__(self, fd, address, source_system=255, notimestamps=False):
|
def __init__(self, fd, address, source_system=255, notimestamps=False, input=True):
|
||||||
global mavfile_global
|
global mavfile_global
|
||||||
|
if input:
|
||||||
mavfile_global = self
|
mavfile_global = self
|
||||||
self.fd = fd
|
self.fd = fd
|
||||||
self.address = address
|
self.address = address
|
||||||
|
@ -59,11 +60,13 @@ class mavfile(object):
|
||||||
else:
|
else:
|
||||||
self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0)
|
self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0)
|
||||||
self.params = {}
|
self.params = {}
|
||||||
self.mav = None
|
|
||||||
self.target_system = 0
|
self.target_system = 0
|
||||||
self.target_component = 0
|
self.target_component = 0
|
||||||
self.mav = mavlink.MAVLink(self, srcSystem=source_system)
|
self.source_system = source_system
|
||||||
self.mav.robust_parsing = True
|
self.first_byte = True
|
||||||
|
self.robust_parsing = True
|
||||||
|
self.mav = mavlink.MAVLink(self, srcSystem=self.source_system)
|
||||||
|
self.mav.robust_parsing = self.robust_parsing
|
||||||
self.logfile = None
|
self.logfile = None
|
||||||
self.logfile_raw = None
|
self.logfile_raw = None
|
||||||
self.param_fetch_in_progress = False
|
self.param_fetch_in_progress = False
|
||||||
|
@ -80,6 +83,36 @@ class mavfile(object):
|
||||||
self.ground_temperature = None
|
self.ground_temperature = None
|
||||||
self.altitude = 0
|
self.altitude = 0
|
||||||
self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
|
self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
|
||||||
|
self.last_seq = -1
|
||||||
|
self.mav_loss = 0
|
||||||
|
self.mav_count = 0
|
||||||
|
self.stop_on_EOF = False
|
||||||
|
|
||||||
|
def auto_mavlink_version(self, buf):
|
||||||
|
'''auto-switch mavlink protocol version'''
|
||||||
|
global mavlink
|
||||||
|
if len(buf) == 0:
|
||||||
|
return
|
||||||
|
if not ord(buf[0]) in [ 85, 254 ]:
|
||||||
|
return
|
||||||
|
self.first_byte = False
|
||||||
|
if self.WIRE_PROTOCOL_VERSION == "0.9" and ord(buf[0]) == 254:
|
||||||
|
import mavlinkv10 as mavlink
|
||||||
|
os.environ['MAVLINK10'] = '1'
|
||||||
|
elif self.WIRE_PROTOCOL_VERSION == "1.0" and ord(buf[0]) == 85:
|
||||||
|
import mavlink as mavlink
|
||||||
|
else:
|
||||||
|
return
|
||||||
|
# switch protocol
|
||||||
|
(callback, callback_args, callback_kwargs) = (self.mav.callback,
|
||||||
|
self.mav.callback_args,
|
||||||
|
self.mav.callback_kwargs)
|
||||||
|
self.mav = mavlink.MAVLink(self, srcSystem=self.source_system)
|
||||||
|
self.mav.robust_parsing = self.robust_parsing
|
||||||
|
self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
|
||||||
|
(self.mav.callback, self.mav.callback_args, self.mav.callback_kwargs) = (callback,
|
||||||
|
callback_args,
|
||||||
|
callback_kwargs)
|
||||||
|
|
||||||
def recv(self, n=None):
|
def recv(self, n=None):
|
||||||
'''default recv method'''
|
'''default recv method'''
|
||||||
|
@ -99,6 +132,9 @@ class mavfile(object):
|
||||||
|
|
||||||
def post_message(self, msg):
|
def post_message(self, msg):
|
||||||
'''default post message call'''
|
'''default post message call'''
|
||||||
|
if '_posted' in msg.__dict__:
|
||||||
|
return
|
||||||
|
msg._posted = True
|
||||||
msg._timestamp = time.time()
|
msg._timestamp = time.time()
|
||||||
type = msg.get_type()
|
type = msg.get_type()
|
||||||
self.messages[type] = msg
|
self.messages[type] = msg
|
||||||
|
@ -112,6 +148,19 @@ class mavfile(object):
|
||||||
else:
|
else:
|
||||||
msg._timestamp = self._timestamp
|
msg._timestamp = self._timestamp
|
||||||
|
|
||||||
|
if not (
|
||||||
|
# its the radio or planner
|
||||||
|
(msg.get_srcSystem() == ord('3') and msg.get_srcComponent() == ord('D')) or
|
||||||
|
msg.get_srcSystem() == 255 or
|
||||||
|
msg.get_type() == 'BAD_DATA'):
|
||||||
|
seq = (self.last_seq+1) % 256
|
||||||
|
seq2 = msg.get_seq()
|
||||||
|
if seq != seq2 and self.last_seq != -1:
|
||||||
|
diff = (seq2 - seq) % 256
|
||||||
|
self.mav_loss += diff
|
||||||
|
self.last_seq = seq2
|
||||||
|
self.mav_count += 1
|
||||||
|
|
||||||
self.timestamp = msg._timestamp
|
self.timestamp = msg._timestamp
|
||||||
if type == 'HEARTBEAT':
|
if type == 'HEARTBEAT':
|
||||||
self.target_system = msg.get_srcSystem()
|
self.target_system = msg.get_srcSystem()
|
||||||
|
@ -119,33 +168,42 @@ class mavfile(object):
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
||||||
self.flightmode = mode_string_v10(msg)
|
self.flightmode = mode_string_v10(msg)
|
||||||
elif type == 'PARAM_VALUE':
|
elif type == 'PARAM_VALUE':
|
||||||
|
s = str(msg.param_id)
|
||||||
self.params[str(msg.param_id)] = msg.param_value
|
self.params[str(msg.param_id)] = msg.param_value
|
||||||
if msg.param_index+1 == msg.param_count:
|
if msg.param_index+1 == msg.param_count:
|
||||||
self.param_fetch_in_progress = False
|
self.param_fetch_in_progress = False
|
||||||
self.param_fetch_complete = True
|
self.param_fetch_complete = True
|
||||||
if str(msg.param_id) == 'GND_ABS_PRESS':
|
|
||||||
self.ground_pressure = msg.param_value
|
|
||||||
if str(msg.param_id) == 'GND_TEMP':
|
|
||||||
self.ground_temperature = msg.param_value
|
|
||||||
elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
|
elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
|
||||||
self.flightmode = mode_string_v09(msg)
|
self.flightmode = mode_string_v09(msg)
|
||||||
elif type == 'GPS_RAW':
|
elif type == 'GPS_RAW':
|
||||||
if self.messages['HOME'].fix_type < 2:
|
if self.messages['HOME'].fix_type < 2:
|
||||||
self.messages['HOME'] = msg
|
self.messages['HOME'] = msg
|
||||||
|
elif type == 'GPS_RAW_INT':
|
||||||
|
if self.messages['HOME'].fix_type < 3:
|
||||||
|
self.messages['HOME'] = msg
|
||||||
for hook in self.message_hooks:
|
for hook in self.message_hooks:
|
||||||
hook(self, msg)
|
hook(self, msg)
|
||||||
|
|
||||||
|
|
||||||
|
def packet_loss(self):
|
||||||
|
'''packet loss as a percentage'''
|
||||||
|
if self.mav_count == 0:
|
||||||
|
return 0
|
||||||
|
return (100.0*self.mav_loss)/(self.mav_count+self.mav_loss)
|
||||||
|
|
||||||
|
|
||||||
def recv_msg(self):
|
def recv_msg(self):
|
||||||
'''message receive routine'''
|
'''message receive routine'''
|
||||||
self.pre_message()
|
self.pre_message()
|
||||||
while True:
|
while True:
|
||||||
n = self.mav.bytes_needed()
|
n = self.mav.bytes_needed()
|
||||||
s = self.recv(n)
|
s = self.recv(n)
|
||||||
if len(s) == 0 and len(self.mav.buf) == 0:
|
if len(s) == 0 and (len(self.mav.buf) == 0 or self.stop_on_EOF):
|
||||||
return None
|
return None
|
||||||
if self.logfile_raw:
|
if self.logfile_raw:
|
||||||
self.logfile_raw.write(str(s))
|
self.logfile_raw.write(str(s))
|
||||||
|
if self.first_byte:
|
||||||
|
self.auto_mavlink_version(s)
|
||||||
msg = self.mav.parse_char(s)
|
msg = self.mav.parse_char(s)
|
||||||
if msg:
|
if msg:
|
||||||
self.post_message(msg)
|
self.post_message(msg)
|
||||||
|
@ -168,6 +226,10 @@ class mavfile(object):
|
||||||
continue
|
continue
|
||||||
return m
|
return m
|
||||||
|
|
||||||
|
def mavlink10(self):
|
||||||
|
'''return True if using MAVLink 1.0'''
|
||||||
|
return self.WIRE_PROTOCOL_VERSION == "1.0"
|
||||||
|
|
||||||
def setup_logfile(self, logfile, mode='w'):
|
def setup_logfile(self, logfile, mode='w'):
|
||||||
'''start logging to the given logfile, with timestamps'''
|
'''start logging to the given logfile, with timestamps'''
|
||||||
self.logfile = open(logfile, mode=mode)
|
self.logfile = open(logfile, mode=mode)
|
||||||
|
@ -197,9 +259,9 @@ class mavfile(object):
|
||||||
|
|
||||||
def param_set_send(self, parm_name, parm_value, parm_type=None):
|
def param_set_send(self, parm_name, parm_value, parm_type=None):
|
||||||
'''wrapper for parameter set'''
|
'''wrapper for parameter set'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
if parm_type == None:
|
if parm_type == None:
|
||||||
parm_type = mavlink.MAV_VAR_FLOAT
|
parm_type = mavlink.MAVLINK_TYPE_FLOAT
|
||||||
self.mav.param_set_send(self.target_system, self.target_component,
|
self.mav.param_set_send(self.target_system, self.target_component,
|
||||||
parm_name, parm_value, parm_type)
|
parm_name, parm_value, parm_type)
|
||||||
else:
|
else:
|
||||||
|
@ -208,35 +270,35 @@ class mavfile(object):
|
||||||
|
|
||||||
def waypoint_request_list_send(self):
|
def waypoint_request_list_send(self):
|
||||||
'''wrapper for waypoint_request_list_send'''
|
'''wrapper for waypoint_request_list_send'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.mission_request_list_send(self.target_system, self.target_component)
|
self.mav.mission_request_list_send(self.target_system, self.target_component)
|
||||||
else:
|
else:
|
||||||
self.mav.waypoint_request_list_send(self.target_system, self.target_component)
|
self.mav.waypoint_request_list_send(self.target_system, self.target_component)
|
||||||
|
|
||||||
def waypoint_clear_all_send(self):
|
def waypoint_clear_all_send(self):
|
||||||
'''wrapper for waypoint_clear_all_send'''
|
'''wrapper for waypoint_clear_all_send'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.mission_clear_all_send(self.target_system, self.target_component)
|
self.mav.mission_clear_all_send(self.target_system, self.target_component)
|
||||||
else:
|
else:
|
||||||
self.mav.waypoint_clear_all_send(self.target_system, self.target_component)
|
self.mav.waypoint_clear_all_send(self.target_system, self.target_component)
|
||||||
|
|
||||||
def waypoint_request_send(self, seq):
|
def waypoint_request_send(self, seq):
|
||||||
'''wrapper for waypoint_request_send'''
|
'''wrapper for waypoint_request_send'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.mission_request_send(self.target_system, self.target_component, seq)
|
self.mav.mission_request_send(self.target_system, self.target_component, seq)
|
||||||
else:
|
else:
|
||||||
self.mav.waypoint_request_send(self.target_system, self.target_component, seq)
|
self.mav.waypoint_request_send(self.target_system, self.target_component, seq)
|
||||||
|
|
||||||
def waypoint_set_current_send(self, seq):
|
def waypoint_set_current_send(self, seq):
|
||||||
'''wrapper for waypoint_set_current_send'''
|
'''wrapper for waypoint_set_current_send'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.mission_set_current_send(self.target_system, self.target_component, seq)
|
self.mav.mission_set_current_send(self.target_system, self.target_component, seq)
|
||||||
else:
|
else:
|
||||||
self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
|
self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
|
||||||
|
|
||||||
def waypoint_current(self):
|
def waypoint_current(self):
|
||||||
'''return current waypoint'''
|
'''return current waypoint'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
m = self.recv_match(type='MISSION_CURRENT', blocking=True)
|
m = self.recv_match(type='MISSION_CURRENT', blocking=True)
|
||||||
else:
|
else:
|
||||||
m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True)
|
m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True)
|
||||||
|
@ -244,14 +306,14 @@ class mavfile(object):
|
||||||
|
|
||||||
def waypoint_count_send(self, seq):
|
def waypoint_count_send(self, seq):
|
||||||
'''wrapper for waypoint_count_send'''
|
'''wrapper for waypoint_count_send'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.mission_count_send(self.target_system, self.target_component, seq)
|
self.mav.mission_count_send(self.target_system, self.target_component, seq)
|
||||||
else:
|
else:
|
||||||
self.mav.waypoint_count_send(self.target_system, self.target_component, seq)
|
self.mav.waypoint_count_send(self.target_system, self.target_component, seq)
|
||||||
|
|
||||||
def set_mode_auto(self):
|
def set_mode_auto(self):
|
||||||
'''enter auto mode'''
|
'''enter auto mode'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.command_long_send(self.target_system, self.target_component,
|
self.mav.command_long_send(self.target_system, self.target_component,
|
||||||
mavlink.MAV_CMD_MISSION_START, 0, 0, 0, 0, 0, 0, 0, 0)
|
mavlink.MAV_CMD_MISSION_START, 0, 0, 0, 0, 0, 0, 0, 0)
|
||||||
else:
|
else:
|
||||||
|
@ -260,7 +322,7 @@ class mavfile(object):
|
||||||
|
|
||||||
def set_mode_rtl(self):
|
def set_mode_rtl(self):
|
||||||
'''enter RTL mode'''
|
'''enter RTL mode'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.command_long_send(self.target_system, self.target_component,
|
self.mav.command_long_send(self.target_system, self.target_component,
|
||||||
mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0)
|
mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0)
|
||||||
else:
|
else:
|
||||||
|
@ -269,7 +331,7 @@ class mavfile(object):
|
||||||
|
|
||||||
def set_mode_loiter(self):
|
def set_mode_loiter(self):
|
||||||
'''enter LOITER mode'''
|
'''enter LOITER mode'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.command_long_send(self.target_system, self.target_component,
|
self.mav.command_long_send(self.target_system, self.target_component,
|
||||||
mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0, 0, 0, 0)
|
mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0, 0, 0, 0)
|
||||||
else:
|
else:
|
||||||
|
@ -278,7 +340,7 @@ class mavfile(object):
|
||||||
|
|
||||||
def calibrate_imu(self):
|
def calibrate_imu(self):
|
||||||
'''calibrate IMU'''
|
'''calibrate IMU'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.command_long_send(self.target_system, self.target_component,
|
self.mav.command_long_send(self.target_system, self.target_component,
|
||||||
mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
|
mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
|
||||||
1, 1, 1, 1, 0, 0, 0)
|
1, 1, 1, 1, 0, 0, 0)
|
||||||
|
@ -288,7 +350,7 @@ class mavfile(object):
|
||||||
|
|
||||||
def calibrate_level(self):
|
def calibrate_level(self):
|
||||||
'''calibrate accels'''
|
'''calibrate accels'''
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.mav.command_long_send(self.target_system, self.target_component,
|
self.mav.command_long_send(self.target_system, self.target_component,
|
||||||
mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
|
mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
|
||||||
1, 1, 1, 1, 0, 0, 0)
|
1, 1, 1, 1, 0, 0, 0)
|
||||||
|
@ -298,7 +360,7 @@ class mavfile(object):
|
||||||
|
|
||||||
def wait_gps_fix(self):
|
def wait_gps_fix(self):
|
||||||
self.recv_match(type='VFR_HUD', blocking=True)
|
self.recv_match(type='VFR_HUD', blocking=True)
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
if self.mavlink10():
|
||||||
self.recv_match(type='GPS_RAW_INT', blocking=True,
|
self.recv_match(type='GPS_RAW_INT', blocking=True,
|
||||||
condition='GPS_RAW_INT.fix_type==3 and GPS_RAW_INT.lat != 0 and GPS_RAW_INT.alt != 0')
|
condition='GPS_RAW_INT.fix_type==3 and GPS_RAW_INT.lat != 0 and GPS_RAW_INT.alt != 0')
|
||||||
else:
|
else:
|
||||||
|
@ -308,7 +370,9 @@ class mavfile(object):
|
||||||
def location(self):
|
def location(self):
|
||||||
'''return current location'''
|
'''return current location'''
|
||||||
self.wait_gps_fix()
|
self.wait_gps_fix()
|
||||||
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
|
# wait for another VFR_HUD, to ensure we have correct altitude
|
||||||
|
self.recv_match(type='VFR_HUD', blocking=True)
|
||||||
|
if self.mavlink10():
|
||||||
return location(self.messages['GPS_RAW_INT'].lat*1.0e-7,
|
return location(self.messages['GPS_RAW_INT'].lat*1.0e-7,
|
||||||
self.messages['GPS_RAW_INT'].lon*1.0e-7,
|
self.messages['GPS_RAW_INT'].lon*1.0e-7,
|
||||||
self.messages['VFR_HUD'].alt,
|
self.messages['VFR_HUD'].alt,
|
||||||
|
@ -319,6 +383,19 @@ class mavfile(object):
|
||||||
self.messages['VFR_HUD'].alt,
|
self.messages['VFR_HUD'].alt,
|
||||||
self.messages['VFR_HUD'].heading)
|
self.messages['VFR_HUD'].heading)
|
||||||
|
|
||||||
|
def field(self, type, field, default=None):
|
||||||
|
'''convenient function for returning an arbitrary MAVLink
|
||||||
|
field with a default'''
|
||||||
|
if not type in self.messages:
|
||||||
|
return default
|
||||||
|
return getattr(self.messages[type], field, default)
|
||||||
|
|
||||||
|
def param(self, name, default=None):
|
||||||
|
'''convenient function for returning an arbitrary MAVLink
|
||||||
|
parameter with a default'''
|
||||||
|
if not name in self.params:
|
||||||
|
return default
|
||||||
|
return self.params[name]
|
||||||
|
|
||||||
class mavserial(mavfile):
|
class mavserial(mavfile):
|
||||||
'''a serial mavlink port'''
|
'''a serial mavlink port'''
|
||||||
|
@ -329,9 +406,11 @@ class mavserial(mavfile):
|
||||||
self.autoreconnect = autoreconnect
|
self.autoreconnect = autoreconnect
|
||||||
self.port = serial.Serial(self.device, self.baud, timeout=0,
|
self.port = serial.Serial(self.device, self.baud, timeout=0,
|
||||||
dsrdtr=False, rtscts=False, xonxoff=False)
|
dsrdtr=False, rtscts=False, xonxoff=False)
|
||||||
|
|
||||||
try:
|
try:
|
||||||
fd = self.port.fileno()
|
fd = self.port.fileno()
|
||||||
|
flags = fcntl.fcntl(fd, fcntl.F_GETFD)
|
||||||
|
flags |= fcntl.FD_CLOEXEC
|
||||||
|
fcntl.fcntl(fd, fcntl.F_SETFD, flags)
|
||||||
except Exception:
|
except Exception:
|
||||||
fd = None
|
fd = None
|
||||||
mavfile.__init__(self, fd, device, source_system=source_system)
|
mavfile.__init__(self, fd, device, source_system=source_system)
|
||||||
|
@ -351,7 +430,7 @@ class mavserial(mavfile):
|
||||||
def write(self, buf):
|
def write(self, buf):
|
||||||
try:
|
try:
|
||||||
return self.port.write(buf)
|
return self.port.write(buf)
|
||||||
except OSError:
|
except Exception:
|
||||||
if self.autoreconnect:
|
if self.autoreconnect:
|
||||||
self.reset()
|
self.reset()
|
||||||
return -1
|
return -1
|
||||||
|
@ -370,7 +449,7 @@ class mavserial(mavfile):
|
||||||
return
|
return
|
||||||
except Exception:
|
except Exception:
|
||||||
print("Failed to reopen %s" % self.device)
|
print("Failed to reopen %s" % self.device)
|
||||||
time.sleep(1)
|
time.sleep(0.5)
|
||||||
|
|
||||||
|
|
||||||
class mavudp(mavfile):
|
class mavudp(mavfile):
|
||||||
|
@ -383,12 +462,16 @@ class mavudp(mavfile):
|
||||||
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
self.udp_server = input
|
self.udp_server = input
|
||||||
if input:
|
if input:
|
||||||
|
self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||||
self.port.bind((a[0], int(a[1])))
|
self.port.bind((a[0], int(a[1])))
|
||||||
else:
|
else:
|
||||||
self.destination_addr = (a[0], int(a[1]))
|
self.destination_addr = (a[0], int(a[1]))
|
||||||
|
flags = fcntl.fcntl(self.port.fileno(), fcntl.F_GETFD)
|
||||||
|
flags |= fcntl.FD_CLOEXEC
|
||||||
|
fcntl.fcntl(self.port.fileno(), fcntl.F_SETFD, flags)
|
||||||
self.port.setblocking(0)
|
self.port.setblocking(0)
|
||||||
self.last_address = None
|
self.last_address = None
|
||||||
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
|
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system, input=input)
|
||||||
|
|
||||||
def close(self):
|
def close(self):
|
||||||
self.port.close()
|
self.port.close()
|
||||||
|
@ -397,7 +480,7 @@ class mavudp(mavfile):
|
||||||
try:
|
try:
|
||||||
data, self.last_address = self.port.recvfrom(300)
|
data, self.last_address = self.port.recvfrom(300)
|
||||||
except socket.error as e:
|
except socket.error as e:
|
||||||
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
|
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]:
|
||||||
return ""
|
return ""
|
||||||
raise
|
raise
|
||||||
return data
|
return data
|
||||||
|
@ -418,6 +501,8 @@ class mavudp(mavfile):
|
||||||
s = self.recv()
|
s = self.recv()
|
||||||
if len(s) == 0:
|
if len(s) == 0:
|
||||||
return None
|
return None
|
||||||
|
if self.first_byte:
|
||||||
|
self.auto_mavlink_version(s)
|
||||||
msg = self.mav.parse_buffer(s)
|
msg = self.mav.parse_buffer(s)
|
||||||
if msg is not None:
|
if msg is not None:
|
||||||
for m in msg:
|
for m in msg:
|
||||||
|
@ -437,6 +522,9 @@ class mavtcp(mavfile):
|
||||||
self.destination_addr = (a[0], int(a[1]))
|
self.destination_addr = (a[0], int(a[1]))
|
||||||
self.port.connect(self.destination_addr)
|
self.port.connect(self.destination_addr)
|
||||||
self.port.setblocking(0)
|
self.port.setblocking(0)
|
||||||
|
flags = fcntl.fcntl(self.port.fileno(), fcntl.F_GETFD)
|
||||||
|
flags |= fcntl.FD_CLOEXEC
|
||||||
|
fcntl.fcntl(self.port.fileno(), fcntl.F_SETFD, flags)
|
||||||
self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
|
self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
|
||||||
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
|
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
|
||||||
|
|
||||||
|
@ -485,6 +573,7 @@ class mavlogfile(mavfile):
|
||||||
self._timestamp = 0
|
self._timestamp = 0
|
||||||
else:
|
else:
|
||||||
self._timestamp = time.time()
|
self._timestamp = time.time()
|
||||||
|
self.stop_on_EOF = True
|
||||||
|
|
||||||
def close(self):
|
def close(self):
|
||||||
self.f.close()
|
self.f.close()
|
||||||
|
@ -500,6 +589,7 @@ class mavlogfile(mavfile):
|
||||||
def pre_message(self):
|
def pre_message(self):
|
||||||
'''read timestamp if needed'''
|
'''read timestamp if needed'''
|
||||||
# read the timestamp
|
# read the timestamp
|
||||||
|
if self.filesize != 0:
|
||||||
self.percent = (100.0 * self.f.tell()) / self.filesize
|
self.percent = (100.0 * self.f.tell()) / self.filesize
|
||||||
if self.notimestamps:
|
if self.notimestamps:
|
||||||
return
|
return
|
||||||
|
@ -510,12 +600,14 @@ class mavlogfile(mavfile):
|
||||||
hnsec = self._two64 + float(tbuf[0:20])
|
hnsec = self._two64 + float(tbuf[0:20])
|
||||||
t = hnsec * 1.0e-7 # convert to seconds
|
t = hnsec * 1.0e-7 # convert to seconds
|
||||||
t -= 719163 * 24 * 60 * 60 # convert to 1970 base
|
t -= 719163 * 24 * 60 * 60 # convert to 1970 base
|
||||||
|
self._link = 0
|
||||||
else:
|
else:
|
||||||
tbuf = self.f.read(8)
|
tbuf = self.f.read(8)
|
||||||
if len(tbuf) != 8:
|
if len(tbuf) != 8:
|
||||||
return
|
return
|
||||||
(tusec,) = struct.unpack('>Q', tbuf)
|
(tusec,) = struct.unpack('>Q', tbuf)
|
||||||
t = tusec * 1.0e-6
|
t = tusec * 1.0e-6
|
||||||
|
self._link = tusec & 0x3
|
||||||
self._timestamp = t
|
self._timestamp = t
|
||||||
|
|
||||||
def post_message(self, msg):
|
def post_message(self, msg):
|
||||||
|
|
|
@ -36,6 +36,15 @@ class MAVWPLoader(object):
|
||||||
w.seq = self.count()
|
w.seq = self.count()
|
||||||
self.wpoints.append(w)
|
self.wpoints.append(w)
|
||||||
|
|
||||||
|
def set(self, w, idx):
|
||||||
|
'''set a waypoint'''
|
||||||
|
w.seq = idx
|
||||||
|
if w.seq == self.count():
|
||||||
|
return self.add(w)
|
||||||
|
if self.count() <= idx:
|
||||||
|
raise MAVWPError('adding waypoint at idx=%u past end of list (count=%u)' % (idx, self.count()))
|
||||||
|
self.wpoints[idx] = w
|
||||||
|
|
||||||
def remove(self, w):
|
def remove(self, w):
|
||||||
'''remove a waypoint'''
|
'''remove a waypoint'''
|
||||||
self.wpoints.remove(w)
|
self.wpoints.remove(w)
|
||||||
|
|
Loading…
Reference in New Issue