Describe problem solved by this pull request

Previously USVs(boats) did not work in mission mode due to the lack of low level control in the rover position control. Due to the low yaw damping on water the boat model would just oscillate on the yaw axis without meaningful navigation.

Describe your solution

This PR adds a rate control loop on the rover position control module and enables way point navigation for boats.

• Added rate control of a rover using the common rate control library
  • Added support for Acro mode for rovers (includes https://github.com/PX4/PX4-Autopilot/pull/18317)
• Had to make commander consider boats as a rover vehicle type for waypoint acceptance (Fixes https://github.com/PX4/PX4-Autopilot/issues/19045)
• This PR needs a modification of the gazebo model for Improved control authority and directional stability of the gazebo boat model https://github.com/PX4/PX4-SITL_gazebo/pull/895

Test data / coverage

After PR

Tested in SITL and demonstrated successfully of following a survey pattern

make px4_sitl gazebo_boat

• Flight log: Log

Before PR

• Flight log: Log

Additional context

• This is a extended PR of https://github.com/PX4/PX4-Autopilot/pull/18317
• @junwoo091400 This should be useful for https://github.com/PX4/PX4-Autopilot/pull/19957
• Fixes https://github.com/PX4/PX4-Autopilot/issues/19045
• Depends on https://github.com/PX4/PX4-SITL_gazebo/pull/895

Actuator controls seem to be saturating a lot...

Retuned rate controller in last commit

Thanks for doing this work, I was wondering why my USV was completing missions at such low speeds and it looks like it could be affected by the position controller issues you fixed in this PR. See attached actuator outputs if that's useful to confirm I'm seeing a similar issue:

Note that I also modified the navigator to support RTL mode by disabling the RTL_STATE_CLIMB state, I was hoping to prep a PR myself but got sidetracked trying to understand why the boat executed the RTL mode at such low speeds.

What are we missing with this PR to be able to merge it? Would it be useful if I help field test it?

@jasta Thanks!

Note that I also modified the navigator to support RTL mode by disabling the RTL_STATE_CLIMB state, I was hoping to prep a PR myself but got sidetracked trying to understand why the boat executed the RTL mode at such low speeds.

This would be awesome, and probably a much needed feature!

What are we missing with this PR to be able to merge it? Would it be useful if I help field test it?

What is missing is a field test on a real boat. I would say this is probably better than the previous state, but it would be great if you could give it a run and see if something critical is missing from this pull request.

Thanks!

I'll definitely give this a try, but after some further digging I realized existing parameters may have been the culprit in my case. Default GND_THR_MAX is 0.3f which seems to line up exactly with what the logs are saying. There are several parameters here that are interacting in ways that are very counter intuitive as well, even when looking at the code. For example, GND_SPEED_TRIM doesn't appear to trim the ground speed, but rather sets the target speed if the current setpoint doesn't specify a cruising speed? So if you set a mission cruise speed of 10m/s, but GND_SPEED_TRIM is 3m/s, you'll still get 10m/s? It also only applies if _control_mode.flag_control_position_enabled is true even though RoverPositionControl still may be in charge of setting speed/velocity?

Also, GND_SPEED_THR_SC is for some reason being read by navigator/rtl.cpp? Seems like dead code to me though??? Return mode seems to always publish setpoints with cruising_speed=-1 though, so that should let RoverPositionControl takeover and use appropriate values.

Oh, and I also seemed to have stumbled upon a bug where if you start a mission with a set cruising speed then issue a return command, then resume the mission, the mission speed will be set to cruising_speed=-1 again so you'll get GND_SPEED_TRIM most likely instead. If you change the mission value, re-upload it, then start the mission, this will be once again the mission speed.

There are several parameters here that are interacting in ways that are very counter intuitive as well, even when looking at the code. For example, GND_SPEED_TRIM doesn't appear to trim the ground speed, but rather sets the target speed if the current setpoint doesn't specify a cruising speed? So if you set a mission cruise speed of 10m/s, but GND_SPEED_TRIM is 3m/s, you'll still get 10m/s? It also only applies if _control_mode.flag_control_position_enabled is true even though RoverPositionControl still may be in charge of setting speed/velocity?

@jasta If you are curious, you can have a look at the logic: https://github.com/PX4/PX4-Autopilot/blob/e780a583cd2f3c0c42978ff0e5c010f104443267/src/modules/rover_pos_control/RoverPositionControl.cpp#L240-L259

Although I would try to separate the discussion from this PR since it is not related.

Part of the reason is these things have been staying like this is because rovers were not being really tested/used in auto modes. Most rovers using PX4 were using offboard and direct actuator controls.

Therefore it would be great if we can address these problems one by one as we move forward. If you can address the points of the throttle control in a separate PR or at least post a issue would be helpful.

Also, GND_SPEED_THR_SC is for some reason being read by navigator/rtl.cpp? Seems like dead code to me though??? Return mode seems to always publish setpoints with cruising_speed=-1 though, so that should let RoverPositionControl takeover and use appropriate values.

Oh, and I also seemed to have stumbled upon a bug where if you start a mission with a set cruising speed then issue a return command, then resume the mission, the mission speed will be set to cruising_speed=-1 again so you'll get GND_SPEED_TRIM most likely instead. If you change the mission value, re-upload it, then start the mission, this will be once again the mission speed.

I think these are all related to the RTL use case, which was never really tested with boats and rovers. Therefore we would need to address these problems one by one - also not related to this PR and probably deserves a separate issue/PR

Ack'd, thanks for the quick info, I'm going to isolate all of these things and do some more field testing shortly. Hopefully I can provide an isolated report of the efficacy of just this diff once I correct for my baseline numbers with updated parameters.

@jasta Would you have an ETA for the testing you mentioned? Thanks!

Rebased on main

Rebased

I'm in need of this excellent work, but I've hit a bug - can anyone here shed light on it? https://github.com/PX4/PX4-Autopilot/issues/20374

@altmattr You would need this fix in the sitl_gazebo submodule: https://github.com/PX4/PX4-SITL_gazebo/pull/917

@Jaeyoung-Lim Thankyou, that fix has worked for me. I am still unable to get a mission going. Missing upload keeps being rejected due to altitude too low. I'll keep working on it!

@altmattr Try changing the altitude of your mission?

@Jaeyoung-Lim Will-do :) I'd really like to be able to test on real-life mission files though and they have no altitude because boats don't fly. I noticed another ticket about having the boat recognised as such so the pre-mission checks are appropriate and I want to experiment with that as well.

@altmattr so does this mean that it does work with the altitudes defined?

@Jaeyoung-Lim The mission is accepted and starts, but can't progress because. I presume this is because it never gains any altitude, so the takeoff phase is never completed.

@Jaeyoung-Lim It's actually more subtle than that. I can get it to do one step of a mission, but it just stops, doesn't try to get to the next waypoint. Presumably because it is not able to match the altitude of the waypoint

Dear @Jaeyoung-Lim How would the code need to be changed as to allow the use of the rate controllers when using offboard mode. I need to be able to send yaw-rate setpoints from offboard to a boat. Im not very familiar with the PX4 code stack so would really appreciate it if you could help me.

Rebased on main

Added an additional commit that I worked on, in sync with https://github.com/mavlink/qgroundcontrol/pull/10483 to enable nice rover PID tuning interface experience.

There is still a hacky part where I embed the throttle setpoint information in local position setpoint's acceleration member, but that can be removed after testing out the vehicle today (planned to do a mission at Zurich lake at Noon)

Regarding throttle calibration (It seems that velocity PID controller output to throttle mapping scalar tuning won't be so trivial), I noticed that QGC doesn't really have a good way to get actuator controls topic / thrust setpoint.

This is dealt in Multirotor Position Controller via vehicle_local_position_setpoint topic, which includes all sorts of (complicated!) information that gives an intel on what the velocity / position setpoints are, roughly.

Ideally, I think this should directly come from actuator_controls topic, or even better, from vehicle_thrust_setpoint, as it gives a clear normalized thrust setpoint information, but afaik this needs to be implemented in QGC, like done for vehicle local position for example: https://github.com/mavlink/qgroundcontrol/blob/2e41ac45c903450edb2a8d85e7f01e745b2d50c7/src/Vehicle/VehicleLocalPositionFactGroup.h#L15-L53

@Jaeyoung-Lim do you have any other ideas? Or, maybe we don't even need the throttle tuning?

Related comment above here: https://github.com/PX4/PX4-Autopilot/pull/20082#discussion_r1019743830

Testing recap

So today finally got to test the boat once again, and here's what happened:

1. The telemetry (SIYI transmitter) was good in the beginning, and the PID tuning plots could be drawn, using the QGC PR
2. Then the SET_NUM_INTERVALS or some MAVLink command created by QGC to request certain MAVLink data (e.g. Vehicle rates / atittude) failed (error shown on screen)
3. After that, telemetry never really recovered to it's original state (even after transmitter / receiver / FMU reboot), and so tuning was not performed
4. Only Manual & Acro modes were tested.

Findings

1. In Manual mode, the actuator output is (throttle - 0.5), whereas in acro mode, actuator output is (throttle). Since the TX was spring-loaded self-centering throttle type, this was a weird user experience (had to lower the throttle for acro mode):
   
2. Boat was constantly suffering the GPS Speed Accuracy too low Preflight failure message, which is caused by sensor_gps/s_variance_m_s (was moving around 0.5) being over the EKF2_REQ_SACC parameter value (was 0.5). This is likely due to the RunCam which was next to the GPS, but it was quite annoying :thinking:. Wondering if the threshold limit is too tight (0.5 m/s). Seems like there's a related issue: https://github.com/PX4/PX4-Autopilot/issues/6275

Pictures

Log

https://logs.px4.io/plot_app?log=b2389301-2c21-48cd-bb34-bd0def7492a5

Btw, the rate control seemed to work quite well!

Btw, the rate control seemed to work quite well!

@junwoo091400 Thanks for testing!

There seems to be quite some steady state error on the rate controller performance. Espeicially the offset seems to be more apparent during high angular rates.

Did you try to tune the feedforward gains?

@junwoo091400 How's this coming along?

@junwoo091400 I tried rebasing the branch, but it seems like you have pushed temporary changes to this branch(includes comments like "hack" or debug outputs), so I have removed them temporarily for now. If you think there are parts that we should salvage, please let me know.

Otherwise, I will try to fix the issues with distinguishing differential vs Ackermann steering vehicles through a parameter and the temporary changes you made shouldn't be required anymore

Let me ask real quick. This PR introduces only 3 commits because the rest have been merged into the main branch? What is needed for this to be merged? Do you want to refactor the rover module and for example split attitude and rate controllers?

@not7cd Yes, this introduces a low level controller controlling rates. Before this PR, the position controller was controlling directly the actuators

As I understand, moving this towards control allocator is outside of the scope of this PR?