Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rover cleanup + arch fixes #16847

Merged
merged 17 commits into from
Feb 14, 2021
Merged

Rover cleanup + arch fixes #16847

merged 17 commits into from
Feb 14, 2021

Conversation

LorenzMeier
Copy link
Member

@LorenzMeier LorenzMeier commented Feb 13, 2021

This cleans up some rover specific things, but also adds / fixes two fundamentals (@dagar):

  • It allows for the manual -1 to +1 input range, as originally designed. This is important for reversing vehicles. I have added hardening because I was concerned about corner cases for multicopters and fixed wings.
  • The reverse setting for PWM channels on PX4IO was off by one. While we will refresh this architecture soon, there is no point of leaving it in the current broken state.

To get to a -1 to 1 scaling for reversing vehicles the calibration in QGC will need to be changed - QGC sets the throttle channel MIN == TRIM, effectively eliminating the negative range. We should change that behavior going forward for reversing vehicle classes.

@dagar
Copy link
Member

dagar commented Feb 13, 2021

FYI @MaEtUgR (manual input range)

@dagar
Copy link
Member

dagar commented Feb 13, 2021

The reverse setting for PWM channels on PX4IO was off by one. While we will refresh this architecture soon, there is no point of leaving it in the current broken state.

I threw this directly into master immediately.

@dagar dagar requested a review from Jaeyoung-Lim February 13, 2021 20:33
If the user calibrated to negative throttle, enable them to use it.
90% of all real-world vehicle configs default to this and it is something that users stumble over if they configure a new system. There are valid cases where this would not be desired - for these it can be still switched off.
PX4 can support negative (reverse) throttle and the fixed wing controller is not expecting this input range. This hardens it against it.
PX4 can support negative (reverse) throttle and the fixed wing controller is not expecting this input range. This hardens it against it.
@@ -53,7 +53,8 @@ bool Sticks::checkAndSetStickInputs()
// Linear scale
_positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1]
_positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1]
_positions(2) = -(manual_control_setpoint.z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
_positions(2) = -(math::constrain(manual_control_setpoint.z, 0.0f, 1.0f)
, - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
, - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
- 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]

LorenzMeier and others added 11 commits February 13, 2021 23:24
PX4 can support negative (reverse) throttle and the multicopter controller is not expecting this input range. This hardens it against it.
This commit moves the steering output from yaw to the roll channel to better reflect the lateral control input / reaction mapping. It also removes old unused parameters and modernizes the mainloop to remove unnecessary polling.
PX4 supports -1 to 1 as input and this module was not protected against the input range.
PX4 supports -1 to 1 as input and this module was not protected against the input range.
This matches better the different platforms that are using this functionality.
Modernize rover position control
This commit adds support for stabilized flight mode for rovers, which enables the rover tracking a fixed heading that is set with a manual input
This allows to give external barometers priority if they are present.
This adds support for the different implementation variants of the v5X standard.
Jaeyoung-Lim
Jaeyoung-Lim previously approved these changes Feb 14, 2021
Copy link
Member

@Jaeyoung-Lim Jaeyoung-Lim left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for pulling this in!


} else {
const float yaw_rate = math::radians(_param_gnd_man_y_max.get());
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.y * yaw_rate;

I think using the roll stick inputs were part of this PR, and need to change for stabilized mode too

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was confused as well, but I did fix that yesterday already, but forgot to push - done now.

It is more intuitive to use the roll stick as lateral control input given that drones defined this category, plus this is how consumer car / rover radio controls have been working already.
@LorenzMeier LorenzMeier merged commit 4c5d236 into master Feb 14, 2021
@LorenzMeier LorenzMeier deleted the pr/rover-cleanup branch February 14, 2021 10:17
@MaEtUgR
Copy link
Member

MaEtUgR commented Feb 15, 2021

@dagar Thanks for the reminder!

It allows for the manual -1 to +1 input range, as originally designed.

Makes sense. I'm enabling this for all vehicles. See #15949
I'll just rebase it and thoroughly test compatibility then we can merge such that it doesn't get stale again.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants