Resetting of setpoints is currently called in the control loop, at whatever frequency it runs at (10 Hz now). That means that when e.g. using depth hold, diving, then releasing the dive button, the new depth setpoint can be up to 100 ms old, which causes the ROV to move back toward the surface slightly. Should reset setpoints when receiving vortex_msgs/PropulsionCommand instead, probably.
Resetting of setpoints is currently called in the control loop, at whatever frequency it runs at (10 Hz now). That means that when e.g. using depth hold, diving, then releasing the dive button, the new depth setpoint can be up to 100 ms old, which causes the ROV to move back toward the surface slightly. Should reset setpoints when receiving
vortex_msgs/PropulsionCommand
instead, probably.