Guided mode with pymavlink

Blue Robotics Software ArduSub
,
1

Hi!

We have a BlueROV2 with a Nortek Nucleus 1000 DVL and are running ArduSub v4.5.3 and BlueOS v1.4.2. We are aiming to use pymavlink to reach goal positions in guided mode. The set control parameters are set following Norteks guide. We are able to set the ROV to guided mode. However, we are not able to get the ROV to react to the set_position_target_local_ned and set_attitude_target messages we are sending. In manual/stabilize/alt_hold mode, however, we are able to control the ROV with RC channels override messages.

Perhaps someone can shed a light on what we are doing wrong?

Code excerpts:


# Called regularly
def goal_callback(self, msg):
    self.master.mav.set_position_target_local_ned_send(
        int(1e3 * (time.time() - self.boot_time)),
        self.master.target_system, self.master.target_component,
        mavutil.mavlink.MAV_FRAME_LOCAL_NED,
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE |
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
        mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
        msg.pose.position.x, msg.pose.position.y, msg.pose.position.z,
        0, 0, 0, # vx, vy, vz
        0, 0, 0, # ax, ay, az
        0, 0 # yaw, yaw_rate
    )

    self.master.mav.set_attitude_target_send(
        int(1e3 * (time.time() - self.boot_time)),
        self.master.target_system, self.master.target_component,
        mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE,
        [msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z], 
        0, 0, 0, 0 # roll rate, pitch rate, yaw rate, thrust
    )

# Also sending RC channels override at 10 Hz
def send_rc_channels(self):
    if self.mode == "GUIDED":
        rc_channels = [65535 for _ in range(18)] # Ignore all channels
        self.master.mav.rc_channels_override_send(
            self.master.target_system, self.master.target_component,
            *rc_channels
        )
    else: 
        # Other logic implemented for other modes
2

Hi @Herman,

A couple of questions:

  1. Does guided mode work as expected when not using programmatic control?
  2. Assuming you don’t have a GNSS system integrated as well, are you setting the GNSS origin before trying to use guided mode?
3

We found the culprit. We were not sending set_position_target_local_ned fast enough. By streaming at a higher rate (25 Hz in our case), the ROV responded as intended.

We also dropped sending rc_channels during guided mode, as we suspected that this might interfere. This, however, caused the ROV to quickly disarm even if we were streaming set_position_target_local_ned at 25 Hz. We then resorted to disabling FS_PILOT_TIMEOUTduring guided mode. Though probably not the best solution, this solved our problem for now.

We have not yet tested guided mode with QGC, but aim to do this. Yes, we are setting the GNSS origin.

3 Likes
4

Hi, we are trying to achieve the same goal where we wanted to control the rov with dvl with python code using pymavlink. We were able to change modes to guided and pos_hold in blueos cockpit. but we werent able to do it with pymavlink. for achieving guided or pos_hold do we need to inject dvl’s positional data through pymavlink simultenously? if so how can we do that?

5

Hi @Flatline, welcome to the forum :slight_smile:

Cockpit doesn’t do this, so if it can enable position holding then you should be able to with Pymavlink as well, as long as you’re setting the origin (as discussed above), and either sending regular heartbeats and pilot inputs (via MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages), or have disabled the both relevant failsafes.

What kind of behaviour are you experiencing, and are you receiving any error messages? It may help if you can share the code you’re running and your current parameters.