Skip to content

Instantly share code, notes, and snippets.

@krawal19
Last active August 11, 2020 18:16
Show Gist options
  • Save krawal19/6359b081d47fb18c0a10c6c0cab6c099 to your computer and use it in GitHub Desktop.
Save krawal19/6359b081d47fb18c0a10c6c0cab6c099 to your computer and use it in GitHub Desktop.
#!/usr/bin/env python
from pymavlink import mavutil
import time
import sys
radio_port = '/dev/ttyUSB2'
radio_baud = 57600
master = mavutil.mavserial(radio_port, radio_baud)
# for ground control
gcs = mavutil.mavudp('127.0.0.1:16789', input=False)
# Heartbeat
master.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (master.target_system, master.target_system))
# Choose a mode
mode = 'OFFBOARD'
# Check if mode is available
if mode not in master.mode_mapping():
print('Unknown mode : {}'.format(mode))
print('Try:', list(master.mode_mapping().keys()))
sys.exit(1)
# Get mode ID
mode_id = master.mode_mapping()[mode]
print('mode_id', mode_id)
# Set new mode
# master.mav.command_long_send(
# master.target_system, master.target_component,
# mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
# 0, mode_id, 0, 0, 0, 0, 0) or:
# master.set_mode(mode_id) or:
master.mav.set_mode_send(
master.target_system,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
mode_id)
while True:
# Wait for ACK command
ack_msg = master.recv_match(type='COMMAND_ACK', blocking=True)
ack_msg = ack_msg.to_dict()
# Check if command in the same in `set_mode`
if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE:
continue
# Print the ACK result !
print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description)
break
-------------------------------ERROR-------------------------------------------------------
Heartbeat from system (system 1 component 1)
('mode_id', (29, 6, 0))
Traceback (most recent call last):
File "test.py", line 40, in <module>
mode_id)
File "/home/.local/lib/python2.7/site-packages/pymavlink/dialects/v20/ardupilotmega.py", line 16131, in set_mode_send
return self.send(self.set_mode_encode(target_system, base_mode, custom_mode), force_mavlink1=force_mavlink1)
File "/home/.local/lib/python2.7/site-packages/pymavlink/dialects/v20/ardupilotmega.py", line 13783, in send
buf = mavmsg.pack(self, force_mavlink1=force_mavlink1)
File "/home/.local/lib/python2.7/site-packages/pymavlink/dialects/v20/ardupilotmega.py", line 7656, in pack
return MAVLink_message.pack(self, mav, 89, struct.pack('<IBB', self.custom_mode, self.target_system, self.base_mode), force_mavlink1=force_mavlink1)
struct.error: cannot convert argument to integer
-------------------------------ERROR-------------------------------------------------------
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment