[Project tracker] Rover control improvements

[junwoo091400]

About

Current Rover Position Control's logic has a number of weird logic and is generally messy. This is a project tracker to cleanup & improve rover control.

Related branch / PRs

While testing integration of Racing Boat with PX4, I have been refactoring the Rover code quite a bit in
https://github.com/junwoo091400/PX4-Autopilot/tree/boat_bigstorm. And my plan is to squash these commits and do a refactor PR with improvements applied from this Issue tracker.

WIP PR is up here: #19957

TODOs

• Make Rover position / velocity / attitude / rate controller cascade each other
• Remove legacy fixed-wing related code in rover controller
• [ ]

Problems

Non-cascaded controller

Currently in Rover Position Control module, each controller (position / velocity / attitude / rate) runs on it's own setpoint -> actuator output pipeline. This in my opinion is bad since we are disregarding all the PID values tuned for the rate controller and we are directly setting the throttle & yaw control in high-level (position control for example). Here are some examples:

Position control:

PX4-Autopilot/src/modules/rover_pos_control/RoverPositionControl.cpp

Lines 297 to 304 in 8ca28f3

	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
	}
	}
	break;
	case STOPPING: {
	_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
	_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;

Velocity control:

PX4-Autopilot/src/modules/rover_pos_control/RoverPositionControl.cpp

Lines 364 to 368 in 8ca28f3

	const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0));
	float control_effort = desired_theta / _param_max_turn_angle.get();
	control_effort = math::constrain(control_effort, -1.0f, 1.0f);
	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;

What I think should be the case is a cascaded controller where each level of controller gives setpoint to the lower level controller, and in the end the rate controller should be the sole part of the code that sets the actuator output.

https://docs.px4.io/main/en/flight_stack/controller_diagrams.html#multicopter-control-architecture

@Jaeyoung-Lim what do you think?

Lots of if / else conditions for checking which 'mode' we are in

There seems to be a lot of if / else statements to manually distinguish and guide code paths so that we can control position / velocity / attitude etc in a correct fashion, like this:

PX4-Autopilot/src/modules/rover_pos_control/RoverPositionControl.cpp

Lines 487 to 499 in 8ca28f3

	// Respond to an attitude update and run the attitude controller if enabled
	if (_control_mode.flag_control_attitude_enabled
	&& !_control_mode.flag_control_position_enabled
	&& !_control_mode.flag_control_velocity_enabled) {
	control_attitude(_vehicle_att, _att_sp);
	}
	/* Only publish if any of the proper modes are enabled */
	if (_control_mode.flag_control_velocity_enabled ||
	_control_mode.flag_control_attitude_enabled ||
	_control_mode.flag_control_position_enabled ||
	_control_mode.flag_control_manual_enabled) {

But I think this is quite hard to read. I haven't checked how the multicopter deals with this, but could there be a better way to do this?

[Jaeyoung-Lim]

Currently in Rover Position Control module, each controller (position / velocity / attitude / rate) runs on it's own setpoint -> actuator output pipeline.

@junwoo091400 The L1 control logic is based on acceleration control and therefore a cascaded control structure (where position is controlled by velocity setpoints) are not relevant for using an L1 guidance logic.

The velocity controller was added in #13225 to support offboard velocity setpoints, and was not intended to be used for position control.

This in my opinion is bad since we are disregarding all the PID values tuned for the rate controller and we are directly setting the throttle & yaw control in high-level (position control for example).

This I agree, and is simply because we are missing a lower level rate controller. This was why I tried to add #18317 .

I would claim that the biggest missing piece for boats are the low level rate controller. This is because unlike rovers boats have a low damping on yaw and therefore actuator inputs do not map directly to yaw rates. For normal rovers this assumption is not too bad and that is why it is was working okay with the current formulation on rovers. IMHO, we should prioritize on getting the rate controller for rovers (boats) reliable before we add any improvements for the position controller.

There seems to be a lot of if / else statements to manually distinguish and guide code paths so that we can control position / velocity / attitude etc in a correct fashion, like this

This is how it is done for multicopter rate control, attitude control position control, fixed wing position control and rate control. Feel free to improve this, but I would not relate this to improving rover. This is related to how commander communicates control modes to other controllers.

[junwoo091400]

This is because unlike rovers boats have a low damping on yaw and therefore actuator inputs do not map directly to yaw rates

What does this mean? I am lacking some understanding here I think 🤔

Also, regarding improving the rate control, how would taking into account the velocity come into play? I am thinking of a case for example when a user tries to do yaw rate control while stationary, and it would saturate the I component, and then would apply this constant when the user then throttles up and starts moving the boat.

[Jaeyoung-Lim]

This is because unlike rovers boats have a low damping on yaw and therefore actuator inputs do not map directly to yaw rates
What does this mean? I am lacking some understanding here I think

If you apply some yaw torque to the vehicle, rovers normally converge to a specific yaw rate very quickly since the ground damping forces are quire significant. For a boat, since water provides much less significant damping effect on the vehicle the vehicle might have more complicated dynamics on yaw

Also, regarding improving the rate control, how would taking into account the velocity come into play? I am thinking of a case for example when a user tries to do yaw rate control while stationary, and it would saturate the I component, and then would apply this constant when the user then throttles up and starts moving the boat.

This needs scaling the actuator effectiveness depending on speed. This may be different whether it is a differential drive vehicle or a ackerman steering vehicle. You can probably use insights from fixed wings relative to airspeed.

[junwoo091400]

Don't think we really need this Issue 👍, we can discuss everything in separate baby step PRs