T200 python manual control error

Blue Robotics Software ArduSub
,
16

Hello Eliot, we changed the code but it stucked in the
if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE: continue
part.

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import argparse
import cv2
import cv2 as CV #if you are using python2 you must add it, otherwise you'll gel warning.
import time
import threading
from pymavlink import mavutil
import sys

mode = 'MANUAL'



master =  mavutil.mavlink_connection( # aracin baglantisi
            '/dev/ttyACM0',
            baud=115200)

heartbeat_requirement = mavutil.periodic_event(1) # 1 Hz heartbeat
def beat_heart_if_necessary():
    if heartbeat_requirement.trigger():
        master.mav.heartbeat_send(
            mavutil.mavlink.MAV_TYPE_GCS,
            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
            0,0,0)



pilot_input_requirement = mavutil.periodic_event(1/3) # 0.33 Hz


def set_rc_channel_pwm(id, pwm=1500if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE:
        continue):

    if id < 1:
        print("Channel does not exist.")
        return


    if id < 9: # communication with ardusub
        rc_channel_values = [65535 for _ in range(8)]
        rc_channel_values[id - 1] = pwm
        master.mav.rc_channels_override_send(
            master.target_system,
            master.target_component,
            *rc_channel_values)

if mode not in master.mode_mapping():
    print('Unknown mode : {}'.format(mode))
    print('Try:', list(master.mode_mapping().keys()))
    exit(1)

# Get mode ID
mode_id = master.mode_mapping()[mode]
# 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)

"""master.mav.set_mode_send(
    master.target_system,
    mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
    mode_id)"""


# Check ACK
ack = False
while not ack:
    # 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

def stop_motion():
    """ Set all motion axes to stopped. """
    for channel in range(1, 7):
        set_rc_channel_pwm(channel, 1500)


print("motors stopping")
stop_motion()
print("motors stopped")
master.arducopter_arm()
print("arming motors")
master.motors_armed_wait()
print("motors armed!")

def simple_move(channel, pwm, period):
    end = time.time() + period
    pilot_input_requirement.force()
    while time.time() < end:
        beat_heart_if_necessary()
        if pilot_input_requirement.trigger():
            set_rc_channel_pwm(channel, pwm)
        time.sleep(0.1) # sleep for 100ms



print('moving forwards for 5 seconds')
simple_move(5, 1600, 5)


print('complete - stopping forwards motion')
set_rc_channel_pwm(5, 1500)

master.mav.command_long_send(
    master.target_system,
    master.target_component,
    mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,	#disarm
    0,
    0, 0, 0, 0, 0, 0, 0)
master.motors_disarmed_wait()
  • We are using Pixhawk 2.4.8, is this version compatible with Ardusub?
    Or do you think this could be the reason of all these problems?

Thanks in advance.