plugin: corrected support for basic actions for ardupilot #1738

ykhedar · 2022-04-14T16:49:24Z

At the moment, takeoff action for APM does not work at all with MAVSDK since takeoff command can only be executed with the flight mode Guided for APM. Arming works sometime but does not work after landing is completed.

Following the work from #1703, this pull request tries to correct the broken actions (takeoff and arming) for Ardupilot.

Following are the changes made:

Arming for Ardupilot(Copter 4.1.2) can be done only in specific flight modes: Hold, Stabilized and Offboard. To keep compatibility with PX4 and MAVSDK, Hold mode is selected for arming of the copter.
Takeoff can only be done in Offboard mode, hence a function specific to apm takeoff is implemented to take care of this.
Flight mode conversion between Arducopter and MAVSDK is also updated.
Integration tests are added/updated for mission raw and action hover. This way 7 more integration tests for APM will run now in addition to the 5 previously.

Looking forward to get reviews :)

JonasVautherin

Nice! 🚀

A few suggestions, and I think you misspelled some APM* tests, I guess that's why it fails the PX4 CI checks

JonasVautherin · 2022-04-14T21:50:18Z

src/mavsdk/plugins/action/action_impl.cpp

@@ -342,21 +343,51 @@ void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const

 void ActionImpl::takeoff_async(const Action::ResultCallback& callback) const
 {
+    if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) {
+        takeoff_async_apm(callback);
+        return;


In that case I would rather go for a clearer separation, something like this:

if (_parent->autopilot() == SystemImpl::Autopilot::PX4) { takeoff_async_px4(callback); } else { takeoff_async_apm(callback); }

JonasVautherin · 2022-04-14T21:55:29Z

src/mavsdk/plugins/action/action_impl.cpp

@@ -603,6 +634,9 @@ void ActionImpl::set_takeoff_altitude_async(
 Action::Result ActionImpl::set_takeoff_altitude(float relative_altitude_m)
 {
    _takeoff_altitude = relative_altitude_m;
+    if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) {
+        return Action::Result::Success;


Same here: by having something like this:

if (_parent->autopilot() == SystemImpl::Autopilot::PX4) { return set_takeoff_altitude_px4(relative_altitude_m); } else { return set_takeoff_altitude_apm(relative_altitude_m); }

It makes it really clear that those are two very different implementations. Feels easier to maintain to me.

JonasVautherin · 2022-04-14T21:58:38Z

src/integration_tests/action_hover_sync.cpp

@@ -26,6 +27,23 @@ TEST_F(SitlTest, PX4ActionHoverSyncLower)
    takeoff_and_hover_at_altitude(1.5f);
 }

+TEST_F(SitlTest, APActionHoverSyncDefault)


Suggested change

TEST_F(SitlTest, APActionHoverSyncDefault)

TEST_F(SitlTest, APMActionHoverSyncDefault)

JonasVautherin · 2022-04-14T21:58:55Z

src/integration_tests/action_hover_sync.cpp

+    takeoff_and_hover_at_altitude_apm(2.5);
+}
+
+TEST_F(SitlTest, APActionHoverSyncHigher)


Suggested change

TEST_F(SitlTest, APActionHoverSyncHigher)

TEST_F(SitlTest, APMActionHoverSyncHigher)

JonasVautherin · 2022-04-14T21:59:09Z

src/integration_tests/action_hover_sync.cpp

+    takeoff_and_hover_at_altitude_apm(5.0f);
+}
+
+TEST_F(SitlTest, APActionHoverSyncLower)


Suggested change

TEST_F(SitlTest, APActionHoverSyncLower)

TEST_F(SitlTest, APMActionHoverSyncLower)

JonasVautherin · 2022-04-14T21:59:53Z

src/integration_tests/action_hover_sync.cpp

+    // EXPECT_TRUE(poll_condition_with_timeout(
+    //     [telemetry]() { return telemetry->flight_mode() == Telemetry::FlightMode::Takeoff; },
+    //     std::chrono::seconds(10)));
+
+    // EXPECT_TRUE(poll_condition_with_timeout(
+    //     [telemetry]() { return telemetry->flight_mode() == Telemetry::FlightMode::Hold; },
+    //     std::chrono::seconds(20)));
+
+    // We need to wait a bit until it stabilizes.


Suggested change

// EXPECT_TRUE(poll_condition_with_timeout(

// [telemetry]() { return telemetry->flight_mode() == Telemetry::FlightMode::Takeoff; },

// std::chrono::seconds(10)));

// EXPECT_TRUE(poll_condition_with_timeout(

// [telemetry]() { return telemetry->flight_mode() == Telemetry::FlightMode::Hold; },

// std::chrono::seconds(20)));

// We need to wait a bit until it stabilizes.

julianoes · 2022-04-16T03:54:47Z

@ykhedar I really appreciate your work here. I hope to get to it soon. That being said I also have a couple of refactoring changes in a local branch related to this, that will heavily conflict. I will have to resolve the conflicts.

…rm etc.

src/mavsdk/core/system_impl.cpp

src/mavsdk/plugins/action/action_impl.h

JonasVautherin · 2022-06-06T22:59:29Z

src/mavsdk/plugins/action/action_impl.cpp

@@ -238,7 +238,8 @@ void ActionImpl::arm_async(const Action::ResultCallback& callback) const
    };

    if (_parent->get_flight_mode() == FlightMode::Mission ||
-        _parent->get_flight_mode() == FlightMode::ReturnToLaunch) {
+        _parent->get_flight_mode() == FlightMode::ReturnToLaunch ||
+        _parent->get_flight_mode() == FlightMode::Land) { // Needed for APM


Probably a nitpick, but I'm very worried about APM/PX4 specifics being documented in comments. Comments tend to disappear, and then we may end up with if statements that we don't really understand and therefore don't dare changing because "maybe some other autopilot needed this".

Could we maybe try something more like below (just a suggestion, happy to have your thoughts of course 😊)?

if (need_hold_before_arm()) { _parent->set_flight_mode_async( FlightMode::Hold, [callback, send_arm_command](MavlinkCommandSender::Result result, float) { Action::Result action_result = action_result_from_command_result(result); if (action_result != Action::Result::Success) { if (callback) { callback(action_result); } } send_arm_command(); }); } else { send_arm_command(); }

With need_hold_before_arm() doing something like this:

bool need_hold_before_arm() { if (_parent->autopilot() == SystemImpl::Autopilot::Px4) { return need_hold_before_arm_px4(); } else { return need_hold_before_arm_apm(); } }

This way the code says what PX4 and APM need, instead of having that important information in a comment that may disappear or get outdated someday. In fact I would actually like to have this pattern (if (PX4) ... else ...) every time there is a difference between PX4 and APM. So that the code clearly tells us where it is different, and we can even search for it.

What do you think? @julianoes am I nitpicking here? 😅

PS: I added an else because the way it was written before confused me and I thought that send_arm_command() was being called twice sometimes. But that's not part of your PR, it's just a suggestion since we're here 😇.

agree with your suggestion :)

ykhedar · 2022-06-07T06:41:55Z

@julianoes you mentioned before that you are working on a local branch which might conflict with this PR. Is that still the case? I did a rebase to the current main branch and resolved some conflicts thus arising.

JonasVautherin

Nice!

JonasVautherin · 2022-06-20T09:10:59Z

Is the CI failure related to this PR?

ykhedar · 2022-06-20T09:27:56Z

Is the CI failure related to this PR?

Not really. Randomly one or two of the tests fail because the health_all_ok is not true. I did some digging and found its mostly because the home position not received/set. I did a test locally by making the home position request to retry every 2 seconds until home position is received something like this:

_parent->add_call_every([this]() { request_home_position_again(); }, 2.0f, &_call_every_cookie);

The change seems to make the integration tests pass (at least for the 10 times i tested locally.)
I was thinking making that another PR since was not related to this PR. If you disagree i can commit the changes to this PR

ykhedar requested review from bazfp, julianoes, JonasVautherin and TSC21 April 14, 2022 16:49

JonasVautherin requested changes Apr 14, 2022

View reviewed changes

ykhedar force-pushed the apm-action-mission branch from 4eb121a to dd8fa16 Compare June 6, 2022 13:01

plugin: corrected broken apm support for basic actions like takeoff a…

c330742

…rm etc.

ykhedar force-pushed the apm-action-mission branch from 22ef360 to c330742 Compare June 6, 2022 13:44

JonasVautherin requested changes Jun 6, 2022

View reviewed changes

clear differentiation between px4 and apm for arming now.

95d4f46

JonasVautherin approved these changes Jun 7, 2022

View reviewed changes

JonasVautherin merged commit 45feed9 into mavlink:main Jun 20, 2022

julianoes added the enhancement label Jun 23, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

plugin: corrected support for basic actions for ardupilot #1738

plugin: corrected support for basic actions for ardupilot #1738

ykhedar commented Apr 14, 2022

JonasVautherin left a comment

JonasVautherin Apr 14, 2022

JonasVautherin Apr 14, 2022

JonasVautherin Apr 14, 2022

JonasVautherin Apr 14, 2022

JonasVautherin Apr 14, 2022

JonasVautherin Apr 14, 2022

julianoes commented Apr 16, 2022

JonasVautherin Jun 6, 2022 •

edited

Loading

ykhedar Jun 7, 2022

ykhedar commented Jun 7, 2022

JonasVautherin left a comment

JonasVautherin commented Jun 20, 2022

ykhedar commented Jun 20, 2022

	TEST_F(SitlTest, APActionHoverSyncDefault)
	TEST_F(SitlTest, APMActionHoverSyncDefault)

	TEST_F(SitlTest, APActionHoverSyncHigher)
	TEST_F(SitlTest, APMActionHoverSyncHigher)

	TEST_F(SitlTest, APActionHoverSyncLower)
	TEST_F(SitlTest, APMActionHoverSyncLower)

plugin: corrected support for basic actions for ardupilot #1738

plugin: corrected support for basic actions for ardupilot #1738

Conversation

ykhedar commented Apr 14, 2022

JonasVautherin left a comment

Choose a reason for hiding this comment

JonasVautherin Apr 14, 2022

Choose a reason for hiding this comment

JonasVautherin Apr 14, 2022

Choose a reason for hiding this comment

JonasVautherin Apr 14, 2022

Choose a reason for hiding this comment

JonasVautherin Apr 14, 2022

Choose a reason for hiding this comment

JonasVautherin Apr 14, 2022

Choose a reason for hiding this comment

JonasVautherin Apr 14, 2022

Choose a reason for hiding this comment

julianoes commented Apr 16, 2022

JonasVautherin Jun 6, 2022 • edited Loading

Choose a reason for hiding this comment

ykhedar Jun 7, 2022

Choose a reason for hiding this comment

ykhedar commented Jun 7, 2022

JonasVautherin left a comment

Choose a reason for hiding this comment

JonasVautherin commented Jun 20, 2022

ykhedar commented Jun 20, 2022

JonasVautherin Jun 6, 2022 •

edited

Loading