Moving a BlueROV with pymavlink

Hi y’all, I am trying guide a blue rov sub to do simple waypoint missions using pymavlink. I am trying to use the set position command to move the ROV, but I have not gotten the motors to spin. Also I am receive position information from the DVL attached to the robot, and sending back to the ROV to update its local and gps position, but I am unsure if the autopilot is receiving these. Do y’all have know what I am doing wrong, or a better way to do it? Also is there a good way to see if the ROV’s computers are receiving the messages I am sending?, Thanks for your help

Here is the code I am using to send the set position command

self.robot.secondary.mav.set_position_target_local_ned_send(
			int(1e3 * (time.time() - self.robot.bootTime)),	# ms since boot
			self.robot.master.target_system, 
			self.robot.master.target_component,
			coordinate_frame=mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED,
			type_mask=( 
				#mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
				#mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
				#mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
				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), 
			x=xd, y=yd, z=zd, 						# (x, y, z) [m] positive is NED
			vx=0, vy=0, vz=0, 						# velocities in NED frame [m/s] (not used)
			afx=0, afy=0, afz=0, 					# accelerations in NED frame [m/s/s]
			yaw=0, yaw_rate=0						# heading [rad] and yaw rate [rad/s] N is 0
    	)

Here is the code I am using to send the position information

           self.master.mav.gps_input_send(
                self.lib.GetTickCount64(),  # Timestamp (micros since boot or Unix epoch)
                0,  # ID of the GPS for multiple GPS inputs
                # Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum).
                # All other fields must be provided.
                0,  # GPS time (milliseconds from start of GPS week)
                0,  # GPS week number
                (mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_HORIZ |
                mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT |
                mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY|
                mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_HDOP|
                mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VDOP),
                5,  # 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
                int(self.pos_GPS['lat']*1E7),  # Latitude (WGS84), in degrees * 1E7
                int(self.pos_GPS['lon']*1E7),  # Longitude (WGS84), in degrees * 1E7
                0,  # Altitude (AMSL, not WGS84), in m (positive for up)
                1,  # GPS HDOP horizontal dilution of position in m
                1,  # GPS VDOP vertical dilution of position in m
                0,  # GPS velocity in m/s in NORTH direction in earth-fixed NED frame
                0,  # GPS velocity in m/s in EAST direction in earth-fixed NED frame
                0,  # GPS velocity in m/s in DOWN direction in earth-fixed NED frame
                0,  # GPS speed accuracy in m/s
                0,  # GPS horizontal accuracy in m
                0,  # GPS vertical accuracy in m
                20, # Number of satellites visible.
            )
            self.master.mav.local_position_ned_send(
                self.lib.GetTickCount64(),
                self.pos['x'],
                self.pos['y'],
                self.pos['z'],
                self.pos['vx'],
                self.pos['vy'],
                self.pos['vz'],
            )

Hi @Ellak, welcome to the forum :slight_smile:

Are you arming it before you try to command a movement?

The vehicle also needs a stable position estimate before it can perform position-related tasks. If your position messages aren’t frequent or consistent/precise enough then it may fail to obtain a position lock.

There are a couple of different ways:

  1. Check the MAVLink telemetry messages, for either direct responses/relays, or indirect results (e.g. GLOBAL_POSITION_INT reports the autopilot’s internal position estimate)
    • These can be checked live (during operation), or afterwards with a telemetry log file
  2. Check the autopilot’s internal DataFlash logs for relevant changes
1 Like

Thank you for your feed back.

I am arming it and setting the mode using these commands

  raub.master.mav.command_long_send(
    raub.master.target_system, raub.master.target_component,
    mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
    217, 1, 0, 0, 0, 0, 0)
    
    
    raub.master.mav.command_long_send(
        raub.master.target_system,
        raub.master.target_component,
        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
        0,
        1, 0, 0, 0, 0, 0, 0)
type or paste code here

I was able to get the motors to spin once I put it in the water, however I am still unsure of what the ROV is doing and how to control it. I tried using manual control to spin the motors using the code shown below, but I am unable to get it to move in a direction and unable to get the motors to stop spinning,
I am using the documentation shown here

self.robot.master.mav.manual_control_send(
		self.robot.master.target_system,
		500, #x positive value
		500, #y negativevalue
		500, #z negative value
		500, #rotation
		0 #Button
		)

I also tried using the send target position command using the code I showed earlier, but instead of moving towards a point it just holds its heading. Is this a sign that it is not reciving position messages, or is it something else?

Hi @Ellak -
Do you have a position sensor like a DVL or GPS (surface only usage) installed? Without one of these providing a position estimate, only Manual, Stabilize and Alt_hold modes are possible.

It is a good idea to setup and drive your vehicle normally (via Cockpit or QGround control), before trying to do so programmatically, just to make sure all your thruster directions are setup correctly and the motion sensors calibrated. …

Yes, I am using a waterlinked A50 dvl for positioning, and am receiving accurate dead reckoning position estimates from it via pymavlink and BlueOS

Previously, I have also driven the ROV around with an xbox controller, and I did not notice anything inconsistent.

Also in its current stabilized mode the ROV will use the motors to return back to its initial heading if pushed away from it. At the moment I am confused why it enters this mode when I send it a position command, and set the mode to position/manual mode. Do you know anything that would cause the ROV to enter this mode or how this mode is typically activated?

Hi @Ellak -
In manual mode, no automatic stabilization occurs - the ROV reacts only to pilot joystick inputs.

In any other mode - stabilize, depth hold, guided, auto, or position hold, ArduSub works to maintain orientation.

Position hold mode would react the way you describe - manual mode would not. When working with the DVL, Manual mode is not the mode to take advantage of your position information with!

I found my error. Something about the way I was changing mode was not working, I switched to the set_mode command and that fixed the issue.

However now I have another issue, when I send the blue rov the following command it will move downward 10cm, when I believe it should be moving laterally 10 cm.

self.robot.master.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message(10, self.robot.master.target_system,
                      self.robot.master.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, int(0b110111111110), .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0))

When I send it this command which should also move it laterally in the y direction it runs the four vertical thrusters in full power. I have checked that all of my thrusters are correctly set up for the Heavy configuration which I am running.

self.robot.master.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message(10, self.robot.master.target_system,
                      self.robot.master.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, int(0b110111111110), .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0))

I was just able to replicated the behavior of the second send target position command in qground control. I believe that my tank was to shallow and so the rov was trying to reach a depth not allowed by the tank. I also solved it in qground control by setting the initial, take off and way point altitude to be 0 ft, and rebooting the ROV to make the EKF origin was at this same altitude.

I was under the impression that the type mask in the send target position command stated to ignore the z axis, but the ROV is responding likes its attempting to move in the Z direction. Would anyone know why this is?

Also I checked the DVL integration tutorial and the waterlinked DVL is set up correctly.

The two code snippets you included are identical, with a target set to 10cm North of wherever you set the origin to. It’s also hard to interpret how the message is supposed to be configured when you’ve used a magic number for the POSITION_TARGET_TYPEMASK bitmask, rather than spelling it out.

If you want to set a position target relative to the vehicle’s current position (instead of the autopilot’s global EKF origin, which can only be set once after each vehicle startup) you should be using MAV_FRAME_LOCAL_OFFSET_NED.

Note also that