Custom Flight Mode in C++: Position Control with a Fixed Pitch Angle

Blue Robotics Software
1

Hi everyone,

I am currently developing a 6DoF ROV for ship hull inspection. The vehicle is equipped with a front-mounted DVL.

The Goal:
I need the ROV to maintain a fixed pitch angle (e.g., pitched down 45 degrees to face the ship hull) while simultaneously performing velocity/position control (X for distance to the wall based on DVL range, Y for lateral movement, Z for depth).

The Problem:
Currently, our control logic runs on a BlueOS Extension using the standard GUIDED mode via MAVLink. However, we’ve hit a limitation: in ArduSub, we cannot mix SET_POSITION_TARGET_LOCAL_NED (for position/velocity control) and SET_ATTITUDE_TARGET (to force the 45-degree pitch). The position controller automatically overrides our custom pitch commands.

Rather than relying on workarounds like sending RC_CHANNELS_OVERRIDE in STABILIZE mode from our extension, we want to build a robust, production-ready solution. Our intention is to implement this natively in the ArduSub firmware (C++) to take advantage of the 400Hz control loop, ensure better safety/failsafes, and keep the architecture clean.

The Proposed Solution:
I am planning to write a custom flight mode (e.g., ModeInspect) in the ArduSub C++ codebase, possibly by copying and modifying the existing mode_guided.cpp or mode_poshold.cpp.

My Questions:

  1. Is creating a new custom mode the recommended approach for this specific use case?
  2. To maintain a fixed pitch, can I simply overwrite the pitch target sent to the attitude controller inside ModeInspect::run() like this?
    attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 4500.0f, target_yaw_cd, true);
  3. Since the vehicle is pitched at 45 degrees, if I want to move horizontally relative to the Earth/wall, can I still rely on pos_control, or would it be better to bypass pos_control for the X/Y axes and feed the thrust directly to the motors (motors->set_forward(), motors->set_lateral()) based on my custom DVL distance PID?

Any advice, code snippets, or pointers to specific files/functions would be greatly appreciated!

Thank you,