Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
3 contributors

Users who have contributed to this file

@tridge @peterbarker @geeksville
189 lines (165 sloc) 6.43 KB
#!/usr/bin/env python
'''arm/disarm command handling'''
import time, os
from MAVProxy.modules.lib import mp_module
from pymavlink import mavutil
arming_masks = {
"all" : 0x0001,
"baro" : 0x0002,
"compass" : 0x0004,
"gps" : 0x0008,
"ins" : 0x0010,
"params" : 0x0020,
"rc" : 0x0040,
"voltage" : 0x0080,
"battery" : 0x0100,
"airspeed": 0x0200,
"logging" : 0x0400,
"switch" : 0x0800,
"gps_config": 0x1000,
}
class ArmModule(mp_module.MPModule):
def __init__(self, mpstate):
super(ArmModule, self).__init__(mpstate, "arm", "arm/disarm handling", public=True)
checkables = "<" + "|".join(arming_masks.keys()) + ">"
self.add_command('arm', self.cmd_arm, 'arm motors', ['check ' + self.checkables(),
'uncheck ' + self.checkables(),
'list',
'throttle',
'safetyon',
'safetystatus',
'safetyoff'])
self.add_command('disarm', self.cmd_disarm, 'disarm motors')
self.was_armed = False
def checkables(self):
return "<" + "|".join(arming_masks.keys()) + ">"
def cmd_arm(self, args):
'''arm commands'''
usage = "usage: arm <check|uncheck|list|throttle|safetyon|safetyoff|safetystatus>"
if len(args) <= 0:
print(usage)
return
if args[0] == "check":
if (len(args) < 2):
print("usage: arm check " + self.checkables())
return
arming_mask = int(self.get_mav_param("ARMING_CHECK",0))
name = args[1].lower()
if name == 'all':
for name in arming_masks.keys():
arming_mask |= arming_masks[name]
elif name in arming_masks:
arming_mask |= arming_masks[name]
else:
print("unrecognized arm check:", name)
return
self.param_set("ARMING_CHECK", arming_mask)
return
if args[0] == "uncheck":
if (len(args) < 2):
print("usage: arm uncheck " + self.checkables())
return
arming_mask = int(self.get_mav_param("ARMING_CHECK",0))
name = args[1].lower()
if name == 'all':
arming_mask = 0
elif name in arming_masks:
arming_mask &= ~arming_masks[name]
else:
print("unrecognized arm check:", args[1])
return
self.param_set("ARMING_CHECK", arming_mask)
return
if args[0] == "list":
arming_mask = int(self.get_mav_param("ARMING_CHECK",0))
if arming_mask == 0:
print("NONE")
for name in arming_masks.keys():
if arming_masks[name] & arming_mask:
print(name)
return
if args[0] == "throttle":
p2 = 0
if len(args) == 2 and args[1] == 'force':
p2 = 2989
self.master.mav.command_long_send(
self.target_system, # target_system
self.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, # command
0, # confirmation
1, # param1 (1 to indicate arm)
p2, # param2 (all other params meaningless)
0, # param3
0, # param4
0, # param5
0, # param6
0) # param7
return
if args[0] == "safetyon":
self.master.mav.set_mode_send(self.target_system,
mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY,
1)
return
if args[0] == "safetystatus":
try:
sys_status = self.master.messages['SYS_STATUS']
except KeyError:
print("Unknown; no SYS_STATUS")
return
if sys_status.onboard_control_sensors_enabled & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS:
print("Safety is OFF (vehicle is dangerous)")
else:
print("Safety is ON (vehicle allegedly safe)")
return
if args[0] == "safetyoff":
self.master.mav.set_mode_send(self.target_system,
mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY,
0)
return
print(usage)
def cmd_disarm(self, args):
'''disarm motors'''
p2 = 0
if len(args) == 1 and args[0] == 'force':
p2 = 21196
self.master.mav.command_long_send(
self.target_system, # target_system
0,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, # command
0, # confirmation
0, # param1 (0 to indicate disarm)
p2, # param2 (all other params meaningless)
0, # param3
0, # param4
0, # param5
0, # param6
0) # param7
def all_checks_enabled(self):
''' returns true if the UAV is skipping any arming checks'''
arming_mask = int(self.get_mav_param("ARMING_CHECK",0))
if arming_mask == 1:
return True
for bit in arming_masks.values():
if not arming_mask & bit and bit != 1:
return False
return True
def mavlink_packet(self, m):
mtype = m.get_type()
if mtype == 'HEARTBEAT' and m.type != mavutil.mavlink.MAV_TYPE_GCS:
armed = self.master.motors_armed()
if armed != self.was_armed:
self.was_armed = armed
if armed and not self.all_checks_enabled():
self.say("Arming checks disabled")
ice_enable = self.get_mav_param('ICE_ENABLE', 0)
if ice_enable == 1:
rc = self.master.messages["RC_CHANNELS"]
v = self.mav_param.get('ICE_START_CHAN', None)
if v is None:
return
v = getattr(rc, 'chan%u_raw' % v)
if v <= 1300:
self.say("ICE Disabled")
def init(mpstate):
'''initialise module'''
return ArmModule(mpstate)
You can’t perform that action at this time.