ALT_HOLD and STABILIZE mode don't work properly

[EliotBR (Eliot)]

Hi @daveycleaning,

In short, calibrate the sensors (including the compass), and try to run automatic motor reversal detection.

---

For some (detailed) context and troubleshooting steps, ArduSub uses feedback control, which requires the autopilot to have all of:

1. A sense of the vehicle’s current state, as it relates to the variable(s) being controlled
   • As @KaracaEmirhan mentioned in their post, calibrating the sensors is relevant for ensuring the state estimates are valid and reasonable, although they forgot to mention/calibrate the compass (magnetometer), which can be important for specifically the vertical axis rotation estimate (together with the gyroscope)
   • Sometimes even well-calibrated sensors can “lie” because of external influences, like if a magnetometer is near a strongly magnetic object or structure, or an incoming storm causes a pressure change that skews the depth estimate
     • This is often difficult to adjust or account for, especially if it is caused by something in the environment that the vehicle needs to be operating around / close to
       • Moving away from such influences can improve calibrations, and regularly re-calibrating pressure when the vehicle is at the surface can help to account for air pressure changes
     • Large magnetic fluctuations from battery or motor wires located close to the flight controller board can be partially calibrated for, but it’s preferable to keep those wires routed further away if at all possible, or twisted and shielded to reduce external impacts/interference
   • Integrated state values (like determining a position from the velocity measurements of a DVL) inherently gain error over time, often at significant rates due to measurement frequency and resolution limits
     • Managing this requires occasional absolute sensor values as a reference (to correct for the error accumulated through repeated relative measurements)
2. A target to change its state towards
   • The target can be defined by the user directly, via joystick inputs (or radio, if enabled and set up)
     • If the vehicle’s problematic movement occurs even in MANUAL mode, then the problem may be with the user input
       • You may have incorrectly mapped input ranges (which is reasonably common for the ArduSub Z axis range)
       • Your joystick springs may be degraded such that it doesn’t fully return to zero, which can generally be calibrated for using a dead-zone in your control station software (like Cockpit or QGroundControl)
   • The target can also come (partly) from the definition of a flight mode
     • ALT_HOLD tries to maintain the latest depth the user moved the vehicle to
     • Both ALT_HOLD and STABILIZE try to maintain the commanded heading rate and attitude angles (e.g. when zero input is provided, it will maintain the current yaw angle (about the vertical axis), and “level” the pitch and roll)
     • POS_HOLD, GUIDED, and other position-based flight modes (including AUTO, in autonomous missions) move to and/or maintain positions (and sometimes angles and/or rates) the user specifies in advance
3. A mechanism for changing its state (generally via controllable motor outputs)
   • The available outputs need to be well-defined, and correctly mapped, so the control algorithms can command a change and have that output occur
     • The major limitations to this are the minimum and maximum control authority available for each motion axis:
       • Motors may not move when commanded to move a very small amount, which feedback control algorithms are well-suited to handling, but may do so with some unwanted oscillation/vibration
       • Thrusters are not limitless, so if your vehicle is fighting a water current that is faster than the thrusters can push it, it may still move (less quickly) away from to the target, or (in the case of a tether snag) it may pull hard and not move anywhere
         • This does not seem to be the described problem, it’s just relevant troubleshooting advice
       • Not all motion axes are available on all vehicles - e.g. a standard BlueROV2 does not support pitch control, in which case commanding a pitch change has no effect
     • If one or more thrusters is reversed compared to the expected thrust directions, or the motors are connected to the wrong output (e.g. thruster 5 is plugged in to the output assigned as motor 3), the motion response will not match the expectations and the vehicle will spiral out of control
       • This is one of the most common failure types, and the automatic motor reversal detection feature that @FlyingManta mentioned is intended to help with it
         • Running it, with the vehicle in the water, allows the autopilot to try to move each of its outputs and confirm the vehicle moves as expected
           • If an output is reversed, it detects this, and re-maps it for you automatically
           • If an output mapped to the incorrect motor, or disconnected entirely, it fails with an error about that output
           • There are also false-negatives, where the vehicle’s mass/buoyancy distribution do not match the parameters the feature was designed with, in which case the motors may be assigned correctly but the vehicle does not move the way the autopilot “expects”
             • In these cases, you can run a manual motor test instead, and check that each thruster spins, in the direction it is supposed to, when you command it to
   • The output hardware also needs to be working
     • If an output pin is broken/fried on the flight controller board it will not be able to send signals to connected devices, so you may need to assign that pin’s output function (SERVOn_FUNCTION) to a different output instead
     • If an ESC and the flight controller have mismatched timing hardware (including if there is excessive capacitance or inductance on the signal wire), the pulses sent to the ESC may be received as longer or shorter than intended, which can result in an offset to how each commanded PWM pulse-duration is interpreted
       • The most common symptom of this is in the motor(s) moving even when the autopilot is disarmed - it’s unlikely this would be a problem only in specific modes
       • It can be compensated for in software by “trimming” the outputs

I’ll try to get this kind of troubleshooting content added to the ArduSub documentation when I have the time to…