T200 python manual control error

[MokhtarYahia]

Hello , we are trying to manage and run our T200 thrusters manually by those codes but there seems to be an error. We are using pixhawk and sending codes by jetson using the pymavlink commands to the t200 thrusters and we are expecting to have full manual control them, but no matter what pwm we pick in the last line set_rc_channel_pwm(5,1650) the speed never changes

#!/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


mode = 'MANUAL'


#Try: ['MANUAL', 'CIRCLE', 'STABILIZE', 'TRAINING', 'ACRO', 'FBWA', 'FBWB', 'CRUISE', 'AUTOTUNE', 'AUTO', 'RTL', 'LOITER', 'TAKEOFF', 'AVOID_ADSB', 'GUIDED', 'INITIALISING', 'QSTABILIZE', 'QHOVER', 'QLOITER', 'QLAND', 'QRTL', 'QAUTOTUNE', 'QACRO', 'THERMAL']


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

#master = "mavutil.mavlink_connection('udpin:192.168.2.2:14550')" #if it is going to be controlled on computer
def set_rc_channel_pwm(id, pwm=1500):

    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) or:
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

time.sleep(3)
master.mav.command_long_send(
    master.target_system,
    master.target_component,
    mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
    0,
    1, 0, 0, 0, 0, 0, 0)

while(True):
	set_rc_channel_pwm(5,1650)
	time.sleep(7)

[EliotBR (Eliot)]

Hi @MokhtarYahia,

These comments (and the threads they’re from) may be relevant:

@EliotBR

I did some testing today and found that the motion should be explicitly set to stopped before arming, otherwise arming can sometimes resume the last thruster state from the last time the vehicle was armed.

@EliotBR

Note also the various failsafes that are built in. MAVLink connections expect to receive a heartbeat message at least every second (if not received the communication may be stopped), and when you’re controlling the vehicle it’s subject to the heartbeat failsafe , and rc override commands are subject to the pilot input failsafe and to RC_OVERRIDE_TIME .

You also seem to send your initial move command without checking if arming was successful. You may want to use master.motors_armed_wait() to wait for arming to complete.

I would also recommend adding a master.wait_heartbeat() call once you make the initial connection, so Pymavlink can determine which MAVLink version to communicate with (it should be MAVLink 2, and you’ll likely need to change your set_rc_channel_pwm to handle the extra 10 values that introduces, like in our example).

---

By the way, I’ve edited your post to have the code in a code block, which formats it and makes it easier to read, or copy for testing. You can read more about that in the Formatting a Post/Comment section of the “How to Use the Forums” post

[MokhtarYahia]

Hello, we are trying to control the motors speed and spinning time. We couldn’t make it yet. We are using pixhawk and sending codes by jetson using the pymavlink commands to the t200 thrusters. Our code is attached below.

#!/usr/bin/env python
# -- coding: utf-8 --
import numpy as np
import argparse
import cv2
import cv2 as CV 
import time
import threading
from pymavlink import mavutil

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

master.wait_heartbeat()
print("connected")


mode = 'MANUAL'


#Try: ['MANUAL', 'CIRCLE', 'STABILIZE', 'TRAINING', 'ACRO', 'FBWA', 'FBWB', 'CRUISE', 'AUTOTUNE', 'AUTO', 'RTL', 'LOITER', 'TAKEOFF', 'AVOID_ADSB', 'GUIDED', 'INITIALISING', 'QSTABILIZE', 'QHOVER', 'QLOITER', 'QLAND', 'QRTL', 'QAUTOTUNE', 'QACRO', 'THERMAL']


def set_rc_channel_pwm(id, pwm=1500):

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


    if id < 9:  
        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) or:
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

time.sleep(3)
master.mav.command_long_send(
    master.target_system,
    master.target_component,
    mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
    0,
    1, 0, 0, 0, 0, 0, 0)
print("arming motors1")
master.motors_armed_wait()
print("armed motors")
while(True):
	set_rc_channel_pwm(5, 1700)
	print("foward done1")
	time.sleep(5)
	set_rc_channel_pwm(5, 1500)
	print("motors stopped")	
	time.sleep(4)
	master.mav.command_long_send(
    	master.target_system,
    	master.target_component,
    	mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
    	0,
    	1, 0, 0, 0, 0, 0, 0)
	print("motors arming")
	master.motors_armed_wait()
	print("armed")
	set_rc_channel_pwm(5, 1550)
	print("forward done2")
	time.sleep(7)

[EliotBR (Eliot)]

Hi @MokhtarYahia,

This new code doesn’t address the points I raised in my comment above. You’re not avoiding the failsafes or timeouts (unless you’ve turned those off with parameters, in which case you should specify what parameters you’ve changed), and I’d also recommend setting the motion to stopped before arming.

[MokhtarYahia]

Thank you, Eliot, for your reply, I really appreciate your help and it was quite useful

I have tried for multiple hours to understand how I could avoid a failsafe or timeouts by adding a heartbeat, but I couldn’t figure out how the protocol could be done, therefore I had tried to manually disable the failsafe parameters totally from Qgroundcontrol, and right now the motor spins, but it still behaves abnormally

apparently, it stops at 1100 instead of 1500, and the bigger the PWM value the faster it spins, even at 1700. it spins faster in and also in the same direction as 1100, I have also troubleshot all the hardware on Qgroundcontrol and it behaved normally over there, but not when I tried to use the pymavlink

so if possible

• Could you please walk me through how I could modify the code to send a regulated heartbeat so that I could avoid a failsafe, or if could redirect me to a thread that has it explained in the simplest details

• I currently only have a single thruster connected to the pixhawk, will that automatically trigger a failsafe , since not all the thrusters are connected according the frame type that I am using " Vectored frame "

• What do you think might be the problem with PWM speed, why isn’t it behaving normally as;
  1100 full reverse
  1500 “stop”
  1900" full forward "

I would really appreciate any kind of help or assistance given

[EliotBR (Eliot)]

@MokhtarYahia

apparently, it stops at 1100 instead of 1500, and the bigger the PWM value the faster it spins, even at 1700. it spins faster in and also in the same direction as 1100

• What do you think might be the problem with PWM speed, why isn’t it behaving normally as;
  1100 full reverse
  1500 “stop”
  1900" full forward "

Which ESCs are you using? Our Basic ESC is bi-directional, but many other ESCs aren’t (drone propellers generally only need to push air in one direction). You’ll need to determine if your ESCs can be re-programmed to be bi-directional, and if not you’ll need different ESCs.

@MokhtarYahia

Could you please walk me through how I could modify the code to send a regulated heartbeat so that I could avoid a failsafe, or if could redirect me to a thread that has it explained in the simplest details

The vehicle needs to receive a heartbeat at ~1Hz. This comment provides an example of how to set up a loop using periodic_events to determine when a heartbeat should be sent.

Alternatively, both the comments I quoted from in my original comment link to this full program example, which sends a regular heartbeat in a separate thread. If you’re not already experienced with threads then it’s recommended to use the periodic_event approach, as covered in the Notes section at the bottom.

As per the second quote in my first comment though, heartbeats aren’t the only failsafe:

@EliotBR

rc override commands are subject to the pilot input failsafe and to RC_OVERRIDE_TIME.

You’ll need to make sure any motion commands are sent more frequently than your vehicle’s FS_PILOT_TIMEOUT and RC_OVERRIDE_TIME parameters (you may need to send the same command several times), or change those parameters to disable the pilot input failsafe and stop rc overrides from timing out.

@MokhtarYahia

I currently only have a single thruster connected to the pixhawk, will that automatically trigger a failsafe , since not all the thrusters are connected according the frame type that I am using " Vectored frame "

No failsafe will be triggered, because PWM is one-directional so the Pixhawk has no way of knowing which motors are actually connected - it just sends control signals based on whatever frame you’ve told it you’re using.

[MokhtarYahia]

• I am indeed currently using blue robotics basic ESC

• As I have understood I should disable both RC_OVERRIDE_TIME & FS_PILOT_TIMEOUT from Qgroudcontrol

• So just by adding the code snippet at the beginning of my code which is changing the mode & Sending RC (Joystick), a heartbeat would be automatically initiated? , such as

import numpy as np
import argparse
import cv2
import cv2 as CV 
import time
import threading
from pymavlink import mavutil

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

master.wait_heartbeat()

heartbeat = mavutil.periodic_event(1) # 1 Hz heartbeat
display = mavutil.periodic_event(5) # 5 Hz output (every 0.2 seconds)

start = time.time()
duration = 160 # seconds

while time.time() - start < duration:
    if heartbeat.trigger():
        master.mav.heartbeat_send(
            mavutil.mavlink.MAV_TYPE_GCS,
            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
            0,0,0)
    
    att = master.recv_match(type='ATTITUDE', blocking=True)
     

* **MY CODE changing the mode & Send RC (Joystick)**


mode = 'MANUAL'


#Try: ['MANUAL', 'CIRCLE', 'STABILIZE', 'TRAINING', 'ACRO', 'FBWA', 'FBWB', 'CRUISE', 'AUTOTUNE', 'AUTO', 'RTL', 'LOITER', 'TAKEOFF', 'AVOID_ADSB', 'GUIDED', 'INITIALISING', 'QSTABILIZE', 'QHOVER', 'QLOITER', 'QLAND', 'QRTL', 'QAUTOTUNE', 'QACRO', 'THERMAL']


def set_rc_channel_pwm(id, pwm=1500):

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


    if id < 9:  
        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) or:
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

time.sleep(3)
master.mav.command_long_send(
    master.target_system,
    master.target_component,
    mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
    0,
    1, 0, 0, 0, 0, 0, 0)
print("arming motors1")
master.motors_armed_wait()
print("armed motors")
while(True):
	set_rc_channel_pwm(5, 1700)
	print("foward done1")
	time.sleep(7)

[/quote]

[MokhtarYahia]

@EliotBR

No failsafe will be triggered because PWM is one-directional, so the Pixhawk has no way of knowing which motors are connected - it just sends control signals based on whatever frame you’ve told it you’re using.

Thank you for your clarification

[EliotBR (Eliot)]

@MokhtarYahia

• I am indeed currently using blue robotics basic ESC

Hmm, ok. Which firmware is on your Pixhawk? ArduSub has the motors set up for bi-directional control, but if you’re running ArduCopter or something else then they’ll be configured differently. You may need to flash ArduSub on using QGroundControl

@MokhtarYahia

• As I have understood I should disable both RC_OVERRIDE_TIME & FS_PILOT_TIMEOUT from Qgroudcontrol

That depends. The failsafes and timeout exist for a reason, but if you’re ok with accepting the risk that your vehicle might drive away from you after you send it a single command then disabling those does at least simplify the control commands. Alternatively you can keep them there and just make sure you’re sending motion control commands regularly when you want the motion to continue.

@MokhtarYahia

So just by adding the code snippet at the beginning of my code which is changing the mode & Sending RC (Joystick), a heartbeat would be automatically initiated?

No, adding a snippet at the start is only possible if you’re using a separate thread. The revised code you’ve posted sends a regular heartbeat and receives ATTITUDE messages for 160 seconds, then continues with your previous code for controlling the motion, no longer sending any heartbeats.

With the direct heartbeat method you can’t sleep at any point (or do other actions between heartbeats) for longer than 1 second. You may want to do something like

import time
from pymavlink import mavutil

master = mavutil.mavlink_connection(...)
master.wait_heartbeat()

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(channel_id, pwm=1500):
    ...

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

... # set mode to MANUAL

stop_motion()
print("motion set to 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 7 seconds')
simple_move(5, 1700, 7)
print('complete - stopping forwards motion')
set_rc_channel_pwm(5, 1500)

master.arducopter_disarm()
print("disarming motors")
master.motors_disarmed_wait()
print("motors disarmed!")

You can string together a series of simple_move commands, but for more complex/dynamically determined movements you’ll likely need to implement some kind of movements queue, or some other approach that isn’t time-based.

[MokhtarYahia]

Hello Eliot, thanks for your helping.
When we try to run a forward motion we get some behavior like this:
-it runs
-it stops
-it changes direction
Despite we send only move forward and stop command.
For you to understand better we upload video to the YouTube.
This is the slow motion version:

T200 thrusters spining slow motion version

This is normal version:

T200 thrusters spinning normal version

Here is our code:

#!/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=1500):

    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) or:
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


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()

master.reboot_autopilot()

[EliotBR (Eliot)]

A few questions:

1. Have you confirmed that the Pixhawk is running ArduSub firmware?
2. Do you have a correspondence between the thruster movements and the print statements? It would be useful to understand how the thruster is moving at each point in the code.
3. Is there a reason you’re rebooting the autopilot at the end?

In addition, your current code doesn’t explicitly stop all motion before arming, as I recommended in my first comment. I’ve added a simple way of doing that to the example code in my previous comment, which I’d suggest you try using.

[MokhtarYahia]

1- The pix-hawk is indeed running ardusub firmware v4.03 on a pixhawk 2.4.8

I get and error once Qground control I am not sure if it might be related

2- here is the sequence order

-The program prints (motors armed!)
-The program prints (‘moving forwards for 5 seconds’)
-The thruster spins clockwise
-The thruster stops
-The thruster spins counter clock
-The program prints (‘complete - stopping forwards motion’)
-The thruster completely stops

3- There is no actual reason for rebooting , I only though that rebooting it might help reset the state of the pix-hawk for the next trial , removing reboot line doesn’t change the behavior

4- I Have tried to stop all the thrusters right before arming , but the same behavior still remains the same

here is the failsafe settings

and the latest code with all your notes

#!/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=1500):

    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) or:
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)

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()

[EliotBR (Eliot)]

@MokhtarYahia

I get and error once Qground control I am not sure if it might be related

That’s a parameter mismatch between your QGC and ArduSub versions. It shouldn’t be causing any particular issues beyond QGC not being able to control everything it wants/expects to.

@MokhtarYahia

-The program prints (‘moving forwards for 5 seconds’)
-The thruster spins clockwise
-The thruster stops
-The thruster spins counter clock
-The program prints (‘complete - stopping forwards motion’)

That seems really odd, unless the vehicle isn’t actually in manual mode. Your code has an acknowledgement check when you change mode, but you didn’t include that in the sequence of print outputs - did you just forget to include it, or are you running different code to what you’ve posted?

@MokhtarYahia

3- There is no actual reason for rebooting , I only though that rebooting it might help reset the state of the pix-hawk for the next trial , removing reboot line doesn’t change the behavior

Fair enough.

@MokhtarYahia

4- I Have tried to stop all the thrusters right before arming , but the same behavior still remains the same

Ok then, although in your latest code snippet you’ve defined stop_motion but you don’t actually call it anywhere.

@MokhtarYahia

here is the failsafe settings

The failsafes should be able to be set to their normal values if you’re sending regular heartbeats and pilot control - is there a reason you’ve still got them turned off? Regardless, the failsafes can only raise warnings or stop the motors - they can’t change the motor directions.

[MokhtarYahia]

@EliotBR

That seems really odd, unless the vehicle isn’t actually in manual mode. Your code has an acknowledgement check when you change mode, but you didn’t include that in the sequence of print outputs - did you just forget to include it, or are you running different code to what you’ve posted?

Actually, our output was as the following

@EliotBR

Ok then, although in your latest code snippet you’ve defined stop_motion but you don’t actually call it anywhere.

We did recall it , such as the following ,

#!/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=1500):

    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) or:
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()

@EliotBR

The failsafes should be able to be set to their normal values if you’re sending regular heartbeats and pilot control - is there a reason you’ve still got them turned off? Regardless, the failsafes can only raise warnings or stop the motors - they can’t change the motor directions.

We enabled the failsafe also, and the result didn’t change.

[EliotBR (Eliot)]

I’m afraid I’m not sure what’s causing the issues.

Are you able to try doing master.set_mode(mode_id) instead of the current master.mav.set_mode_send(...) call?

[MokhtarYahia]

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.

[EliotBR (Eliot)]

@MokhtarYahia

if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE: continue

Looking more into this, the SET_MODE message is deprecated, and shouldn’t be used anymore. The new approach uses the MAV_CMD_DO_SET_MODE command instead (which is what is sent by master.set_mode(mode_id)), so the acknowledged command check should be for mavutil.mavlink.MAV_CMD_DO_SET_MODE.

I’m unsure whether this is actually what’s causing your issue(s), but without looking further into ArduSub’s codebase my understanding is it’s at least possible.

@MokhtarYahia

• 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?

Pixhawk 2.4.8 is known to use some cheap hardware components, so may not be super reliable, but this performance issue seems to be in software (manual mode shouldn’t allow the outputs to change without the commanded motion changing), so I don’t expect that’s the cause of the problem here.

That said, if the set_mode change doesn’t fix it then I’m pretty much out of other ideas…