Sets attitude for bluerov2 using pymavlink

attitudeControl.py

# Import mavutil
from pymavlink import mavutil
from pymavlink.quaternion import QuaternionBase
import math
import time
import sys
ALT_HOLD_MODE = 2

def is_armed():
    try:
        return bool(master.wait_heartbeat().base_mode & 0b10000000)
    except:
        return False

def mode_is(mode):
    try:
        return bool(master.wait_heartbeat().custom_mode == mode)
    except:
        return False

def set_target_depth(depth):
    master.mav.set_position_target_global_int_send(
        0,     
        0, 0,   
        mavutil.mavlink.MAV_FRAME_GLOBAL_INT, # frame
        0b0000111111111000,
        0,0, depth,
        0 , 0 , 0 , # x , y , z velocity in m/ s ( not used )
        0 , 0 , 0 , # x , y , z acceleration ( not supported yet , ignored in GCS Mavlink )
        0 , 0 ) # yaw , yawrate ( not supported yet , ignored in GCS Mavlink )

def set_target_attitude(roll, pitch, yaw, control_yaw=True):
    bitmask = (1<<6 | 1<<3)  if control_yaw else 1<<6

    master.mav.set_attitude_target_send(
        0,     
        0, 0,   
        bitmask,
        QuaternionBase([math.radians(roll), math.radians(pitch), math.radians(yaw)]), # -> attitude quaternion (w, x, y, z | zero-rotation is 1, 0, 0, 0)
        0, #roll rate
        0, #pitch rate
        0, 0)    # yaw rate, thrust 

# Create the connection
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
# Wait a heartbeat before sending commands
print(master.wait_heartbeat())


while not is_armed():
    master.arducopter_arm()

while not mode_is(ALT_HOLD_MODE):
    master.set_mode('ALT_HOLD')


pitch = yaw = roll = 0
for i in range(500): 
    yaw += 10
    set_target_attitude(roll, pitch, yaw) 
    time.sleep(1)

Hi William,

I am having some trouble getting is_armed() to work. I started breaking it down into smaller pieces realized that

print (bool(master.wait_heartbeat().base_mode & 0b10000000))

returns true even when the drone is disarmed and I never sent an arm command. I was expecting it to return false. Am I mistaken or is something not working? I noticed that the result is also True regardless of what arm state the flight controller is in.

returns true even when the drone is disarmed and I never sent an arm command. I was expecting it to return false. Am I mistaken or is something not working? I noticed that the result is also True regardless of what arm state the flight controller is in.

I can't replicate this here. is this the case for sub 4.0?

Hi Willian!

I have a few questions on how attitude control works and was wondering if you might have some insight. I am currently using Sub 4.1 in the Alt_hold mode, controlled with a companion computer using pymavlink. I am attempting to set both the depth and the compass heading.

1. Is it true that the attitude target (and depth target) needs to be set periodically (every ~second) to ensure that the pilot input failsafe doesn't override the setting? This seems to be working for the depth setting.
2. I set attitude using set_attitude_target_send (every .3s). However, the nav_controller_output.nav_bearing is not reporting that heading as I would expect it to. It is fluctuating (particularly when I turn the pixhawk) and settling on a number that is different from what I am setting, and very different from the current heading, seeming to be almost off by 180 degrees (though not consistently). When the sub is not armed, the nav_bearing messages tracks the current heading as I would expect. Is there a different message I should be reading to get the target yaw? Have I set the heading correctly (shoud I be using the "yaw" option in set_position_target_global_int_send)? Also is the set_attitude_target_send and nav_bearing in the global(earth) frame of reference or the sub's frame of reference? The other thought I had is maybe the nav_bearing is not actually the target heading, but is instead the interim target heading that the sub uses to calculate the rate of turning -- if so, is there a way to get the actual target heading?
   Happy to share code if that is helpful.

Update: I have noticed that if the target heading and the current heading are too different from eachother, the target heading is no longer held at the setpoint. If the pixhawk is upside-down (which maybe is why it seemed to be 180 degrees off?) and if they are within ~20 degrees of eachother, the set point is held. So maybe this is by design?