Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: update copies of pymavlink in autotest

This commit is contained in:
Andrew Tridgell 2011-12-29 08:29:37 +11:00
parent 074fd31506
commit ddab189e42
4 changed files with 398 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/autotest/pymavlink/mavextra.py
View File

@ -32,3 +32,7 @@ def TrueHeading(SERVO_OUTPUT_RAW):
def kmh(mps):
'''convert m/s to Km/h'''
return mps*3.6
def altitude(press_abs, ground_press=955.0, ground_temp=30):
'''calculate barometric altitude'''
return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001

151
Tools/autotest/pymavlink/mavlink.py
View File

@ -156,6 +156,18 @@ MAV_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values
# will be only accepted if in pre-flight mode.
MAV_CMD_ENUM_END = 246 #
# 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_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 #
# MAV_DATA_STREAM
MAV_DATA_STREAM_ALL = 0 # Enable all data streams
MAV_DATA_STREAM_RAW_SENSORS = 1 # Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
@ -188,6 +200,9 @@ MAVLINK_MSG_ID_DIGICAM_CONTROL = 155
MAVLINK_MSG_ID_MOUNT_CONFIGURE = 156
MAVLINK_MSG_ID_MOUNT_CONTROL = 157
MAVLINK_MSG_ID_MOUNT_STATUS = 158
MAVLINK_MSG_ID_FENCE_POINT = 160
MAVLINK_MSG_ID_FENCE_FETCH_POINT = 161
MAVLINK_MSG_ID_FENCE_STATUS = 162
MAVLINK_MSG_ID_HEARTBEAT = 0
MAVLINK_MSG_ID_BOOT = 1
MAVLINK_MSG_ID_SYSTEM_TIME = 2
@ -423,6 +438,54 @@ class MAVLink_mount_status_message(MAVLink_message):
def pack(self, mav):
return MAVLink_message.pack(self, mav, 233, struct.pack('>BBiii', self.target_system, self.target_component, self.pointing_a, self.pointing_b, self.pointing_c))
class MAVLink_fence_point_message(MAVLink_message):
'''
A fence point. Used to set a point when from GCS
-> MAV. Also used to return a point from MAV -> GCS
'''
def __init__(self, target_system, target_component, idx, count, lat, lng):
MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT')
self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng']
self.target_system = target_system
self.target_component = target_component
self.idx = idx
self.count = count
self.lat = lat
self.lng = lng
def pack(self, mav):
return MAVLink_message.pack(self, mav, 18, struct.pack('>BBBBff', self.target_system, self.target_component, self.idx, self.count, self.lat, self.lng))
class MAVLink_fence_fetch_point_message(MAVLink_message):
'''
Request a current fence point from MAV
'''
def __init__(self, target_system, target_component, idx):
MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_FETCH_POINT, 'FENCE_FETCH_POINT')
self._fieldnames = ['target_system', 'target_component', 'idx']
self.target_system = target_system
self.target_component = target_component
self.idx = idx
def pack(self, mav):
return MAVLink_message.pack(self, mav, 68, struct.pack('>BBB', self.target_system, self.target_component, self.idx))
class MAVLink_fence_status_message(MAVLink_message):
'''
Status of geo-fencing. Sent in extended status
stream when fencing enabled
'''
def __init__(self, breach_status, breach_count, breach_type, breach_time):
MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_STATUS, 'FENCE_STATUS')
self._fieldnames = ['breach_status', 'breach_count', 'breach_type', 'breach_time']
self.breach_status = breach_status
self.breach_count = breach_count
self.breach_type = breach_type
self.breach_time = breach_time
def pack(self, mav):
return MAVLink_message.pack(self, mav, 136, struct.pack('>BHBI', self.breach_status, self.breach_count, self.breach_type, self.breach_time))
class MAVLink_heartbeat_message(MAVLink_message):
'''
The heartbeat message shows that a system is present and
@ -1718,6 +1781,9 @@ mavlink_map = {
MAVLINK_MSG_ID_MOUNT_CONFIGURE : ( '>BBBBBB', MAVLink_mount_configure_message, [0, 1, 2, 3, 4, 5], 19 ),
MAVLINK_MSG_ID_MOUNT_CONTROL : ( '>BBiiiB', MAVLink_mount_control_message, [0, 1, 2, 3, 4, 5], 97 ),
MAVLINK_MSG_ID_MOUNT_STATUS : ( '>BBiii', MAVLink_mount_status_message, [0, 1, 2, 3, 4], 233 ),
MAVLINK_MSG_ID_FENCE_POINT : ( '>BBBBff', MAVLink_fence_point_message, [0, 1, 2, 3, 4, 5], 18 ),
MAVLINK_MSG_ID_FENCE_FETCH_POINT : ( '>BBB', MAVLink_fence_fetch_point_message, [0, 1, 2], 68 ),
MAVLINK_MSG_ID_FENCE_STATUS : ( '>BHBI', MAVLink_fence_status_message, [0, 1, 2, 3], 136 ),
MAVLINK_MSG_ID_HEARTBEAT : ( '>BBB', MAVLink_heartbeat_message, [0, 1, 2], 72 ),
MAVLINK_MSG_ID_BOOT : ( '>I', MAVLink_boot_message, [0], 39 ),
MAVLINK_MSG_ID_SYSTEM_TIME : ( '>Q', MAVLink_system_time_message, [0], 190 ),
@ -2267,6 +2333,90 @@ 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.
Also used to return a point from MAV -> GCS
target_system : System ID (uint8_t)
target_component : Component ID (uint8_t)
idx : point index (first point is 1, 0 is for return point) (uint8_t)
count : total number of points (for sanity checking) (uint8_t)
lat : Latitude of point (float)
lng : Longitude of point (float)
'''
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.
Also used to return a point from MAV -> GCS
target_system : System ID (uint8_t)
target_component : Component ID (uint8_t)
idx : point index (first point is 1, 0 is for return point) (uint8_t)
count : total number of points (for sanity checking) (uint8_t)
lat : Latitude of point (float)
lng : Longitude of point (float)
'''
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
target_system : System ID (uint8_t)
target_component : Component ID (uint8_t)
idx : point index (first point is 1, 0 is for return point) (uint8_t)
'''
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
target_system : System ID (uint8_t)
target_component : Component ID (uint8_t)
idx : point index (first point is 1, 0 is for return point) (uint8_t)
'''
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
fencing enabled
breach_status : 0 if currently inside fence, 1 if outside (uint8_t)
breach_count : number of fence breaches (uint16_t)
breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t)
breach_time : time of last breach in milliseconds since boot (uint32_t)
'''
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
fencing enabled
breach_status : 0 if currently inside fence, 1 if outside (uint8_t)
breach_count : number of fence breaches (uint16_t)
breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t)
breach_time : time of last breach in milliseconds since boot (uint32_t)
'''
return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time))
def heartbeat_encode(self, type, autopilot, mavlink_version=2):
'''
The heartbeat message shows that a system is present and responding.
@ -4578,3 +4728,4 @@ class MAVLink(object):
'''
return self.send(self.debug_encode(ind, value))

2
Tools/autotest/pymavlink/mavutil.py
View File

@ -359,8 +359,6 @@ class mavlogfile(mavfile):
self.planner_format = planner_format
self.notimestamps = notimestamps
self._two64 = math.pow(2.0, 63)
if planner_format is None and self.filename.endswith(".tlog"):
self.planner_format = True
mode = 'rb'
if self.writeable:
if append:

65
Tools/autotest/pymavlink/mavwp.py
View File

@ -2,6 +2,11 @@
module for loading/saving waypoints
'''
import os
if os.getenv('MAVLINK10'):
import mavlinkv10 as mavlink
else:
import mavlink
class MAVWPError(Exception):
@ -133,3 +138,63 @@ class MAVWPLoader(object):
w.param1, w.param2, w.param3, w.param4,
w.x, w.y, w.z, w.autocontinue))
f.close()
class MAVFenceError(Exception):
'''MAVLink fence error class'''
def __init__(self, msg):
Exception.__init__(self, msg)
self.message = msg
class MAVFenceLoader(object):
'''MAVLink geo-fence loader'''
def __init__(self, target_system=0, target_component=0):
self.points = []
self.target_system = target_system
self.target_component = target_component
def count(self):
'''return number of points'''
return len(self.points)
def point(self, i):
'''return a point'''
return self.points[i]
def add(self, p):
'''add a point'''
self.points.append(p)
def clear(self):
'''clear point list'''
self.points = []
def load(self, filename):
'''load points from a file.
returns number of points loaded'''
f = open(filename, mode='r')
self.clear()
for line in f:
if line.startswith('#'):
continue
line = line.strip()
if not line:
continue
a = line.split()
if len(a) != 2:
raise MAVFenceError("invalid fence point line: %s" % line)
p = mavlink.MAVLink_fence_point_message(self.target_system, self.target_component,
self.count(), 0, float(a[0]), float(a[1]))
self.add(p)
f.close()
for i in range(self.count()):
self.points[i].count = self.count()
return len(self.points)
def save(self, filename):
'''save fence points to a file'''
f = open(filename, mode='w')
for p in self.points:
f.write("%f\t%f\n" % (p.lat, p.lng))
f.close()