-
Notifications
You must be signed in to change notification settings - Fork 17.2k
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: Refactor soaring.cpp. #14830
Plane: Refactor soaring.cpp. #14830
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I've only glanced through this at this point.
@magicrub - I think you already reviewed this and deemed it good to be merged?
ArduPlane/soaring.cpp
Outdated
set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED); | ||
} | ||
break; | ||
default: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Really looking like we might need to change our style guide to dictate one or the other of case-is-indented-to-same-level-as-switch or case-indented-another-step-from-switch.
Because otherwise we're going to end up with whitespace wars...
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I have no preference . . . happy to change either way
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@samuelctabor in that case, please move it back to not having the extra indent. Thanks.
ArduPlane/soaring.cpp
Outdated
// Some other loiter status, we need to think about exiting loiter. | ||
uint32_t timer = AP_HAL::millis() - plane.soaring_mode_timer; | ||
|
||
if (!soaring_exit_heading_aligned() && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We don't usually use exponents for this...
Also, we like to append _ms
to times in milliseconds...
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Fixed.
ArduPlane/soaring.cpp
Outdated
if (!(mission.get_next_nav_cmd(mission.get_prev_nav_cmd_with_wp_index(), prev_nav_cmd) && | ||
prev_nav_cmd.content.location.get_vector_xy_from_origin_NE(prev_wp) && | ||
current_nav_cmd.content.location.get_vector_xy_from_origin_NE(next_wp))) { | ||
prev_wp.x = 0.0; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We have .zero()
methods on vectors.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A short comment indicating why it's OK for prev_wp and next_wp to be zero would be good. To my eye cruise critiera will always be false in that case.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Switched to using .zero() and added comment.
Thanks @peterbarker , I'll get those fixed up later today. |
e538952
to
17a44b6
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There a few whitespace consistency issues; we usually like whitespace around things like !=
and the like.
Some of the points I've made in here don't really need to be acted on as they're pre-existing.
ArduPlane/soaring.cpp
Outdated
set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED); | ||
} | ||
break; | ||
default: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@samuelctabor in that case, please move it back to not having the extra indent. Thanks.
ArduPlane/soaring.cpp
Outdated
// Update thermal estimate and check for switch back to AUTO | ||
g2.soaring_controller.update_thermalling(); // Update estimate | ||
if (g2.soaring_controller.check_thermal_criteria()) { | ||
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_loiter.name()); | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Extra whitespace added.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Fixed.
ArduPlane/soaring.cpp
Outdated
Vector3f position; | ||
if (!ahrs.get_relative_position_NED_home(position)) { | ||
return; | ||
} else if (g2.soaring_controller.max_radius >= 0 && |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The else is pointless here.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Removed.
ArduPlane/soaring.cpp
Outdated
g2.soaring_controller.update_thermalling(); // Update estimate | ||
|
||
// Check distance to home. | ||
Vector3f position; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It looks like you only care about this in the case that max_radius >=0 - perhaps move the check into the check against soaring_controller.max_radius
?
This check was also interesting combined with the code that was on line 121 which was also checking to see if home was set. That's been factored out now into a separate function, however.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I've added a comment to clarify that home needs to be set for other reasons too.
ArduPlane/soaring.cpp
Outdated
previous_mode->mode_number()!=Mode::Number::AUTO) { | ||
// Some other loiter status, and outside of maximum soaring radius, and previous mode wasn't AUTO | ||
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Outside SOAR_MAX_RADIUS, RTL"); | ||
set_mode(mode_rtl, ModeReason::SOARING_DRIFT_EXCEEDED); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'm a bit surprised by the fall-through here. It looks like we go into RTL but then continue to update the local soaring state - and might even choose a different mode to exit to, depending on what else has gone wrong.
Deserves a comment if nothing else :-)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Good catch. I've added a break.
ArduPlane/soaring.cpp
Outdated
return; | ||
} else if (g2.soaring_controller.max_radius >= 0 && | ||
powf(position.x,2)+powf(position.y,2) > powf(g2.soaring_controller.max_radius,2) && | ||
previous_mode->mode_number()!=Mode::Number::AUTO) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You don't need to call mode_number() here - you can compare against &mode_auto
- not that it matters all that much.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Good tip, thanks!
This is looking good. @samuelctabor and @peterbarker how are you feeling about this PR? |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nothing in my review here should be considered a blocker.
This case/switch whitespace war is starting to irk me, however.
} | ||
|
||
// Check distance to home against MAX_RADIUS. | ||
if (g2.soaring_controller.max_radius >= 0 && |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
zero here is an interesting value - Is it worth saying zero-disables instead?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Left unchanged for now.
prev_wp.y = 0.0; | ||
next_wp.x = 0.0; | ||
next_wp.y = 0.0; | ||
prev_wp.zero(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If there was some guarantee on get_vector_xy_from_origin_NE
that its argument would remain unchanged then we wouldn't need to zero here (Vector2 and Vector3 are implicitly zeroed in ArduPilot). Sadly, there's no such comment in the header ('though the current implementation has the desired property).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Left unchanged for now.
ArduPlane/soaring.cpp
Outdated
break; | ||
} | ||
|
||
// Some other loiter status, we need to think about exiting loiter. | ||
uint32_t timer = AP_HAL::millis() - plane.soaring_mode_timer; | ||
uint32_t timer_ms = AP_HAL::millis() - plane.soaring_mode_timer_ms; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This could be const
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It could also have a better name - time_spent_in_loiter_ms
or time_in_loiter_ms
or similar.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done
const bool headingLinedupToWP = plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location); | ||
if (!headingLinedupToWP && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) { | ||
if (loiterStatus == SoaringController::LoiterStatus::ALT_TOO_LOW && | ||
((previous_mode == &mode_cruise) || (previous_mode == &mode_fbwb))) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nomination of specific modes like this can lead to issues where somebody adds another throttle-controlled mode but forgets to update this sort of list. That's why we try to have callbacks on the mode saying they have a particular property.
This isn't a blocker, just an observation.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is allllll over the place. Part of the onion v2 stuff will take care of all that so lets not worry about it for now
ArduPlane/soaring.cpp
Outdated
break; | ||
} // switch previous_mode | ||
case SoaringController::LoiterStatus::ALT_TOO_LOW: | ||
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Too low, restoring %s", exit_mode->name()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Might save a few bytes by creating a "soaring_restore_mode":
soaring_restore_mode("Too low", ModeReason::SOARING_ALT_TOO_LOW, *exit_mode);
void Plane::soaring_restore_mode(const char *reason, ModeReason modereason, Mode &exit_mode)
{
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: %s, restoring %s", exit_mode.name());
set_mode(exit_mode, modereason);
}
A lookup table from SoaringController::LoiterStatus -> const char*description might make reuse of the strings possible, too.
Not a blocker.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I was thinking this same thing
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Great, added this helper method.
ArduPlane/soaring.cpp
Outdated
// Return true if the current heading is aligned with the next objective. | ||
// If home is not set, or heading not locked, return true to avoid delaying mode change. | ||
switch (previous_mode->mode_number()) { | ||
case Mode::Number::AUTO: { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The rest of this file doesn't have case
indented from switch
, and while I blieve soaring.cpp is your domain, I think tridge has a preference for indented-to-same-level.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
All fixed, thanks.
Thanks guys. I'm going to flight test this, and then if there are no more comments I will squash it down. |
Flight tested, all good. Overall this cleanup saves 432 bytes. |
@@ -1072,6 +1072,8 @@ class Plane : public AP_Vehicle { | |||
// soaring.cpp | |||
#if SOARING_ENABLED == ENABLED | |||
void update_soaring(); | |||
bool soaring_exit_heading_aligned() const; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Potential future cleanup: pull these methods and the soaring mode change timer into a little class to keep them grouped nicely. Would have the side-effect of compiling out that timer variable which would be nice.
We do this a bit in Plane and Copter. SurfaceTracking in Copter, for example.
c80a6af
to
84d5621
Compare
Now squashed. |
ArduPlane/soaring.cpp
Outdated
|
||
// Check distance to home against MAX_RADIUS. | ||
if (g2.soaring_controller.max_radius >= 0 && | ||
powf(position.x,2)+powf(position.y,2) > powf(g2.soaring_controller.max_radius,2) && |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'd prefer sq(x) to powf(x,2)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks, done
The only immediate comment on the dev call was from @tridge to use sq(x) rather than powf(x,2), this is now done. There was good discussion on the opportunities for better structuring soaring within Plane as a longer term project, but this PR should be good to merge. |
Merged! Thanks @samuelctabor ! Definitely time to play with this locally. |
There should be no change in behaviour, I flight tested this before splitting out from #14815 and didn't find any problems.