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.

Ardusub loiter like mode
7

Hi, we were able to change mode to guided and position hold with pymavlink but when we tried to setpoint in the code for example, go to 1m in x axis(in guided mode) it starts to behave abruptly going diagonally without stopping. we had to kill the power to turn the thrusters off.
Here’s the snippet of our code:

def set_positional_target_distance(position):
    master.mav.set_position_target_local_ned_send(
        int(1e3 * (time.time() - boot_time)),
        master.target_system, 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,
        position[0], position[1],position[2],
        0, 0, 0, # vx, vy, vz
        0, 0, 0, # ax, ay, az
        0, 0 # yaw, yaw_rate
    )
def main():
    arm_vehicle()

    # set_custom_mode('POSHOLD')
    guided_mode()

    stop()
    set_positional_target_distance([1,0,0])
    stop()
    disarm()
8

Nice!

Which way was the vehicle facing? If you’re using a North-East-Down (NED) frame then the X direction is North, which could be diagonally depending on your current vehicle heading. If you want X to be forwards (using the vehicle’s attitude) then you’ll need to use a Forward-Right-Down (FRD) frame (e.g. MAV_FRAME_BODY_FRD).

Your shared code doesn’t make it clear how it’s trying to stop the vehicle - what does your stop function do? If it’s immediate then the vehicle should barely move at all, because it should get the disarm command very soon after entering guided mode.

To confirm, have you managed to control the ROV in guided mode with Cockpit, or just enter the mode?