Last active
August 11, 2020 18:16
-
-
Save krawal19/6359b081d47fb18c0a10c6c0cab6c099 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/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