From 830e5997d2c507bd12ff3c118854227c9f05cb6b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Jun 2012 17:21:13 +1000 Subject: [PATCH] mavlink: added auto-detection of mavlink protocol version --- Tools/autotest/autotest.py | 7 - Tools/autotest/pymavlink/mavextra.py | 6 +- Tools/autotest/pymavlink/mavlink.py | 35 +- Tools/autotest/pymavlink/mavlinkv10.py | 662 ++++++++++++++----------- Tools/autotest/pymavlink/mavutil.py | 158 ++++-- Tools/autotest/pymavlink/mavwp.py | 9 + 6 files changed, 529 insertions(+), 348 deletions(-) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 2da7d3fce1..80cec3410d 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -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("--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("--mav09", action='store_true', default=False, help="Use MAVLink protocol 0.9") 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 steps = [ diff --git a/Tools/autotest/pymavlink/mavextra.py b/Tools/autotest/pymavlink/mavextra.py index 4a8eec08b8..b678f1a0fd 100644 --- a/Tools/autotest/pymavlink/mavextra.py +++ b/Tools/autotest/pymavlink/mavextra.py @@ -17,10 +17,10 @@ def altitude(SCALED_PRESSURE): '''calculate barometric altitude''' import mavutil 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 - scaling = self.ground_pressure / (SCALED_PRESSURE.press_abs*100.0) - temp = self.ground_temperature + 273.15 + scaling = self.param('GND_ABS_PRESS', 1) / (SCALED_PRESSURE.press_abs*100.0) + temp = self.param('GND_TEMP', 0) + 273.15 return log(scaling) * temp * 29271.267 * 0.001 diff --git a/Tools/autotest/pymavlink/mavlink.py b/Tools/autotest/pymavlink/mavlink.py index 693afa2a06..f8bde1ce6a 100644 --- a/Tools/autotest/pymavlink/mavlink.py +++ b/Tools/autotest/pymavlink/mavlink.py @@ -10,6 +10,21 @@ import struct, array, mavutil, time 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): '''MAVLink message header''' def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0): @@ -34,7 +49,9 @@ class MAVLink_message(object): self._type = name def get_msgbuf(self): - return self._msgbuf + if isinstance(self._msgbuf, str): + return self._msgbuf + return self._msgbuf.tostring() def get_header(self): return self._header @@ -2510,7 +2527,7 @@ class MAVLink(object): msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw) msg.pack(self) return msg - + def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): ''' Status of DCM attitude estimator @@ -2525,7 +2542,7 @@ class MAVLink(object): ''' return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)) - + def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): ''' Status of simulation environment, if used @@ -2544,7 +2561,7 @@ class MAVLink(object): msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro) msg.pack(self) return msg - + def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): ''' Status of simulation environment, if used @@ -2561,7 +2578,7 @@ class MAVLink(object): ''' return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)) - + def hwstatus_encode(self, Vcc, I2Cerr): ''' Status of key hardware @@ -2573,7 +2590,7 @@ class MAVLink(object): msg = MAVLink_hwstatus_message(Vcc, I2Cerr) msg.pack(self) return msg - + def hwstatus_send(self, Vcc, I2Cerr): ''' Status of key hardware @@ -2583,7 +2600,7 @@ class MAVLink(object): ''' return self.send(self.hwstatus_encode(Vcc, I2Cerr)) - + def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): ''' Status generated by radio @@ -2600,7 +2617,7 @@ class MAVLink(object): msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed) msg.pack(self) return msg - + def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): ''' Status generated by radio @@ -2615,7 +2632,7 @@ class MAVLink(object): ''' return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)) - + def heartbeat_encode(self, type, autopilot, mavlink_version=2): ''' The heartbeat message shows that a system is present and responding. diff --git a/Tools/autotest/pymavlink/mavlinkv10.py b/Tools/autotest/pymavlink/mavlinkv10.py index 0983079bbf..9c6c055c0f 100644 --- a/Tools/autotest/pymavlink/mavlinkv10.py +++ b/Tools/autotest/pymavlink/mavlinkv10.py @@ -10,6 +10,21 @@ import struct, array, mavutil, time 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): '''MAVLink message header''' def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0): @@ -34,7 +49,9 @@ class MAVLink_message(object): self._type = name def get_msgbuf(self): - return self._msgbuf + if isinstance(self._msgbuf, str): + return self._msgbuf + return self._msgbuf.tostring() def get_header(self): return self._header @@ -69,7 +86,7 @@ class MAVLink_message(object): v = getattr(self, a) ret += '%s : %s, ' % (a, v) ret = ret[0:-2] + '}' - return ret + return ret def pack(self, mav, crc_extra, payload): self._payload = payload @@ -95,7 +112,7 @@ MAV_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink R MAV_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with # stabilization MAV_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt -MAV_MOUNT_MODE_ENUM_END = 5 # +MAV_MOUNT_MODE_ENUM_END = 5 # # MAV_CMD MAV_CMD_NAV_WAYPOINT = 16 # Navigate to MISSION. @@ -153,19 +170,19 @@ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of syst MAV_CMD_OVERRIDE_GOTO = 252 # Hold / continue the current action MAV_CMD_MISSION_START = 300 # start running a mission MAV_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component -MAV_CMD_ENUM_END = 401 # +MAV_CMD_ENUM_END = 401 # # FENCE_ACTION FENCE_ACTION_NONE = 0 # Disable fenced mode FENCE_ACTION_GUIDED = 1 # Switched to guided mode to return point (fence point 0) -FENCE_ACTION_ENUM_END = 2 # +FENCE_ACTION_ENUM_END = 2 # # FENCE_BREACH FENCE_BREACH_NONE = 0 # No last fence breach FENCE_BREACH_MINALT = 1 # Breached minimum altitude FENCE_BREACH_MAXALT = 2 # Breached minimum altitude FENCE_BREACH_BOUNDARY = 3 # Breached fence boundary -FENCE_BREACH_ENUM_END = 4 # +FENCE_BREACH_ENUM_END = 4 # # MAV_AUTOPILOT MAV_AUTOPILOT_GENERIC = 0 # Generic autopilot, full support for everything @@ -181,7 +198,7 @@ MAV_AUTOPILOT_INVALID = 8 # No valid autopilot, e.g. a GCS or other MAVLink comp MAV_AUTOPILOT_PPZ = 9 # PPZ UAV - http://nongnu.org/paparazzi MAV_AUTOPILOT_UDB = 10 # UAV Dev Board MAV_AUTOPILOT_FP = 11 # FlexiPilot -MAV_AUTOPILOT_ENUM_END = 12 # +MAV_AUTOPILOT_ENUM_END = 12 # # MAV_TYPE MAV_TYPE_GENERIC = 0 # Generic micro air vehicle. @@ -201,7 +218,8 @@ MAV_TYPE_HEXAROTOR = 13 # Hexarotor MAV_TYPE_OCTOROTOR = 14 # Octorotor MAV_TYPE_TRICOPTER = 15 # Octorotor 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_CUSTOM_MODE_ENABLED = 1 # 0b00000001 Reserved for future use. @@ -221,7 +239,7 @@ MAV_MODE_FLAG_HIL_ENABLED = 32 # 0b00100000 hardware in the loop simulation. All MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64 # 0b01000000 remote control input is enabled. MAV_MODE_FLAG_SAFETY_ARMED = 128 # 0b10000000 MAV safety set to armed. Motors are enabled / running / can # start. Ready to fly. -MAV_MODE_FLAG_ENUM_END = 129 # +MAV_MODE_FLAG_ENUM_END = 129 # # MAV_MODE_FLAG_DECODE_POSITION MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE = 1 # Eighth bit: 00000001 @@ -232,14 +250,14 @@ MAV_MODE_FLAG_DECODE_POSITION_STABILIZE = 16 # Fourth bit: 00010000 MAV_MODE_FLAG_DECODE_POSITION_HIL = 32 # Third bit: 00100000 MAV_MODE_FLAG_DECODE_POSITION_MANUAL = 64 # Second bit: 01000000 MAV_MODE_FLAG_DECODE_POSITION_SAFETY = 128 # First bit: 10000000 -MAV_MODE_FLAG_DECODE_POSITION_ENUM_END = 129 # +MAV_MODE_FLAG_DECODE_POSITION_ENUM_END = 129 # # MAV_GOTO MAV_GOTO_DO_HOLD = 0 # Hold at the current position. MAV_GOTO_DO_CONTINUE = 1 # Continue with the next item in mission execution. MAV_GOTO_HOLD_AT_CURRENT_POSITION = 2 # Hold at the current position of the system MAV_GOTO_HOLD_AT_SPECIFIED_POSITION = 3 # Hold at the position specified in the parameters of the DO_HOLD action -MAV_GOTO_ENUM_END = 4 # +MAV_GOTO_ENUM_END = 4 # # MAV_MODE MAV_MODE_PREFLIGHT = 0 # System is not ready to fly, booting, calibrating, etc. No flag is set. @@ -263,7 +281,7 @@ MAV_MODE_GUIDED_ARMED = 216 # System is allowed to be active, under autonomous c MAV_MODE_AUTO_ARMED = 220 # System is allowed to be active, under autonomous control and # navigation (the trajectory is decided # onboard and not pre-programmed by MISSIONs) -MAV_MODE_ENUM_END = 221 # +MAV_MODE_ENUM_END = 221 # # MAV_STATE MAV_STATE_UNINIT = 0 # Uninitialized system, state is unknown. @@ -276,36 +294,36 @@ MAV_STATE_EMERGENCY = 6 # System is in a non-normal flight mode. It lost control # over the whole airframe. It is in mayday and # going down. MAV_STATE_POWEROFF = 7 # System just initialized its power-down sequence, will shut down now. -MAV_STATE_ENUM_END = 8 # +MAV_STATE_ENUM_END = 8 # # MAV_COMPONENT -MAV_COMP_ID_ALL = 0 # -MAV_COMP_ID_CAMERA = 100 # -MAV_COMP_ID_SERVO1 = 140 # -MAV_COMP_ID_SERVO2 = 141 # -MAV_COMP_ID_SERVO3 = 142 # -MAV_COMP_ID_SERVO4 = 143 # -MAV_COMP_ID_SERVO5 = 144 # -MAV_COMP_ID_SERVO6 = 145 # -MAV_COMP_ID_SERVO7 = 146 # -MAV_COMP_ID_SERVO8 = 147 # -MAV_COMP_ID_SERVO9 = 148 # -MAV_COMP_ID_SERVO10 = 149 # -MAV_COMP_ID_SERVO11 = 150 # -MAV_COMP_ID_SERVO12 = 151 # -MAV_COMP_ID_SERVO13 = 152 # -MAV_COMP_ID_SERVO14 = 153 # -MAV_COMP_ID_MAPPER = 180 # -MAV_COMP_ID_MISSIONPLANNER = 190 # -MAV_COMP_ID_PATHPLANNER = 195 # -MAV_COMP_ID_IMU = 200 # -MAV_COMP_ID_IMU_2 = 201 # -MAV_COMP_ID_IMU_3 = 202 # -MAV_COMP_ID_GPS = 220 # -MAV_COMP_ID_UDP_BRIDGE = 240 # -MAV_COMP_ID_UART_BRIDGE = 241 # -MAV_COMP_ID_SYSTEM_CONTROL = 250 # -MAV_COMPONENT_ENUM_END = 251 # +MAV_COMP_ID_ALL = 0 # +MAV_COMP_ID_CAMERA = 100 # +MAV_COMP_ID_SERVO1 = 140 # +MAV_COMP_ID_SERVO2 = 141 # +MAV_COMP_ID_SERVO3 = 142 # +MAV_COMP_ID_SERVO4 = 143 # +MAV_COMP_ID_SERVO5 = 144 # +MAV_COMP_ID_SERVO6 = 145 # +MAV_COMP_ID_SERVO7 = 146 # +MAV_COMP_ID_SERVO8 = 147 # +MAV_COMP_ID_SERVO9 = 148 # +MAV_COMP_ID_SERVO10 = 149 # +MAV_COMP_ID_SERVO11 = 150 # +MAV_COMP_ID_SERVO12 = 151 # +MAV_COMP_ID_SERVO13 = 152 # +MAV_COMP_ID_SERVO14 = 153 # +MAV_COMP_ID_MAPPER = 180 # +MAV_COMP_ID_MISSIONPLANNER = 190 # +MAV_COMP_ID_PATHPLANNER = 195 # +MAV_COMP_ID_IMU = 200 # +MAV_COMP_ID_IMU_2 = 201 # +MAV_COMP_ID_IMU_3 = 202 # +MAV_COMP_ID_GPS = 220 # +MAV_COMP_ID_UDP_BRIDGE = 240 # +MAV_COMP_ID_UART_BRIDGE = 241 # +MAV_COMP_ID_SYSTEM_CONTROL = 250 # +MAV_COMPONENT_ENUM_END = 251 # # MAV_FRAME MAV_FRAME_GLOBAL = 0 # Global coordinate frame, WGS84 coordinate system. First value / x: @@ -321,16 +339,16 @@ MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 # Global coordinate frame, WGS84 coordinate sy # positive altitude with 0 being at the # altitude of the home location. MAV_FRAME_LOCAL_ENU = 4 # Local coordinate frame, Z-down (x: east, y: north, z: up) -MAV_FRAME_ENUM_END = 5 # +MAV_FRAME_ENUM_END = 5 # # MAVLINK_DATA_STREAM_TYPE -MAVLINK_DATA_STREAM_IMG_JPEG = 1 # -MAVLINK_DATA_STREAM_IMG_BMP = 2 # -MAVLINK_DATA_STREAM_IMG_RAW8U = 3 # -MAVLINK_DATA_STREAM_IMG_RAW32U = 4 # -MAVLINK_DATA_STREAM_IMG_PGM = 5 # -MAVLINK_DATA_STREAM_IMG_PNG = 6 # -MAVLINK_DATA_STREAM_TYPE_ENUM_END = 7 # +MAVLINK_DATA_STREAM_IMG_JPEG = 1 # +MAVLINK_DATA_STREAM_IMG_BMP = 2 # +MAVLINK_DATA_STREAM_IMG_RAW8U = 3 # +MAVLINK_DATA_STREAM_IMG_RAW32U = 4 # +MAVLINK_DATA_STREAM_IMG_PGM = 5 # +MAVLINK_DATA_STREAM_IMG_PNG = 6 # +MAVLINK_DATA_STREAM_TYPE_ENUM_END = 7 # # MAV_DATA_STREAM MAV_DATA_STREAM_ALL = 0 # Enable all data streams @@ -343,7 +361,7 @@ MAV_DATA_STREAM_POSITION = 6 # Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POS MAV_DATA_STREAM_EXTRA1 = 10 # Dependent on the autopilot MAV_DATA_STREAM_EXTRA2 = 11 # Dependent on the autopilot MAV_DATA_STREAM_EXTRA3 = 12 # Dependent on the autopilot -MAV_DATA_STREAM_ENUM_END = 13 # +MAV_DATA_STREAM_ENUM_END = 13 # # MAV_ROI MAV_ROI_NONE = 0 # No region of interest. @@ -351,7 +369,7 @@ MAV_ROI_WPNEXT = 1 # Point toward next MISSION. MAV_ROI_WPINDEX = 2 # Point toward given MISSION. MAV_ROI_LOCATION = 3 # Point toward fixed location. MAV_ROI_TARGET = 4 # Point toward of given id. -MAV_ROI_ENUM_END = 5 # +MAV_ROI_ENUM_END = 5 # # MAV_CMD_ACK MAV_CMD_ACK_OK = 1 # Command / mission item is ok. @@ -369,17 +387,7 @@ MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE = 6 # The coordinate frame of this comm MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE = 7 # The X or latitude value is out of range. MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE = 8 # The Y or longitude 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_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_CMD_ACK_ENUM_END = 10 # # MAV_RESULT MAV_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED @@ -387,7 +395,7 @@ MAV_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED MAV_RESULT_DENIED = 2 # Command PERMANENTLY DENIED MAV_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED MAV_RESULT_FAILED = 4 # Command executed, but failed -MAV_RESULT_ENUM_END = 5 # +MAV_RESULT_ENUM_END = 5 # # MAV_MISSION_RESULT MAV_MISSION_ACCEPTED = 0 # mission accepted OK @@ -405,7 +413,7 @@ MAV_MISSION_INVALID_PARAM6_Y = 11 # y/param6 has an invalid value MAV_MISSION_INVALID_PARAM7 = 12 # param7 has an invalid value MAV_MISSION_INVALID_SEQUENCE = 13 # received waypoint out of sequence MAV_MISSION_DENIED = 14 # not accepting any mission commands from this communication partner -MAV_MISSION_RESULT_ENUM_END = 15 # +MAV_MISSION_RESULT_ENUM_END = 15 # # MAV_SEVERITY MAV_SEVERITY_EMERGENCY = 0 # System is unusable. This is a "panic" condition. @@ -423,7 +431,7 @@ MAV_SEVERITY_INFO = 6 # Normal operational messages. Useful for logging. No acti # for these messages. MAV_SEVERITY_DEBUG = 7 # Useful non-operational messages that can assist in debugging. These # should not occur during normal operation. -MAV_SEVERITY_ENUM_END = 8 # +MAV_SEVERITY_ENUM_END = 8 # # message IDs MAVLINK_MSG_ID_BAD_DATA = -1 @@ -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_SWARM_ROLL_PITCH_YAW_THRUST = 61 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_REQUEST_DATA_STREAM = 66 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): ''' - + 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') - self._fieldnames = ['target_systems', 'roll', 'pitch', 'yaw', 'thrust'] - self.target_systems = target_systems + self._fieldnames = ['group', 'mode', 'roll', 'pitch', 'yaw', 'thrust'] + self.group = group + self.mode = mode self.roll = roll self.pitch = pitch self.yaw = yaw self.thrust = thrust 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): ''' @@ -1768,6 +1778,26 @@ class MAVLink_nav_controller_output_message(MAVLink_message): def pack(self, mav): return MAVLink_message.pack(self, mav, 183, struct.pack('= 2: (magic, self.expected_length) = struct.unpack('BB', self.buf[0:2]) @@ -2456,7 +2487,7 @@ class MAVLink(object): except struct.error as emsg: raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg) crc2 = mavutil.x25crc(msgbuf[1:-2]) - if True: # using CRC extra + if True: # using CRC extra crc2.accumulate(chr(crc_extra)) if crc != crc2.crc: raise MAVError('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc)) @@ -2511,7 +2542,7 @@ class MAVLink(object): msg = MAVLink_sensor_offsets_message(mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z) msg.pack(self) return msg - + def sensor_offsets_send(self, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z): ''' Offsets and calibrations values for hardware sensors. This @@ -2532,7 +2563,7 @@ class MAVLink(object): ''' return self.send(self.sensor_offsets_encode(mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z)) - + def set_mag_offsets_encode(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z): ''' set the magnetometer offsets @@ -2547,7 +2578,7 @@ class MAVLink(object): msg = MAVLink_set_mag_offsets_message(target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z) msg.pack(self) return msg - + def set_mag_offsets_send(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z): ''' set the magnetometer offsets @@ -2560,7 +2591,7 @@ class MAVLink(object): ''' return self.send(self.set_mag_offsets_encode(target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z)) - + def meminfo_encode(self, brkval, freemem): ''' state of APM memory @@ -2572,7 +2603,7 @@ class MAVLink(object): msg = MAVLink_meminfo_message(brkval, freemem) msg.pack(self) return msg - + def meminfo_send(self, brkval, freemem): ''' state of APM memory @@ -2582,7 +2613,7 @@ class MAVLink(object): ''' return self.send(self.meminfo_encode(brkval, freemem)) - + def ap_adc_encode(self, adc1, adc2, adc3, adc4, adc5, adc6): ''' raw ADC output @@ -2598,7 +2629,7 @@ class MAVLink(object): msg = MAVLink_ap_adc_message(adc1, adc2, adc3, adc4, adc5, adc6) msg.pack(self) return msg - + def ap_adc_send(self, adc1, adc2, adc3, adc4, adc5, adc6): ''' raw ADC output @@ -2612,7 +2643,7 @@ class MAVLink(object): ''' return self.send(self.ap_adc_encode(adc1, adc2, adc3, adc4, adc5, adc6)) - + def digicam_configure_encode(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value): ''' Configure on-board Camera Control System. @@ -2633,7 +2664,7 @@ class MAVLink(object): msg = MAVLink_digicam_configure_message(target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value) msg.pack(self) return msg - + def digicam_configure_send(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value): ''' Configure on-board Camera Control System. @@ -2652,7 +2683,7 @@ class MAVLink(object): ''' return self.send(self.digicam_configure_encode(target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value)) - + def digicam_control_encode(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value): ''' Control on-board Camera Control System to take shots. @@ -2672,7 +2703,7 @@ class MAVLink(object): msg = MAVLink_digicam_control_message(target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value) msg.pack(self) return msg - + def digicam_control_send(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value): ''' Control on-board Camera Control System to take shots. @@ -2690,7 +2721,7 @@ class MAVLink(object): ''' return self.send(self.digicam_control_encode(target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value)) - + def mount_configure_encode(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw): ''' Message to configure a camera mount, directional antenna, etc. @@ -2706,7 +2737,7 @@ class MAVLink(object): msg = MAVLink_mount_configure_message(target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw) msg.pack(self) return msg - + def mount_configure_send(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw): ''' Message to configure a camera mount, directional antenna, etc. @@ -2720,7 +2751,7 @@ class MAVLink(object): ''' return self.send(self.mount_configure_encode(target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw)) - + def mount_control_encode(self, target_system, target_component, input_a, input_b, input_c, save_position): ''' Message to control a camera mount, directional antenna, etc. @@ -2736,7 +2767,7 @@ class MAVLink(object): msg = MAVLink_mount_control_message(target_system, target_component, input_a, input_b, input_c, save_position) msg.pack(self) return msg - + def mount_control_send(self, target_system, target_component, input_a, input_b, input_c, save_position): ''' Message to control a camera mount, directional antenna, etc. @@ -2750,7 +2781,7 @@ class MAVLink(object): ''' return self.send(self.mount_control_encode(target_system, target_component, input_a, input_b, input_c, save_position)) - + def mount_status_encode(self, target_system, target_component, pointing_a, pointing_b, pointing_c): ''' Message with some status from APM to GCS about camera or antenna mount @@ -2765,7 +2796,7 @@ class MAVLink(object): msg = MAVLink_mount_status_message(target_system, target_component, pointing_a, pointing_b, pointing_c) msg.pack(self) return msg - + def mount_status_send(self, target_system, target_component, pointing_a, pointing_b, pointing_c): ''' Message with some status from APM to GCS about camera or antenna mount @@ -2778,7 +2809,7 @@ class MAVLink(object): ''' return self.send(self.mount_status_encode(target_system, target_component, pointing_a, pointing_b, pointing_c)) - + def fence_point_encode(self, target_system, target_component, idx, count, lat, lng): ''' A fence point. Used to set a point when from GCS -> MAV. @@ -2795,7 +2826,7 @@ class MAVLink(object): msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng) msg.pack(self) return msg - + def fence_point_send(self, target_system, target_component, idx, count, lat, lng): ''' A fence point. Used to set a point when from GCS -> MAV. @@ -2810,7 +2841,7 @@ class MAVLink(object): ''' return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng)) - + def fence_fetch_point_encode(self, target_system, target_component, idx): ''' Request a current fence point from MAV @@ -2823,7 +2854,7 @@ class MAVLink(object): msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx) msg.pack(self) return msg - + def fence_fetch_point_send(self, target_system, target_component, idx): ''' Request a current fence point from MAV @@ -2834,7 +2865,7 @@ class MAVLink(object): ''' return self.send(self.fence_fetch_point_encode(target_system, target_component, idx)) - + def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time): ''' Status of geo-fencing. Sent in extended status stream when @@ -2849,7 +2880,7 @@ class MAVLink(object): msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time) msg.pack(self) return msg - + def fence_status_send(self, breach_status, breach_count, breach_type, breach_time): ''' Status of geo-fencing. Sent in extended status stream when @@ -2862,7 +2893,7 @@ class MAVLink(object): ''' return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time)) - + def ahrs_encode(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): ''' Status of DCM attitude estimator @@ -2879,7 +2910,7 @@ class MAVLink(object): msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw) msg.pack(self) return msg - + def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): ''' Status of DCM attitude estimator @@ -2894,7 +2925,7 @@ class MAVLink(object): ''' return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)) - + def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): ''' Status of simulation environment, if used @@ -2913,7 +2944,7 @@ class MAVLink(object): msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro) msg.pack(self) return msg - + def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): ''' Status of simulation environment, if used @@ -2930,7 +2961,7 @@ class MAVLink(object): ''' return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)) - + def hwstatus_encode(self, Vcc, I2Cerr): ''' Status of key hardware @@ -2942,7 +2973,7 @@ class MAVLink(object): msg = MAVLink_hwstatus_message(Vcc, I2Cerr) msg.pack(self) return msg - + def hwstatus_send(self, Vcc, I2Cerr): ''' Status of key hardware @@ -2952,7 +2983,7 @@ class MAVLink(object): ''' return self.send(self.hwstatus_encode(Vcc, I2Cerr)) - + def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): ''' Status generated by radio @@ -2969,7 +3000,7 @@ class MAVLink(object): msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed) msg.pack(self) return msg - + def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): ''' Status generated by radio @@ -2984,7 +3015,7 @@ class MAVLink(object): ''' return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)) - + def heartbeat_encode(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3): ''' The heartbeat message shows that a system is present and responding. @@ -3004,7 +3035,7 @@ class MAVLink(object): msg = MAVLink_heartbeat_message(type, autopilot, base_mode, custom_mode, system_status, mavlink_version) msg.pack(self) return msg - + def heartbeat_send(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3): ''' The heartbeat message shows that a system is present and responding. @@ -3022,7 +3053,7 @@ class MAVLink(object): ''' return self.send(self.heartbeat_encode(type, autopilot, base_mode, custom_mode, system_status, mavlink_version)) - + def sys_status_encode(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4): ''' The general system state. If the system is following the MAVLink @@ -3063,7 +3094,7 @@ class MAVLink(object): msg = MAVLink_sys_status_message(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4) msg.pack(self) return msg - + def sys_status_send(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4): ''' The general system state. If the system is following the MAVLink @@ -3102,7 +3133,7 @@ class MAVLink(object): ''' return self.send(self.sys_status_encode(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4)) - + def system_time_encode(self, time_unix_usec, time_boot_ms): ''' The system time is the time of the master clock, typically the @@ -3115,7 +3146,7 @@ class MAVLink(object): msg = MAVLink_system_time_message(time_unix_usec, time_boot_ms) msg.pack(self) return msg - + def system_time_send(self, time_unix_usec, time_boot_ms): ''' The system time is the time of the master clock, typically the @@ -3126,7 +3157,7 @@ class MAVLink(object): ''' return self.send(self.system_time_encode(time_unix_usec, time_boot_ms)) - + def ping_encode(self, time_usec, seq, target_system, target_component): ''' A ping message either requesting or responding to a ping. This allows @@ -3142,7 +3173,7 @@ class MAVLink(object): msg = MAVLink_ping_message(time_usec, seq, target_system, target_component) msg.pack(self) return msg - + def ping_send(self, time_usec, seq, target_system, target_component): ''' A ping message either requesting or responding to a ping. This allows @@ -3156,7 +3187,7 @@ class MAVLink(object): ''' return self.send(self.ping_encode(time_usec, seq, target_system, target_component)) - + def change_operator_control_encode(self, target_system, control_request, version, passkey): ''' Request to control this MAV @@ -3170,7 +3201,7 @@ class MAVLink(object): msg = MAVLink_change_operator_control_message(target_system, control_request, version, passkey) msg.pack(self) return msg - + def change_operator_control_send(self, target_system, control_request, version, passkey): ''' Request to control this MAV @@ -3182,7 +3213,7 @@ class MAVLink(object): ''' return self.send(self.change_operator_control_encode(target_system, control_request, version, passkey)) - + def change_operator_control_ack_encode(self, gcs_system_id, control_request, ack): ''' Accept / deny control of this MAV @@ -3195,7 +3226,7 @@ class MAVLink(object): msg = MAVLink_change_operator_control_ack_message(gcs_system_id, control_request, ack) msg.pack(self) return msg - + def change_operator_control_ack_send(self, gcs_system_id, control_request, ack): ''' Accept / deny control of this MAV @@ -3206,7 +3237,7 @@ class MAVLink(object): ''' return self.send(self.change_operator_control_ack_encode(gcs_system_id, control_request, ack)) - + def auth_key_encode(self, key): ''' Emit an encrypted signature / key identifying this system. PLEASE @@ -3220,7 +3251,7 @@ class MAVLink(object): msg = MAVLink_auth_key_message(key) msg.pack(self) return msg - + def auth_key_send(self, key): ''' Emit an encrypted signature / key identifying this system. PLEASE @@ -3232,7 +3263,7 @@ class MAVLink(object): ''' return self.send(self.auth_key_encode(key)) - + def set_mode_encode(self, target_system, base_mode, custom_mode): ''' Set the system mode, as defined by enum MAV_MODE. There is no target @@ -3247,7 +3278,7 @@ class MAVLink(object): msg = MAVLink_set_mode_message(target_system, base_mode, custom_mode) msg.pack(self) return msg - + def set_mode_send(self, target_system, base_mode, custom_mode): ''' Set the system mode, as defined by enum MAV_MODE. There is no target @@ -3260,7 +3291,7 @@ class MAVLink(object): ''' return self.send(self.set_mode_encode(target_system, base_mode, custom_mode)) - + def param_request_read_encode(self, target_system, target_component, param_id, param_index): ''' Request to read the onboard parameter with the param_id string id. @@ -3275,14 +3306,14 @@ class MAVLink(object): target_system : System ID (uint8_t) target_component : Component ID (uint8_t) - param_id : Onboard parameter id (char) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) + 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 (else the param id will be ignored) (int16_t) ''' msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index) msg.pack(self) return msg - + def param_request_read_send(self, target_system, target_component, param_id, param_index): ''' Request to read the onboard parameter with the param_id string id. @@ -3297,12 +3328,12 @@ class MAVLink(object): target_system : System ID (uint8_t) target_component : Component ID (uint8_t) - param_id : Onboard parameter id (char) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) + 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 (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)) - + def param_request_list_encode(self, target_system, target_component): ''' Request all parameters of this component. After his request, all @@ -3315,7 +3346,7 @@ class MAVLink(object): msg = MAVLink_param_request_list_message(target_system, target_component) msg.pack(self) return msg - + def param_request_list_send(self, target_system, target_component): ''' Request all parameters of this component. After his request, all @@ -3326,7 +3357,7 @@ class MAVLink(object): ''' return self.send(self.param_request_list_encode(target_system, target_component)) - + def param_value_encode(self, param_id, param_value, param_type, param_count, param_index): ''' Emit the value of a onboard parameter. The inclusion of param_count @@ -3334,9 +3365,9 @@ class MAVLink(object): keep track of received parameters and allows him to 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_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_index : Index of this onboard parameter (uint16_t) @@ -3344,7 +3375,7 @@ class MAVLink(object): msg = MAVLink_param_value_message(param_id, param_value, param_type, param_count, param_index) msg.pack(self) return msg - + def param_value_send(self, param_id, param_value, param_type, param_count, param_index): ''' Emit the value of a onboard parameter. The inclusion of param_count @@ -3352,15 +3383,15 @@ class MAVLink(object): keep track of received parameters and allows him to 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_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_index : Index of this onboard parameter (uint16_t) ''' return self.send(self.param_value_encode(param_id, param_value, param_type, param_count, param_index)) - + def param_set_encode(self, target_system, target_component, param_id, param_value, param_type): ''' Set a parameter value TEMPORARILY to RAM. It will be reset to default @@ -3376,15 +3407,15 @@ class MAVLink(object): target_system : System 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_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.pack(self) return msg - + def param_set_send(self, target_system, target_component, param_id, param_value, param_type): ''' Set a parameter value TEMPORARILY to RAM. It will be reset to default @@ -3400,13 +3431,13 @@ class MAVLink(object): target_system : System 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_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)) - + def gps_raw_int_encode(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible): ''' The global position, as returned by the Global Positioning System @@ -3431,7 +3462,7 @@ class MAVLink(object): msg = MAVLink_gps_raw_int_message(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible) msg.pack(self) return msg - + def gps_raw_int_send(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible): ''' The global position, as returned by the Global Positioning System @@ -3454,7 +3485,7 @@ class MAVLink(object): ''' return self.send(self.gps_raw_int_encode(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible)) - + def gps_status_encode(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): ''' The positioning status, as reported by GPS. This message is intended @@ -3474,7 +3505,7 @@ class MAVLink(object): msg = MAVLink_gps_status_message(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr) msg.pack(self) return msg - + def gps_status_send(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): ''' The positioning status, as reported by GPS. This message is intended @@ -3492,7 +3523,7 @@ class MAVLink(object): ''' return self.send(self.gps_status_encode(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr)) - + def scaled_imu_encode(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): ''' The RAW IMU readings for the usual 9DOF sensor setup. This message @@ -3514,7 +3545,7 @@ class MAVLink(object): msg = MAVLink_scaled_imu_message(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) msg.pack(self) return msg - + def scaled_imu_send(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): ''' The RAW IMU readings for the usual 9DOF sensor setup. This message @@ -3534,7 +3565,7 @@ class MAVLink(object): ''' return self.send(self.scaled_imu_encode(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - + def raw_imu_encode(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): ''' The RAW IMU readings for the usual 9DOF sensor setup. This message @@ -3556,7 +3587,7 @@ class MAVLink(object): msg = MAVLink_raw_imu_message(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) msg.pack(self) return msg - + def raw_imu_send(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): ''' The RAW IMU readings for the usual 9DOF sensor setup. This message @@ -3576,7 +3607,7 @@ class MAVLink(object): ''' return self.send(self.raw_imu_encode(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - + def raw_pressure_encode(self, time_usec, press_abs, press_diff1, press_diff2, temperature): ''' The RAW pressure readings for the typical setup of one absolute @@ -3593,7 +3624,7 @@ class MAVLink(object): msg = MAVLink_raw_pressure_message(time_usec, press_abs, press_diff1, press_diff2, temperature) msg.pack(self) return msg - + def raw_pressure_send(self, time_usec, press_abs, press_diff1, press_diff2, temperature): ''' The RAW pressure readings for the typical setup of one absolute @@ -3608,7 +3639,7 @@ class MAVLink(object): ''' return self.send(self.raw_pressure_encode(time_usec, press_abs, press_diff1, press_diff2, temperature)) - + def scaled_pressure_encode(self, time_boot_ms, press_abs, press_diff, temperature): ''' The pressure readings for the typical setup of one absolute and @@ -3624,7 +3655,7 @@ class MAVLink(object): msg = MAVLink_scaled_pressure_message(time_boot_ms, press_abs, press_diff, temperature) msg.pack(self) return msg - + def scaled_pressure_send(self, time_boot_ms, press_abs, press_diff, temperature): ''' The pressure readings for the typical setup of one absolute and @@ -3638,7 +3669,7 @@ class MAVLink(object): ''' return self.send(self.scaled_pressure_encode(time_boot_ms, press_abs, press_diff, temperature)) - + def attitude_encode(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): ''' The attitude in the aeronautical frame (right-handed, Z-down, X-front, @@ -3656,7 +3687,7 @@ class MAVLink(object): msg = MAVLink_attitude_message(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed) msg.pack(self) return msg - + def attitude_send(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): ''' The attitude in the aeronautical frame (right-handed, Z-down, X-front, @@ -3672,7 +3703,7 @@ class MAVLink(object): ''' return self.send(self.attitude_encode(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)) - + def attitude_quaternion_encode(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed): ''' The attitude in the aeronautical frame (right-handed, Z-down, X-front, @@ -3691,7 +3722,7 @@ class MAVLink(object): msg = MAVLink_attitude_quaternion_message(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed) msg.pack(self) return msg - + def attitude_quaternion_send(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed): ''' The attitude in the aeronautical frame (right-handed, Z-down, X-front, @@ -3708,7 +3739,7 @@ class MAVLink(object): ''' return self.send(self.attitude_quaternion_encode(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed)) - + def local_position_ned_encode(self, time_boot_ms, x, y, z, vx, vy, vz): ''' The filtered local position (e.g. fused computer vision and @@ -3728,7 +3759,7 @@ class MAVLink(object): msg = MAVLink_local_position_ned_message(time_boot_ms, x, y, z, vx, vy, vz) msg.pack(self) return msg - + def local_position_ned_send(self, time_boot_ms, x, y, z, vx, vy, vz): ''' The filtered local position (e.g. fused computer vision and @@ -3746,7 +3777,7 @@ class MAVLink(object): ''' return self.send(self.local_position_ned_encode(time_boot_ms, x, y, z, vx, vy, vz)) - + def global_position_int_encode(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg): ''' The filtered global position (e.g. fused GPS and accelerometers). The @@ -3768,7 +3799,7 @@ class MAVLink(object): msg = MAVLink_global_position_int_message(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg) msg.pack(self) return msg - + def global_position_int_send(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg): ''' The filtered global position (e.g. fused GPS and accelerometers). The @@ -3788,7 +3819,7 @@ class MAVLink(object): ''' return self.send(self.global_position_int_encode(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg)) - + def rc_channels_scaled_encode(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): ''' The scaled values of the RC channels received. (-100%) -10000, (0%) 0, @@ -3810,7 +3841,7 @@ class MAVLink(object): msg = MAVLink_rc_channels_scaled_message(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi) msg.pack(self) return msg - + def rc_channels_scaled_send(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): ''' The scaled values of the RC channels received. (-100%) -10000, (0%) 0, @@ -3830,7 +3861,7 @@ class MAVLink(object): ''' return self.send(self.rc_channels_scaled_encode(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi)) - + def rc_channels_raw_encode(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): ''' The RAW values of the RC channels received. The standard PPM @@ -3854,7 +3885,7 @@ class MAVLink(object): msg = MAVLink_rc_channels_raw_message(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi) msg.pack(self) return msg - + def rc_channels_raw_send(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): ''' The RAW values of the RC channels received. The standard PPM @@ -3876,7 +3907,7 @@ class MAVLink(object): ''' return self.send(self.rc_channels_raw_encode(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi)) - + def servo_output_raw_encode(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): ''' The RAW values of the servo outputs (for RC input from the remote, use @@ -3899,7 +3930,7 @@ class MAVLink(object): msg = MAVLink_servo_output_raw_message(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw) msg.pack(self) return msg - + def servo_output_raw_send(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): ''' The RAW values of the servo outputs (for RC input from the remote, use @@ -3920,7 +3951,7 @@ class MAVLink(object): ''' return self.send(self.servo_output_raw_encode(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw)) - + def mission_request_partial_list_encode(self, target_system, target_component, start_index, end_index): ''' Request the overall list of MISSIONs from the system/component. @@ -3935,7 +3966,7 @@ class MAVLink(object): msg = MAVLink_mission_request_partial_list_message(target_system, target_component, start_index, end_index) msg.pack(self) return msg - + def mission_request_partial_list_send(self, target_system, target_component, start_index, end_index): ''' Request the overall list of MISSIONs from the system/component. @@ -3948,7 +3979,7 @@ class MAVLink(object): ''' return self.send(self.mission_request_partial_list_encode(target_system, target_component, start_index, end_index)) - + def mission_write_partial_list_encode(self, target_system, target_component, start_index, end_index): ''' This message is sent to the MAV to write a partial list. If start @@ -3965,7 +3996,7 @@ class MAVLink(object): msg = MAVLink_mission_write_partial_list_message(target_system, target_component, start_index, end_index) msg.pack(self) return msg - + def mission_write_partial_list_send(self, target_system, target_component, start_index, end_index): ''' This message is sent to the MAV to write a partial list. If start @@ -3980,7 +4011,7 @@ class MAVLink(object): ''' return self.send(self.mission_write_partial_list_encode(target_system, target_component, start_index, end_index)) - + def mission_item_encode(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): ''' Message encoding a mission item. This message is emitted to announce @@ -4010,7 +4041,7 @@ class MAVLink(object): msg = MAVLink_mission_item_message(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z) msg.pack(self) return msg - + def mission_item_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): ''' Message encoding a mission item. This message is emitted to announce @@ -4038,7 +4069,7 @@ class MAVLink(object): ''' return self.send(self.mission_item_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)) - + def mission_request_encode(self, target_system, target_component, seq): ''' Request the information of the mission item with the sequence number @@ -4054,7 +4085,7 @@ class MAVLink(object): msg = MAVLink_mission_request_message(target_system, target_component, seq) msg.pack(self) return msg - + def mission_request_send(self, target_system, target_component, seq): ''' Request the information of the mission item with the sequence number @@ -4068,7 +4099,7 @@ class MAVLink(object): ''' return self.send(self.mission_request_encode(target_system, target_component, seq)) - + def mission_set_current_encode(self, target_system, target_component, seq): ''' Set the mission item with sequence number seq as current item. This @@ -4084,7 +4115,7 @@ class MAVLink(object): msg = MAVLink_mission_set_current_message(target_system, target_component, seq) msg.pack(self) return msg - + def mission_set_current_send(self, target_system, target_component, seq): ''' Set the mission item with sequence number seq as current item. This @@ -4098,7 +4129,7 @@ class MAVLink(object): ''' return self.send(self.mission_set_current_encode(target_system, target_component, seq)) - + def mission_current_encode(self, seq): ''' Message that announces the sequence number of the current active @@ -4111,7 +4142,7 @@ class MAVLink(object): msg = MAVLink_mission_current_message(seq) msg.pack(self) return msg - + def mission_current_send(self, seq): ''' Message that announces the sequence number of the current active @@ -4122,7 +4153,7 @@ class MAVLink(object): ''' return self.send(self.mission_current_encode(seq)) - + def mission_request_list_encode(self, target_system, target_component): ''' Request the overall list of mission items from the system/component. @@ -4134,7 +4165,7 @@ class MAVLink(object): msg = MAVLink_mission_request_list_message(target_system, target_component) msg.pack(self) return msg - + def mission_request_list_send(self, target_system, target_component): ''' Request the overall list of mission items from the system/component. @@ -4144,7 +4175,7 @@ class MAVLink(object): ''' return self.send(self.mission_request_list_encode(target_system, target_component)) - + def mission_count_encode(self, target_system, target_component, count): ''' This message is emitted as response to MISSION_REQUEST_LIST by the MAV @@ -4160,7 +4191,7 @@ class MAVLink(object): msg = MAVLink_mission_count_message(target_system, target_component, count) msg.pack(self) return msg - + def mission_count_send(self, target_system, target_component, count): ''' This message is emitted as response to MISSION_REQUEST_LIST by the MAV @@ -4174,7 +4205,7 @@ class MAVLink(object): ''' return self.send(self.mission_count_encode(target_system, target_component, count)) - + def mission_clear_all_encode(self, target_system, target_component): ''' Delete all mission items at once. @@ -4186,7 +4217,7 @@ class MAVLink(object): msg = MAVLink_mission_clear_all_message(target_system, target_component) msg.pack(self) return msg - + def mission_clear_all_send(self, target_system, target_component): ''' Delete all mission items at once. @@ -4196,7 +4227,7 @@ class MAVLink(object): ''' return self.send(self.mission_clear_all_encode(target_system, target_component)) - + def mission_item_reached_encode(self, seq): ''' A certain mission item has been reached. The system will either hold @@ -4210,7 +4241,7 @@ class MAVLink(object): msg = MAVLink_mission_item_reached_message(seq) msg.pack(self) return msg - + def mission_item_reached_send(self, seq): ''' A certain mission item has been reached. The system will either hold @@ -4222,7 +4253,7 @@ class MAVLink(object): ''' return self.send(self.mission_item_reached_encode(seq)) - + def mission_ack_encode(self, target_system, target_component, type): ''' Ack message during MISSION handling. The type field states if this @@ -4237,7 +4268,7 @@ class MAVLink(object): msg = MAVLink_mission_ack_message(target_system, target_component, type) msg.pack(self) return msg - + def mission_ack_send(self, target_system, target_component, type): ''' Ack message during MISSION handling. The type field states if this @@ -4250,7 +4281,7 @@ class MAVLink(object): ''' return self.send(self.mission_ack_encode(target_system, target_component, type)) - + def set_gps_global_origin_encode(self, target_system, latitude, longitude, altitude): ''' As local MISSIONs exist, the global MISSION reference allows to @@ -4268,7 +4299,7 @@ class MAVLink(object): msg = MAVLink_set_gps_global_origin_message(target_system, latitude, longitude, altitude) msg.pack(self) return msg - + def set_gps_global_origin_send(self, target_system, latitude, longitude, altitude): ''' As local MISSIONs exist, the global MISSION reference allows to @@ -4284,7 +4315,7 @@ class MAVLink(object): ''' return self.send(self.set_gps_global_origin_encode(target_system, latitude, longitude, altitude)) - + def gps_global_origin_encode(self, latitude, longitude, altitude): ''' Once the MAV sets a new GPS-Local correspondence, this message @@ -4298,7 +4329,7 @@ class MAVLink(object): msg = MAVLink_gps_global_origin_message(latitude, longitude, altitude) msg.pack(self) return msg - + def gps_global_origin_send(self, latitude, longitude, altitude): ''' Once the MAV sets a new GPS-Local correspondence, this message @@ -4310,7 +4341,7 @@ class MAVLink(object): ''' return self.send(self.gps_global_origin_encode(latitude, longitude, altitude)) - + def set_local_position_setpoint_encode(self, target_system, target_component, coordinate_frame, x, y, z, yaw): ''' Set the setpoint for a local position controller. This is the position @@ -4333,7 +4364,7 @@ class MAVLink(object): msg = MAVLink_set_local_position_setpoint_message(target_system, target_component, coordinate_frame, x, y, z, yaw) msg.pack(self) return msg - + def set_local_position_setpoint_send(self, target_system, target_component, coordinate_frame, x, y, z, yaw): ''' Set the setpoint for a local position controller. This is the position @@ -4354,7 +4385,7 @@ class MAVLink(object): ''' return self.send(self.set_local_position_setpoint_encode(target_system, target_component, coordinate_frame, x, y, z, yaw)) - + def local_position_setpoint_encode(self, coordinate_frame, x, y, z, yaw): ''' Transmit the current local setpoint of the controller to other MAVs @@ -4370,7 +4401,7 @@ class MAVLink(object): msg = MAVLink_local_position_setpoint_message(coordinate_frame, x, y, z, yaw) msg.pack(self) return msg - + def local_position_setpoint_send(self, coordinate_frame, x, y, z, yaw): ''' Transmit the current local setpoint of the controller to other MAVs @@ -4384,7 +4415,7 @@ class MAVLink(object): ''' return self.send(self.local_position_setpoint_encode(coordinate_frame, x, y, z, yaw)) - + def global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw): ''' Transmit the current local setpoint of the controller to other MAVs @@ -4400,7 +4431,7 @@ class MAVLink(object): msg = MAVLink_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw) msg.pack(self) return msg - + def global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw): ''' Transmit the current local setpoint of the controller to other MAVs @@ -4414,7 +4445,7 @@ class MAVLink(object): ''' return self.send(self.global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw)) - + def set_global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw): ''' Set the current global position setpoint. @@ -4429,7 +4460,7 @@ class MAVLink(object): msg = MAVLink_set_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw) msg.pack(self) return msg - + def set_global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw): ''' Set the current global position setpoint. @@ -4442,7 +4473,7 @@ class MAVLink(object): ''' return self.send(self.set_global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw)) - + def safety_set_allowed_area_encode(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): ''' Set a safety zone (volume), which is defined by two corners of a cube. @@ -4465,7 +4496,7 @@ class MAVLink(object): msg = MAVLink_safety_set_allowed_area_message(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z) msg.pack(self) return msg - + def safety_set_allowed_area_send(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): ''' Set a safety zone (volume), which is defined by two corners of a cube. @@ -4486,7 +4517,7 @@ class MAVLink(object): ''' return self.send(self.safety_set_allowed_area_encode(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z)) - + def safety_allowed_area_encode(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): ''' Read out the safety zone the MAV currently assumes. @@ -4503,7 +4534,7 @@ class MAVLink(object): msg = MAVLink_safety_allowed_area_message(frame, p1x, p1y, p1z, p2x, p2y, p2z) msg.pack(self) return msg - + def safety_allowed_area_send(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): ''' Read out the safety zone the MAV currently assumes. @@ -4518,7 +4549,7 @@ class MAVLink(object): ''' return self.send(self.safety_allowed_area_encode(frame, p1x, p1y, p1z, p2x, p2y, p2z)) - + def set_roll_pitch_yaw_thrust_encode(self, target_system, target_component, roll, pitch, yaw, thrust): ''' Set roll, pitch and yaw. @@ -4534,7 +4565,7 @@ class MAVLink(object): msg = MAVLink_set_roll_pitch_yaw_thrust_message(target_system, target_component, roll, pitch, yaw, thrust) msg.pack(self) return msg - + def set_roll_pitch_yaw_thrust_send(self, target_system, target_component, roll, pitch, yaw, thrust): ''' Set roll, pitch and yaw. @@ -4548,7 +4579,7 @@ class MAVLink(object): ''' return self.send(self.set_roll_pitch_yaw_thrust_encode(target_system, target_component, roll, pitch, yaw, thrust)) - + def set_roll_pitch_yaw_speed_thrust_encode(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): ''' Set roll, pitch and yaw. @@ -4564,7 +4595,7 @@ class MAVLink(object): msg = MAVLink_set_roll_pitch_yaw_speed_thrust_message(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust) msg.pack(self) return msg - + def set_roll_pitch_yaw_speed_thrust_send(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): ''' Set roll, pitch and yaw. @@ -4578,7 +4609,7 @@ class MAVLink(object): ''' return self.send(self.set_roll_pitch_yaw_speed_thrust_encode(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust)) - + def roll_pitch_yaw_thrust_setpoint_encode(self, time_boot_ms, roll, pitch, yaw, thrust): ''' Setpoint in roll, pitch, yaw currently active on the system. @@ -4593,7 +4624,7 @@ class MAVLink(object): msg = MAVLink_roll_pitch_yaw_thrust_setpoint_message(time_boot_ms, roll, pitch, yaw, thrust) msg.pack(self) return msg - + def roll_pitch_yaw_thrust_setpoint_send(self, time_boot_ms, roll, pitch, yaw, thrust): ''' Setpoint in roll, pitch, yaw currently active on the system. @@ -4606,7 +4637,7 @@ class MAVLink(object): ''' return self.send(self.roll_pitch_yaw_thrust_setpoint_encode(time_boot_ms, roll, pitch, yaw, thrust)) - + def roll_pitch_yaw_speed_thrust_setpoint_encode(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust): ''' Setpoint in rollspeed, pitchspeed, yawspeed currently active on the @@ -4622,7 +4653,7 @@ class MAVLink(object): msg = MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust) msg.pack(self) return msg - + def roll_pitch_yaw_speed_thrust_setpoint_send(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust): ''' Setpoint in rollspeed, pitchspeed, yawspeed currently active on the @@ -4636,7 +4667,7 @@ class MAVLink(object): ''' return self.send(self.roll_pitch_yaw_speed_thrust_setpoint_encode(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust)) - + def set_quad_motors_setpoint_encode(self, target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw): ''' Setpoint in the four motor speeds @@ -4651,7 +4682,7 @@ class MAVLink(object): msg = MAVLink_set_quad_motors_setpoint_message(target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw) msg.pack(self) return msg - + def set_quad_motors_setpoint_send(self, target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw): ''' Setpoint in the four motor speeds @@ -4664,35 +4695,37 @@ 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)) - - 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 - - target_systems : System IDs for 6 quadrotors: 0..5, the ID's are the MAVLink IDs (uint8_t) - roll : Desired roll angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) - pitch : Desired pitch angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) - yaw : Desired yaw angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) - thrust : Collective thrust, scaled to uint16 for 6 quadrotors: 0..5 (uint16_t) + 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) + 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_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) 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 - - target_systems : System IDs for 6 quadrotors: 0..5, the ID's are the MAVLink IDs (uint8_t) - roll : Desired roll angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) - pitch : Desired pitch angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) - yaw : Desired yaw angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) - thrust : Collective thrust, scaled to uint16 for 6 quadrotors: 0..5 (uint16_t) + 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) + 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_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): ''' Outputs of the APM navigation controller. The primary use of this @@ -4713,7 +4746,7 @@ class MAVLink(object): msg = MAVLink_nav_controller_output_message(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error) msg.pack(self) return msg - + def nav_controller_output_send(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): ''' Outputs of the APM navigation controller. The primary use of this @@ -4732,7 +4765,43 @@ 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)) + + 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): ''' Corrects the systems state by adding an error correction term to the @@ -4753,7 +4822,7 @@ class MAVLink(object): msg = MAVLink_state_correction_message(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr) msg.pack(self) return msg - + def state_correction_send(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): ''' Corrects the systems state by adding an error correction term to the @@ -4772,10 +4841,10 @@ class MAVLink(object): ''' return self.send(self.state_correction_encode(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr)) - + def request_data_stream_encode(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): ''' - + target_system : The target requested to send the message stream. (uint8_t) target_component : The target requested to send the message stream. (uint8_t) @@ -4787,10 +4856,10 @@ class MAVLink(object): msg = MAVLink_request_data_stream_message(target_system, target_component, req_stream_id, req_message_rate, start_stop) msg.pack(self) return msg - + def request_data_stream_send(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): ''' - + target_system : The target requested to send the message stream. (uint8_t) target_component : The target requested to send the message stream. (uint8_t) @@ -4800,10 +4869,10 @@ class MAVLink(object): ''' return self.send(self.request_data_stream_encode(target_system, target_component, req_stream_id, req_message_rate, start_stop)) - + def data_stream_encode(self, stream_id, message_rate, on_off): ''' - + stream_id : The ID of the requested data stream (uint8_t) message_rate : The requested interval between two messages of this type (uint16_t) @@ -4813,10 +4882,10 @@ class MAVLink(object): msg = MAVLink_data_stream_message(stream_id, message_rate, on_off) msg.pack(self) return msg - + def data_stream_send(self, stream_id, message_rate, on_off): ''' - + stream_id : The ID of the requested data stream (uint8_t) message_rate : The requested interval between two messages of this type (uint16_t) @@ -4824,10 +4893,10 @@ class MAVLink(object): ''' return self.send(self.data_stream_encode(stream_id, message_rate, on_off)) - + def manual_control_encode(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): ''' - + target : The system to be controlled (uint8_t) roll : roll (float) @@ -4843,10 +4912,10 @@ class MAVLink(object): msg = MAVLink_manual_control_message(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual) msg.pack(self) return msg - + def manual_control_send(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): ''' - + target : The system to be controlled (uint8_t) roll : roll (float) @@ -4860,7 +4929,7 @@ class MAVLink(object): ''' return self.send(self.manual_control_encode(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual)) - + def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): ''' The RAW values of the RC channels sent to the MAV to override info @@ -4887,7 +4956,7 @@ class MAVLink(object): msg = MAVLink_rc_channels_override_message(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw) msg.pack(self) return msg - + def rc_channels_override_send(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): ''' The RAW values of the RC channels sent to the MAV to override info @@ -4912,7 +4981,7 @@ class MAVLink(object): ''' return self.send(self.rc_channels_override_encode(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw)) - + def vfr_hud_encode(self, airspeed, groundspeed, heading, throttle, alt, climb): ''' Metrics typically displayed on a HUD for fixed wing aircraft @@ -4928,7 +4997,7 @@ class MAVLink(object): msg = MAVLink_vfr_hud_message(airspeed, groundspeed, heading, throttle, alt, climb) msg.pack(self) return msg - + def vfr_hud_send(self, airspeed, groundspeed, heading, throttle, alt, climb): ''' Metrics typically displayed on a HUD for fixed wing aircraft @@ -4942,7 +5011,7 @@ class MAVLink(object): ''' return self.send(self.vfr_hud_encode(airspeed, groundspeed, heading, throttle, alt, climb)) - + def command_long_encode(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7): ''' Send a command with up to four parameters to the MAV @@ -4963,7 +5032,7 @@ class MAVLink(object): msg = MAVLink_command_long_message(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7) msg.pack(self) return msg - + def command_long_send(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7): ''' Send a command with up to four parameters to the MAV @@ -4982,7 +5051,7 @@ class MAVLink(object): ''' return self.send(self.command_long_encode(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7)) - + def command_ack_encode(self, command, result): ''' Report status of a command. Includes feedback wether the command was @@ -4995,7 +5064,7 @@ class MAVLink(object): msg = MAVLink_command_ack_message(command, result) msg.pack(self) return msg - + def command_ack_send(self, command, result): ''' Report status of a command. Includes feedback wether the command was @@ -5006,7 +5075,7 @@ class MAVLink(object): ''' return self.send(self.command_ack_encode(command, result)) - + def local_position_ned_system_global_offset_encode(self, time_boot_ms, x, y, z, roll, pitch, yaw): ''' The offset in X, Y, Z and yaw between the LOCAL_POSITION_NED messages @@ -5027,7 +5096,7 @@ class MAVLink(object): msg = MAVLink_local_position_ned_system_global_offset_message(time_boot_ms, x, y, z, roll, pitch, yaw) msg.pack(self) return msg - + def local_position_ned_system_global_offset_send(self, time_boot_ms, x, y, z, roll, pitch, yaw): ''' The offset in X, Y, Z and yaw between the LOCAL_POSITION_NED messages @@ -5046,7 +5115,7 @@ class MAVLink(object): ''' return self.send(self.local_position_ned_system_global_offset_encode(time_boot_ms, x, y, z, roll, pitch, yaw)) - + def hil_state_encode(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): ''' Sent from simulation to autopilot. This packet is useful for high @@ -5074,7 +5143,7 @@ class MAVLink(object): msg = MAVLink_hil_state_message(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc) msg.pack(self) return msg - + def hil_state_send(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): ''' Sent from simulation to autopilot. This packet is useful for high @@ -5100,7 +5169,7 @@ class MAVLink(object): ''' return self.send(self.hil_state_encode(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc)) - + def hil_controls_encode(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode): ''' Sent from autopilot to simulation. Hardware in the loop control @@ -5122,7 +5191,7 @@ class MAVLink(object): msg = MAVLink_hil_controls_message(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode) msg.pack(self) return msg - + def hil_controls_send(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode): ''' Sent from autopilot to simulation. Hardware in the loop control @@ -5142,7 +5211,7 @@ class MAVLink(object): ''' return self.send(self.hil_controls_encode(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode)) - + def hil_rc_inputs_raw_encode(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi): ''' Sent from simulation to autopilot. The RAW values of the RC channels @@ -5170,7 +5239,7 @@ class MAVLink(object): msg = MAVLink_hil_rc_inputs_raw_message(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi) msg.pack(self) return msg - + def hil_rc_inputs_raw_send(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi): ''' Sent from simulation to autopilot. The RAW values of the RC channels @@ -5196,7 +5265,7 @@ class MAVLink(object): ''' return self.send(self.hil_rc_inputs_raw_encode(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi)) - + def optical_flow_encode(self, time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance): ''' Optical flow from a flow sensor (e.g. optical mouse sensor) @@ -5214,7 +5283,7 @@ class MAVLink(object): msg = MAVLink_optical_flow_message(time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance) msg.pack(self) return msg - + def optical_flow_send(self, time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance): ''' Optical flow from a flow sensor (e.g. optical mouse sensor) @@ -5230,10 +5299,10 @@ class MAVLink(object): ''' return self.send(self.optical_flow_encode(time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance)) - + def global_vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X position (float) @@ -5247,10 +5316,10 @@ class MAVLink(object): msg = MAVLink_global_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw) msg.pack(self) return msg - + def global_vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X position (float) @@ -5262,10 +5331,10 @@ class MAVLink(object): ''' return self.send(self.global_vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - + def vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X position (float) @@ -5279,10 +5348,10 @@ class MAVLink(object): msg = MAVLink_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw) msg.pack(self) return msg - + def vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X position (float) @@ -5294,10 +5363,10 @@ class MAVLink(object): ''' return self.send(self.vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - + def vision_speed_estimate_encode(self, usec, x, y, z): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X speed (float) @@ -5308,10 +5377,10 @@ class MAVLink(object): msg = MAVLink_vision_speed_estimate_message(usec, x, y, z) msg.pack(self) return msg - + def vision_speed_estimate_send(self, usec, x, y, z): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X speed (float) @@ -5320,10 +5389,10 @@ class MAVLink(object): ''' return self.send(self.vision_speed_estimate_encode(usec, x, y, z)) - + def vicon_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X position (float) @@ -5337,10 +5406,10 @@ class MAVLink(object): msg = MAVLink_vicon_position_estimate_message(usec, x, y, z, roll, pitch, yaw) msg.pack(self) return msg - + def vicon_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): ''' - + usec : Timestamp (milliseconds) (uint64_t) x : Global X position (float) @@ -5352,7 +5421,7 @@ class MAVLink(object): ''' return self.send(self.vicon_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - + def memory_vect_encode(self, address, ver, type, value): ''' Send raw controller memory. The use of this message is discouraged for @@ -5368,7 +5437,7 @@ class MAVLink(object): msg = MAVLink_memory_vect_message(address, ver, type, value) msg.pack(self) return msg - + def memory_vect_send(self, address, ver, type, value): ''' Send raw controller memory. The use of this message is discouraged for @@ -5382,10 +5451,10 @@ class MAVLink(object): ''' return self.send(self.memory_vect_encode(address, ver, type, value)) - + def debug_vect_encode(self, name, time_usec, x, y, z): ''' - + name : Name (char) time_usec : Timestamp (uint64_t) @@ -5397,10 +5466,10 @@ class MAVLink(object): msg = MAVLink_debug_vect_message(name, time_usec, x, y, z) msg.pack(self) return msg - + def debug_vect_send(self, name, time_usec, x, y, z): ''' - + name : Name (char) time_usec : Timestamp (uint64_t) @@ -5410,7 +5479,7 @@ class MAVLink(object): ''' return self.send(self.debug_vect_encode(name, time_usec, x, y, z)) - + def named_value_float_encode(self, time_boot_ms, name, value): ''' Send a key-value pair as float. The use of this message is discouraged @@ -5426,7 +5495,7 @@ class MAVLink(object): msg = MAVLink_named_value_float_message(time_boot_ms, name, value) msg.pack(self) return msg - + def named_value_float_send(self, time_boot_ms, name, value): ''' Send a key-value pair as float. The use of this message is discouraged @@ -5440,7 +5509,7 @@ class MAVLink(object): ''' return self.send(self.named_value_float_encode(time_boot_ms, name, value)) - + def named_value_int_encode(self, time_boot_ms, name, value): ''' Send a key-value pair as integer. The use of this message is @@ -5456,7 +5525,7 @@ class MAVLink(object): msg = MAVLink_named_value_int_message(time_boot_ms, name, value) msg.pack(self) return msg - + def named_value_int_send(self, time_boot_ms, name, value): ''' Send a key-value pair as integer. The use of this message is @@ -5470,7 +5539,7 @@ class MAVLink(object): ''' return self.send(self.named_value_int_encode(time_boot_ms, name, value)) - + def statustext_encode(self, severity, text): ''' Status text message. These messages are printed in yellow in the COMM @@ -5487,7 +5556,7 @@ class MAVLink(object): msg = MAVLink_statustext_message(severity, text) msg.pack(self) return msg - + def statustext_send(self, severity, text): ''' Status text message. These messages are printed in yellow in the COMM @@ -5502,7 +5571,7 @@ class MAVLink(object): ''' return self.send(self.statustext_encode(severity, text)) - + def debug_encode(self, time_boot_ms, ind, value): ''' Send a debug value. The index is used to discriminate between values. @@ -5517,7 +5586,7 @@ class MAVLink(object): msg = MAVLink_debug_message(time_boot_ms, ind, value) msg.pack(self) return msg - + def debug_send(self, time_boot_ms, ind, value): ''' Send a debug value. The index is used to discriminate between values. @@ -5530,3 +5599,4 @@ class MAVLink(object): ''' return self.send(self.debug_encode(time_boot_ms, ind, value)) + diff --git a/Tools/autotest/pymavlink/mavutil.py b/Tools/autotest/pymavlink/mavutil.py index 12a1d91f02..3f0a013444 100644 --- a/Tools/autotest/pymavlink/mavutil.py +++ b/Tools/autotest/pymavlink/mavutil.py @@ -6,7 +6,7 @@ Copyright Andrew Tridgell 2011 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 mavextra import * @@ -47,9 +47,10 @@ class location(object): class mavfile(object): '''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 - mavfile_global = self + if input: + mavfile_global = self self.fd = fd self.address = address self.messages = { 'MAV' : self } @@ -59,11 +60,13 @@ class mavfile(object): else: self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0) self.params = {} - self.mav = None self.target_system = 0 self.target_component = 0 - self.mav = mavlink.MAVLink(self, srcSystem=source_system) - self.mav.robust_parsing = True + self.source_system = source_system + 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_raw = None self.param_fetch_in_progress = False @@ -80,6 +83,36 @@ class mavfile(object): self.ground_temperature = None self.altitude = 0 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): '''default recv method''' @@ -99,6 +132,9 @@ class mavfile(object): def post_message(self, msg): '''default post message call''' + if '_posted' in msg.__dict__: + return + msg._posted = True msg._timestamp = time.time() type = msg.get_type() self.messages[type] = msg @@ -112,6 +148,19 @@ class mavfile(object): else: 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 if type == 'HEARTBEAT': self.target_system = msg.get_srcSystem() @@ -119,33 +168,42 @@ class mavfile(object): if mavlink.WIRE_PROTOCOL_VERSION == '1.0': self.flightmode = mode_string_v10(msg) elif type == 'PARAM_VALUE': + s = str(msg.param_id) self.params[str(msg.param_id)] = msg.param_value if msg.param_index+1 == msg.param_count: self.param_fetch_in_progress = False 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': self.flightmode = mode_string_v09(msg) elif type == 'GPS_RAW': if self.messages['HOME'].fix_type < 2: 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: 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): '''message receive routine''' self.pre_message() while True: n = self.mav.bytes_needed() 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 if self.logfile_raw: self.logfile_raw.write(str(s)) + if self.first_byte: + self.auto_mavlink_version(s) msg = self.mav.parse_char(s) if msg: self.post_message(msg) @@ -168,6 +226,10 @@ class mavfile(object): continue 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'): '''start logging to the given logfile, with timestamps''' 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): '''wrapper for parameter set''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + if self.mavlink10(): 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, parm_name, parm_value, parm_type) else: @@ -208,35 +270,35 @@ class mavfile(object): def waypoint_request_list_send(self): '''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) else: self.mav.waypoint_request_list_send(self.target_system, self.target_component) def waypoint_clear_all_send(self): '''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) else: self.mav.waypoint_clear_all_send(self.target_system, self.target_component) def waypoint_request_send(self, seq): '''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) else: self.mav.waypoint_request_send(self.target_system, self.target_component, seq) def waypoint_set_current_send(self, seq): '''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) else: self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq) def waypoint_current(self): '''return current waypoint''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + if self.mavlink10(): m = self.recv_match(type='MISSION_CURRENT', blocking=True) else: m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True) @@ -244,14 +306,14 @@ class mavfile(object): def waypoint_count_send(self, seq): '''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) else: self.mav.waypoint_count_send(self.target_system, self.target_component, seq) def set_mode_auto(self): '''enter auto mode''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + if self.mavlink10(): self.mav.command_long_send(self.target_system, self.target_component, mavlink.MAV_CMD_MISSION_START, 0, 0, 0, 0, 0, 0, 0, 0) else: @@ -260,7 +322,7 @@ class mavfile(object): def set_mode_rtl(self): '''enter RTL mode''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + if self.mavlink10(): 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) else: @@ -269,7 +331,7 @@ class mavfile(object): def set_mode_loiter(self): '''enter LOITER mode''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + if self.mavlink10(): 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) else: @@ -278,7 +340,7 @@ class mavfile(object): def calibrate_imu(self): '''calibrate IMU''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + if self.mavlink10(): self.mav.command_long_send(self.target_system, self.target_component, mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0, 1, 1, 1, 1, 0, 0, 0) @@ -288,7 +350,7 @@ class mavfile(object): def calibrate_level(self): '''calibrate accels''' - if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + if self.mavlink10(): self.mav.command_long_send(self.target_system, self.target_component, mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0, 1, 1, 1, 1, 0, 0, 0) @@ -298,7 +360,7 @@ class mavfile(object): def wait_gps_fix(self): 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, condition='GPS_RAW_INT.fix_type==3 and GPS_RAW_INT.lat != 0 and GPS_RAW_INT.alt != 0') else: @@ -308,7 +370,9 @@ class mavfile(object): def location(self): '''return current location''' 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, self.messages['GPS_RAW_INT'].lon*1.0e-7, self.messages['VFR_HUD'].alt, @@ -319,6 +383,19 @@ class mavfile(object): self.messages['VFR_HUD'].alt, 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): '''a serial mavlink port''' @@ -329,9 +406,11 @@ class mavserial(mavfile): self.autoreconnect = autoreconnect self.port = serial.Serial(self.device, self.baud, timeout=0, dsrdtr=False, rtscts=False, xonxoff=False) - try: fd = self.port.fileno() + flags = fcntl.fcntl(fd, fcntl.F_GETFD) + flags |= fcntl.FD_CLOEXEC + fcntl.fcntl(fd, fcntl.F_SETFD, flags) except Exception: fd = None mavfile.__init__(self, fd, device, source_system=source_system) @@ -351,7 +430,7 @@ class mavserial(mavfile): def write(self, buf): try: return self.port.write(buf) - except OSError: + except Exception: if self.autoreconnect: self.reset() return -1 @@ -370,7 +449,7 @@ class mavserial(mavfile): return except Exception: print("Failed to reopen %s" % self.device) - time.sleep(1) + time.sleep(0.5) class mavudp(mavfile): @@ -383,12 +462,16 @@ class mavudp(mavfile): self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udp_server = input if input: + self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.port.bind((a[0], int(a[1]))) else: 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.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): self.port.close() @@ -397,7 +480,7 @@ class mavudp(mavfile): try: data, self.last_address = self.port.recvfrom(300) except socket.error as e: - if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]: + if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]: return "" raise return data @@ -418,6 +501,8 @@ class mavudp(mavfile): s = self.recv() if len(s) == 0: return None + if self.first_byte: + self.auto_mavlink_version(s) msg = self.mav.parse_buffer(s) if msg is not None: for m in msg: @@ -437,6 +522,9 @@ class mavtcp(mavfile): self.destination_addr = (a[0], int(a[1])) self.port.connect(self.destination_addr) 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) mavfile.__init__(self, self.port.fileno(), device, source_system=source_system) @@ -485,6 +573,7 @@ class mavlogfile(mavfile): self._timestamp = 0 else: self._timestamp = time.time() + self.stop_on_EOF = True def close(self): self.f.close() @@ -500,7 +589,8 @@ class mavlogfile(mavfile): def pre_message(self): '''read timestamp if needed''' # read the timestamp - self.percent = (100.0 * self.f.tell()) / self.filesize + if self.filesize != 0: + self.percent = (100.0 * self.f.tell()) / self.filesize if self.notimestamps: return if self.planner_format: @@ -510,12 +600,14 @@ class mavlogfile(mavfile): hnsec = self._two64 + float(tbuf[0:20]) t = hnsec * 1.0e-7 # convert to seconds t -= 719163 * 24 * 60 * 60 # convert to 1970 base + self._link = 0 else: tbuf = self.f.read(8) if len(tbuf) != 8: return (tusec,) = struct.unpack('>Q', tbuf) t = tusec * 1.0e-6 + self._link = tusec & 0x3 self._timestamp = t def post_message(self, msg): @@ -586,7 +678,7 @@ class periodic_event(object): def force(self): '''force immediate triggering''' self.last_time = 0 - + def trigger(self): '''return True if we should trigger now''' tnow = time.time() diff --git a/Tools/autotest/pymavlink/mavwp.py b/Tools/autotest/pymavlink/mavwp.py index 6fd1e10e2b..58bd0ec434 100644 --- a/Tools/autotest/pymavlink/mavwp.py +++ b/Tools/autotest/pymavlink/mavwp.py @@ -36,6 +36,15 @@ class MAVWPLoader(object): w.seq = self.count() 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): '''remove a waypoint''' self.wpoints.remove(w)