Plane: Refactor soaring.cpp. #14830

samuelctabor · 2020-07-16T12:54:16Z

Move check on heading alignment to a separate method
Fix indent
Re-arrange and simplify logic

There should be no change in behaviour, I flight tested this before splitting out from #14815 and didn't find any problems.

peterbarker

I've only glanced through this at this point.

@magicrub - I think you already reviewed this and deemed it good to be merged?

peterbarker · 2020-07-20T10:57:49Z

ArduPlane/soaring.cpp

-            set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED);
-        }
-        break;
+        default:


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...

I have no preference . . . happy to change either way

@samuelctabor in that case, please move it back to not having the extra indent. Thanks.

peterbarker · 2020-07-20T11:10:33Z

ArduPlane/soaring.cpp

+            // 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) {


We don't usually use exponents for this...

Also, we like to append _ms to times in milliseconds...

peterbarker · 2020-07-20T11:13:23Z

ArduPlane/soaring.cpp

+                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;


We have .zero() methods on vectors.

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.

Switched to using .zero() and added comment.

ArduPlane/soaring.cpp

samuelctabor · 2020-07-20T11:48:18Z

Thanks @peterbarker , I'll get those fixed up later today.

peterbarker

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.

peterbarker · 2020-07-26T23:04:13Z

ArduPlane/soaring.cpp

-            set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED);
-        }
-        break;
+        default:


@samuelctabor in that case, please move it back to not having the extra indent. Thanks.

peterbarker · 2020-07-26T23:06:23Z

ArduPlane/soaring.cpp

-        // 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());



Extra whitespace added.

peterbarker · 2020-07-26T23:06:58Z

ArduPlane/soaring.cpp

+            Vector3f position;
+            if (!ahrs.get_relative_position_NED_home(position)) {
+                return;
+            } else if (g2.soaring_controller.max_radius >= 0 &&


The else is pointless here.

peterbarker · 2020-07-26T23:16:35Z

ArduPlane/soaring.cpp

+            g2.soaring_controller.update_thermalling();  // Update estimate
+
+            // Check distance to home.
+            Vector3f position;


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.

I've added a comment to clarify that home needs to be set for other reasons too.

peterbarker · 2020-07-26T23:26:32Z

ArduPlane/soaring.cpp

+                    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);


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 :-)

Good catch. I've added a break.

peterbarker · 2020-07-26T23:35:22Z

ArduPlane/soaring.cpp

+                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) {


You don't need to call mode_number() here - you can compare against &mode_auto - not that it matters all that much.

Good tip, thanks!

magicrub · 2020-07-31T17:33:34Z

This is looking good. @samuelctabor and @peterbarker how are you feeling about this PR?

peterbarker

Nothing in my review here should be considered a blocker.

This case/switch whitespace war is starting to irk me, however.

peterbarker · 2020-07-31T23:33:59Z

ArduPlane/soaring.cpp

+        }
+
+        // Check distance to home against MAX_RADIUS.
+        if (g2.soaring_controller.max_radius >= 0 &&


zero here is an interesting value - Is it worth saying zero-disables instead?

Left unchanged for now.

peterbarker · 2020-07-31T23:38:12Z

ArduPlane/soaring.cpp

-                prev_wp.y = 0.0;
-                next_wp.x = 0.0;
-                next_wp.y = 0.0;
+                prev_wp.zero();


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).

Left unchanged for now.

peterbarker · 2020-07-31T23:41:44Z

ArduPlane/soaring.cpp

            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;


This could be const.

It could also have a better name - time_spent_in_loiter_ms or time_in_loiter_ms or similar.

peterbarker · 2020-07-31T23:46:53Z

ArduPlane/soaring.cpp

-            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))) {


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.

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

peterbarker · 2020-07-31T23:53:10Z

ArduPlane/soaring.cpp

-            break;
-        } // switch previous_mode
+            case SoaringController::LoiterStatus::ALT_TOO_LOW:
+                gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Too low, restoring %s", exit_mode->name());


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.

I was thinking this same thing

Great, added this helper method.

peterbarker · 2020-07-31T23:55:15Z

ArduPlane/soaring.cpp

+    // 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: {


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.

All fixed, thanks.

samuelctabor · 2020-08-01T17:34:09Z

Thanks guys. I'm going to flight test this, and then if there are no more comments I will squash it down.

samuelctabor · 2020-08-01T20:29:15Z

Flight tested, all good. Overall this cleanup saves 432 bytes.

peterbarker · 2020-08-03T00:47:40Z

ArduPlane/Plane.h

@@ -1072,6 +1072,8 @@ class Plane : public AP_Vehicle {
    // soaring.cpp
 #if SOARING_ENABLED == ENABLED
    void update_soaring();
+    bool soaring_exit_heading_aligned() const;


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.

samuelctabor · 2020-08-03T09:35:05Z

Now squashed.

tridge · 2020-08-04T00:10:25Z

ArduPlane/soaring.cpp

+
+        // 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) &&


I'd prefer sq(x) to powf(x,2)

Thanks, done

samuelctabor · 2020-08-04T13:05:16Z

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.

peterbarker · 2020-08-05T01:59:21Z

Merged! Thanks @samuelctabor !

Definitely time to play with this locally.

samuelctabor mentioned this pull request Jul 20, 2020

Soaring: Use RCX_OPTION #14815

Merged

peterbarker reviewed Jul 20, 2020

View reviewed changes

samuelctabor force-pushed the pr/soar-refactor branch from e538952 to 17a44b6 Compare July 21, 2020 08:37

peterbarker reviewed Jul 26, 2020

View reviewed changes

peterbarker reviewed Jul 31, 2020

View reviewed changes

peterbarker approved these changes Aug 3, 2020

View reviewed changes

peterbarker added the DevCallTopic label Aug 3, 2020

Plane: Refactor soaring.cpp.

84d5621

samuelctabor force-pushed the pr/soar-refactor branch from c80a6af to 84d5621 Compare August 3, 2020 08:10

tridge approved these changes Aug 4, 2020

View reviewed changes

CraigElder removed the DevCallTopic label Aug 4, 2020

Plane: Use sq() rather than powf() in soaring.cpp.

e51a0f1

peterbarker merged commit aa776b0 into ArduPilot:master Aug 5, 2020

samuelctabor deleted the pr/soar-refactor branch August 5, 2020 10:30

Plane: Refactor soaring.cpp. #14830

Plane: Refactor soaring.cpp. #14830

Conversation

samuelctabor commented Jul 16, 2020

peterbarker left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

samuelctabor commented Jul 20, 2020

peterbarker left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

magicrub commented Jul 31, 2020

peterbarker left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

samuelctabor commented Aug 1, 2020

samuelctabor commented Aug 1, 2020

Choose a reason for hiding this comment

samuelctabor commented Aug 3, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

samuelctabor commented Aug 4, 2020

peterbarker commented Aug 5, 2020