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

Plane: Add fwd. throttle cutoff voltage parameter #29080

Open
wants to merge 1 commit into
base: master
Choose a base branch
from

Conversation

rubenp02
Copy link
Contributor

@rubenp02 rubenp02 commented Jan 15, 2025

Plane: Add fwd. throttle cutoff voltage parameter

Updated the forward throttle battery voltage compensation feature to disable the throttle entirely when the sag-compensated voltage drops below the new parameter FWD_BAT_THR_CUT.

Key changes:

  • Added new parameter FWD_BAT_THR_CUT to control the voltage threshold for this feature. The default value of 0 matches the original behavior of never cutting the throttle due to low voltage.
  • Modified forward throttle battery voltage compensation logic in the servos code to cut off the throttle if the resting voltage estimate of the FWD_BAT_IDX battery is under FWD_BAT_THR_CUT.

@Hwurzburg Hwurzburg added the WikiNeeded needs wiki update label Jan 15, 2025
@Hwurzburg
Copy link
Collaborator

Needs library commit split

@IamPete1
Copy link
Member

Have you considered using scripting for this?

@rubenp02
Copy link
Contributor Author

Have you considered using scripting for this?

Would it work as cleanly? This has the advantage that it keeps the aircraft armed.

@IamPete1
Copy link
Member

Would it work as cleanly? This has the advantage that it keeps the aircraft armed.

You could trigger the E-Stop aux function which will only stop the motors. You could also force override the throttle PWM to 0. Or set the throttle max param to some low value such that you still get a little throttle.

The nice thing about scripting is that it keeps it very contained, you only have to consider your use case. For example this PR would be very bad on a hovering quadplane, to get it into the main code you need to deal with that case (and I'm sure we will think of some others). If its just a script then that is not a problem.

Another option could be to support glide as a battery fail safe action. We support this already for RC/GCS failsafe. That means you don't have to touch the throttle suppression code which we have to be very careful with.

@rubenp02
Copy link
Contributor Author

Makes sense, thank you! Though I still think it may be valuable to support this as part of the main code, and not as a failsafe action, as I think those are separate. For example, I may want to RTL and then land or to go directly to the landing sequence on battery critical, but to have the throttle shut down either way.

What do you think is best? Should I just submit this as a script, or should I flesh out this implementation and get it into the main code?

@IamPete1
Copy link
Member

What do you think is best? Should I just submit this as a script, or should I flesh out this implementation and get it into the main code?

Your welcome carry on with the C++ implementation just be warned that we will be quite picky with failsafes and things touching throttle suppression ;)

@rubenp02 rubenp02 marked this pull request as draft January 15, 2025 16:47
@rubenp02 rubenp02 force-pushed the feature/supress-throttle-on-batt-critical-fs branch from d91053b to 8de814f Compare January 16, 2025 12:59
@rubenp02 rubenp02 changed the title Plane: Add suppress thr. on batt. critical param. Plane: Add throttle cutoff voltage parameter Jan 16, 2025
@rubenp02
Copy link
Contributor Author

@IamPete1 Is this more reasonable? I have aligned it more closely to my intended use case by making it entirely separate from the battery failsafes. I don't see the need to have this work in forward flight only (and to have exceptions for VTOL) since the whole point is to prevent the motor from wasting power when it can no longer provide any thrust. In a quadplane you'll have this configured based on the forward flight motors, so by the time the voltage is so low those are useless, the hovering capability will already be long gone.

@Hwurzburg
Copy link
Collaborator

Hwurzburg commented Jan 16, 2025

this is now an ESC voltage cut-off equivalent, I think...will apply only to FW battery IF its the primary battery indx, I think...seems okay to me

Copy link
Member

@IamPete1 IamPete1 left a comment

Choose a reason for hiding this comment

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

I'm really not a fan of using throttle suppression here. It could cause all sorts of odd stuff, this also needs to use a battery index, just using the first may not be valid.

How about linking into the existing battery voltage compensation code path. Here:

void ParametersG2::FWD_BATT_CMP::update()

It has a index parameter already. There is a function for setting min and max here:
https://github.com/ArduPilot/ardupilot/blob/25e1701447e582bc378c9cd0c7f3f554f424d2de/ArduPlane/servos.cpp#L441C34-L441C47.

So in this new case you could just set both min and max to 0 if under the threshold.

@rubenp02
Copy link
Contributor Author

Makes sense but what sort of issues does this cause? For future reference.
I can see how basing this on the primary battery can be a weird implementation detail

@IamPete1
Copy link
Member

Makes sense but what sort of issues does this cause?

I have no idea off the top of my head, the throttle suppression stuff is used in lots of places. Although I guess your not actually setting the throttle_suppressed variable but you do early return before other code that might set it. This part of plane is not very clean it is easy to get unexpected edge cases so we need to be very careful. The FWD_BATT_CMP stuff is more contained so its easier to be sure we have not broken other things.

@rubenp02 rubenp02 force-pushed the feature/supress-throttle-on-batt-critical-fs branch from 8de814f to 7cc7e75 Compare January 17, 2025 17:00
@rubenp02 rubenp02 changed the title Plane: Add throttle cutoff voltage parameter Add fwd. throttle cutoff voltage parameter Jan 17, 2025
@rubenp02 rubenp02 changed the title Add fwd. throttle cutoff voltage parameter Plane: Add fwd. throttle cutoff voltage parameter Jan 17, 2025
@rubenp02 rubenp02 marked this pull request as ready for review January 17, 2025 17:02
@rubenp02 rubenp02 requested a review from IamPete1 January 17, 2025 17:02
Copy link
Member

@IamPete1 IamPete1 left a comment

Choose a reason for hiding this comment

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

Basically looks good. I guess it still works as expected for your use case?

// @Units: V
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("FWD_THR_CUTOFF_V", 37, ParametersG2, fwd_batt_cmp.batt_voltage_throttle_cutoff, 0.0f),
Copy link
Member

Choose a reason for hiding this comment

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

It would be nice to keep the "FWD_BAT_" prefix so the parameters turn up together in a search. Although I can't think of any great options.

"FWD_BAT_THR_CUT" maybe?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Other options that come to mind are FWD_BAT_CUT_VOLT, or to make it more consistent with the other names, FWD_BAT_VOLT_CUT. But I think so far I prefer FWD_BAT_THR_CUT

ArduPlane/Parameters.h Outdated Show resolved Hide resolved
@@ -449,7 +457,6 @@ void ParametersG2::FWD_BATT_CMP::apply_min_max(int8_t &min_throttle, int8_t &max
// Ratio will always be >= 1, ensure still within max limits
min_throttle = int8_t(MAX((ratio * (float)min_throttle), -100));
max_throttle = int8_t(MIN((ratio * (float)max_throttle), 100));

Copy link
Member

Choose a reason for hiding this comment

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

accidental white space change.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

The couple newlines I removed were to improve consistency with the surrounding code. I guess I shouldn't do that? Just so I know for the next time.

ArduPlane/servos.cpp Outdated Show resolved Hide resolved
@rubenp02
Copy link
Contributor Author

I guess it still works as expected for your use case?

To a T, thanks. That one failing plane test is unrelated, right?

Updated the forward throttle battery voltage compensation feature to
disable the throttle entirely when the sag-compensated voltage drops
below the new parameter FWD_THR_CUTOFF_V.

Key changes:
- Added new parameter FWD_THR_CUTOFF_V to control the voltage threshold
  for this feature. The default value of 0 matches the original behavior
  of never cutting the throttle due to low voltage.
- Modified forward throttle battery voltage compensation logic in the
  servos code to cut off the throttle if the resting voltage estimate of
  the FWD_BAT_IDX battery is under FWD_THR_CUTOFF_V.
@rubenp02 rubenp02 force-pushed the feature/supress-throttle-on-batt-critical-fs branch from 7cc7e75 to ae3c5cd Compare January 18, 2025 17:07
@rubenp02 rubenp02 requested a review from IamPete1 January 18, 2025 17:10
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
WikiNeeded needs wiki update
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants