Home        Store        Docs        Blog

Desired depth and heading commands in a manual mode (e.g. depth hold mode)

Hi - Is there anyway to send desired depth and heading commands while the ROV is in a manual mode such as DEPTH HOLD mode or STABILIZE mode via MAVProxy?

I am integrating a BlueROV2 with a larger ASV, and I would like to send depth and heading commands to the ROV from the ASV. I can do this in the GUIDED mode via mavlink_msg_set_position_target_local_ned message or mavlink_msg_command_long (MAV_CMD_CONDITION_YAW) message. But as soon as I put the ROV to the GUIDED mode (with a faked underwater GPS input) it does some weird movements and we can’t afford that when the ROV is attached to another robot. So I am wondering if we can send depth and heading set points when the ROV is in the DEPTH HOLD mode? Or do you have any other suggestions?

For depth hold, you can use set_attitude_target_no_gps to select the desired attitude of the vehicle, for the depth, you’ll need to use rc_channels_override_send with thrust set points until you hit the desired position.

This is probably because your GPS information is not a real system that can give position error information to the ROV, without a real position information (or something close to it), the vehicle will do weird things.

Hi @patrickelectric -
Thanks a lot for your response. I’m currently creating a MOOS interface app for BlueROV2.
A couple of questions:

For depth hold, you can use set_attitude_target_no_gps to select the desired attitude of the vehicle

  1. How do you send set_attitude_target_no_gps commands? I can use below piece of code to send mavlink_msg_set_attitude_target messages, but it seems this only works for GUIDED MODE. How do you send set_attitude_target_no_gps commands?

     if(key == "ROV_DESIRED_HEADING") {
           m_rov_desired_heading_deg = dval;
           // Euler angles to Quaternion
           double cy = cos(m_rov_desired_heading_deg*M_PI/180* 0.5);
           double sy = sin(m_rov_desired_heading_deg*M_PI/180 * 0.5);
           double cp = cos(m_rov_desired_pitch_deg*M_PI/180 * 0.5);
           double sp = sin(m_rov_desired_pitch_deg*M_PI/180 * 0.5);
           double cr = cos(m_rov_desired_roll_deg*M_PI/180 * 0.5);
           double sr = sin(m_rov_desired_roll_deg*M_PI/180 * 0.5);
           double q_w = cy * cp * cr + sy * sp * sr;
           double q_x = cy * cp * sr - sy * sp * cr;
           double q_y = sy * cp * sr + cy * sp * cr;
           double q_z = sy * cp * cr - cy * sp * sr;
           float q[4];
           q[0] = q_w;
           q[1] = q_x;
           q[2] = q_y;
           q[3] = q_z;
           time_boot_ms = (uint32_t) (get_time_usec()/1000);
           uint16_t length2 = mavlink_msg_set_attitude_target_pack(system_id, component_id, &m_mavlink_msg,
                                                                   time_boot_ms, target_system, target_component, SET_ATTITUDE_NO_RATE, q, 0, 0, 0, 0);
           m_udp_client->send(&m_mavlink_msg);
       }
    

for the depth, you’ll need to use rc_channels_override_send with thrust set points until you hit the desired position.

  1. It seems like I need to control individual thrusters if I use mavlink_msg_rc_channels_override. Can’t I use mavlink_msg_manual_control messages in DEPTH HOLD mode for this purpose? That way I just have to give a thrust in the z direction until it reaches the desired depth. See the below code:
if(key == "ROV_THRUST_Z") {
          double m_rov_thrust_z = dval;
          uint16_t length2 = mavlink_msg_manual_control_pack(system_id,1,&m_mavlink_msg,target_system,0,0,m_rov_thrust_z,0,1);
          m_udp_client->send(&m_mavlink_msg);
      }

Thanks,
Supun.

set_attitude_target_no_gps Is only possible in ALT HOLD mode.
Your codes appears to be fine, the only problem is the mode that you are using.
Take a look in the patch that add such feature here, it’ll probably help.

Yes, you can, both messages are valid.

Thanks @patrickelectric. I tried mavlink_msg_set_attitude_target message according to my above code in ALT HOLD mode, but the ROV did not respond.

My BlueROV-MOOS interface apps run in the ASV’s computer and ideal would be just sending desired depth and heading commands to the ROV. I can run a simple feedback controller in the topside (ASV) computer but sometimes data through the mavlink_msg_vfr_hud message has a large delay (>3 seconds). Is this usual?

Hello,

The SET_ATTITUDE_TARGET message is currently not supported in ArduSub Stable, only in Beta.

VFR_HUD is the message used by QGC to show the ROV heading, this delay seems unusual.