Altitude hold and depth hold in pymavlink

Blue Robotics Software
,
1

Hello, I’m trying to implement altitude hold on BlueROV using altitude measurement from a DVL. I followed some code snippets in ping1d_mavlink_driver and pymavlink, but neither worked. I taped the pressure sensor and the vehicle sank once I enabled alt_hold mode. Is the altitude hold not supported anymore in this commit?

So I have a few questions with regard to altitude and depth hold modes.

  1. What’s the easiest way to implement altitude hold assuming altitude is available? I could write a PID controller for that but it will be great if I can leverage existing functions in ArduSub.
  2. Is there a way to change the desired depth value in pymavlink during depth hold mode without using a joystick?
  3. When I changed the depth during depth hold mode, I noticed that the vehicle jerked a little bit as you can see from the figure. As a result, the vehicle didn’t hold the exact depth when I released the depth knob.

2019-08-02-13-24
2019-08-02-13-241671×281 46.3 KB

Regards,
Jinkun

2

Hello Jinkun,

Yes, that commit disabled it as we didn’t think it was good enough.

You can revert that commit and try to improve the behavior.

I’m afraid that not right now. SET_POSITION_TARGET_GLOBAL_INT and SET_POSITION_TARGET_LOCAL_NED Should be able to set a depth target, but they are only supported in Guided or Auto modes right now. It should be possible to add support for this message to Depth Hold too.

We recently worked on the depth hold controller to mitigate a behavior just like that. Are you running ArduSub 3.5.4?

3

Hi Willian,

Thanks for the reply. We are using ArduSub 3.5.3. It seems the code to disable altitude hold hasn’t been committed yet in 3.5.3. So the altitude should have worked. Can you verify the following code is the correct?

    bridge.conn.mav.distance_sensor_send(
        int(time * 1000),
        min_altitude * 100,
        max_altitude * 100,
        altitude * 100,
        mavutils.mavlink.MAV_DISTANCE_SENSOR_UNKNOWN,
        1,
        mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270,
        0
    )

Should I update to 3.5.4 to use the improved depth hold controller?

4

Hi,
If you move to 3.5.4 you will be unable to use these messages at all.
you could build 3.5.4 yourself reverting the commit and try to to get it working.

5

Hello again,

I’ve been looking into this issue. You should be able to set depth target with this gist in the sub 3.6 branch after cherry-picking this commit.

I’m not sure this is the best approach, so I can’t guarantee we’ll get this merged.

1 Like
6

Thanks! If it works, I don’t need altitude hold any more.

7

I tried to build ArduSub 3.6, but I got this error,

The board target px4-v2 has been removed from ArduPilot with the removal of NuttX support and HAL_PX4.

Please use a replacement build as follows:

 px4-v2     Use Pixhawk1 build

Then I changed px4-v2 to Pixhawk1 but I couldn’t find .px4 file in the build folder.

So how can I build sub 3.6 with your commit? Can I use stable branch?

8

Try:
./waf configure --board Pixhawk1
./waf sub
use the file ardusub.apj from the folder ardupilot/build/Pixhawk1/bin
The files were recently renamed from .px4 to .apj. you may need to rename it to flash using the companion interface.

9

Hi, we use your script Gnist for 3.6 Beta
This works, but the new set target must be within an meter of current depth…
Why ?

Example:
At surface we set target depth -5 meter, the sub will dive to -1
If I run the script again still with -5 meter as target the new depth would be -2 meters.
Now if we set new target to -2.5 meters the new depth will be -2.5 meter (because the new target is within one meter of current depth)

We use a Ardusub 4.0.1 with custom frame compiled, no pilot input or RC override involved.

How to get past this limitation ?

Kato

10

Hi @kg9316.

I was unable to replicate this, do you mind sharing a log or the exact steps you took?

11

2020-09-08 09-19-55.tlog (396.8 KB)

Sorry, due to bad weather I was not able to take the sub to the sea and give you real life logs.
The attached log is from a test done in the office today, submerged in a tub.

I used Missionplanner as groundcontrol station, this makes it easy to monitor “targetalt”

09.30.35 I send “arm”, “depth mode” and target depth -5 meter
targetdepth ramps down to -1 meter (command was -5 meters)
a few seconds later it ramps back to 0 (this going back to 0 doesn’t happen in a real environment)
09.30.55 I send -5 meter command again

In a real mission the sub responds to commands >1 meter (from current depth) as 1m
Smaller commands work as the should.

12

Hi @kg9316.

Any chance you could find the dataflash log for this dive? I’m particularly interested in seeing how CTUN.DAlt and CTUN.Alt behaved. I couldn’t find any SET_POSITION_GLOBAL_INT commands neither see any depth changes at all…

I intend to have an ROV in the pool later today and I’ll run some more tests, too.

13

log_1_UnknownDate.bin (1.3 MB)

Had to keep the file small, slow file transfer via mavlink…
The small 2" Enclosure we use for Pixhawk, NanoPi Duo2 and Fathom X (with 60V power via tether) is oil filled. Troublesome to remove the flash card.

Commands sendt:
Arm
Depth Hold
-5 meters
-3 meters
Manual
Disarm

I can see the events “bottomed” and “not_bottomed”, and CTUN.DAtl stops at -1.17 meter (guess this is measured depth when command is issued, -1 meter)

In a few days I could do a real dive so you also can compare with CTUN.Atl and the behavior with depth changes being limited to ±1 meter of measured depth.

Kato

14

Hi @kg9316,

From the log it looks like your rov is not diving at all. That is triggering the “bottomed” event as it assumes it can’t go down because it hit the bottom.

Can you verify that your depth readings are good? I’m not sure if the oil can affect them.

15

Dept readings are good.
As mentioned earlier, everything works like it should out of the office.
The only problem is that depth setpoint only can be adjusted ±1 meter from measured depth
The provided logs from today also shows that.
Depth set at -5m/-3m shows displays as -1 in CTUN.DAtl

I will provide real logs from a dive in a few days

Kato

16

Right!

My mistake. I managed to replicate the issue here. Something seems to be limiting the DAlt distance. Here it happened with different values (12~14 m) but it is the same behavior.

17

This seems to be the issue.

The position controller has a built-in “leash” and that is limiting the maximum distance of the altitude setpoint.
You can increase PILOT_SPEED_* parameters and decrease PILOT_ACCEL_Z to make that distance larger (these parameters are only used in depth hold and position-enabled modes, so there should be no adverse effects)

Since you already have a custom frame, I assume you have the environment set up to build customs versions of the code. You could try changing the calc_leash_length_z() implementation.

1 Like
18

Great !, Thanks
I will try that.