Copter: reload takeoff cmd on arm when using AllowTakeOffWithoutRaisi… #26589

khancyr · 2024-03-22T16:52:16Z

…ngThrottle

USING AUTO_OPTIONS 135 we set auto mode and arm from it if we got a takeoff cmd as first nav cmd. Doing this, the takeoff cmd is loaded when we enter the auto mode and set the target alt relatively to origin https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/mode_auto.cpp#L343 This is never called again. So when we reset the home from the first arming, we don't update the target alt relativly to origin. This leads to a wrong takeoff alt settle due to the alt drift on ground.

rmackay9 · 2024-03-23T01:01:03Z

Hi @khancyr,

Thanks very much for finding this issue. Could you provide steps to reproduce the issue in SITL?

I'd much prefer if the logic to handle this was within the _AutoTakeoff class (e.g. takeoff.cpp) and let's avoid reloading the mission command if at all possible. I think we should be able to handle this at a lower level.. a more precise fix. In particular if you look at _AutoTakeoff::run() it already has handling for when copter.ap.land_complete is true so may be in this area? I'm not totally sure but doing a complete reset by reloading the mission command seems like too large a change and I worry about future knock-on effects.

khancyr · 2024-03-25T09:31:45Z

@rmackay9 I don't know yet how to reproduce this on SITL. This need to offset the current position alt before taking off.
On a real drone, it is quite simple as the baro drift on ground will do this.

I can probably move the logic into the takeoff class. But I need to check back the impact on the other other mode using the takeoff cmd.
About reloading the takeoff cmd, unfortunately, I don't think we can avoid it for now. We need the information of the target takeoff altitude and in what frame it is. Those two info are only in the mission msg. That is also how it is done when we are using the MISSION_START cmd. We do a resume() or start() that reload the mission

IamPete1 · 2024-03-25T12:06:25Z

I think the fundamental mistake is that were taking off relative to origin altitude. Because we make the conversion from whatever alt frame when the command it started if home vs origin changes we take off to the wrong alt. Instead we should take the location object into the auto take off controller and calculate the altitude at run time.

rmackay9 · 2024-03-26T02:33:48Z

@IamPete1,

You may be right but it may be that we are just using the takeoff altitude as a distance to climb. Because the horizontal position doesn't change, the frame may not matter.

@khancyr, sorry to be pushy but there should be a way to do this without re-loading the command. Reloading in response to a MISSION_START makes total sense but doing it down in the auto takeoff code is something I'd like to avoid.

khancyr · 2024-03-26T08:10:14Z

Will look how to save all infos cleanly

khancyr · 2024-03-28T17:51:48Z

updated with takeoff location storage.

To reproduce on SITL. create a mission with a takeoff altitude as a relative altitude (in the example 50m).
set AUTO_OPTIONS 135

Wait for home, then move it to some higher altitude : example
long DO_SET_HOME 0 0 0 0 53.34642031503822 -7.714834673686349 90

Change to auto
Change back the home to the correct initial position (with mavproxy , click set home (with height))
arm throttle

module load graph :
graph GLOBAL_POSITION_INT.relative_alt/1000

MASTER

PR

peterbarker · 2024-04-02T11:21:42Z

We've discussed this before at DevCall.

Plane takes off above-current-altitude, but there's no appropriate mavlink frame for that.

I've created this for discussion: ArduPilot/mavlink#358

ArduCopter/mode_auto.cpp

tridge · 2024-04-03T07:49:05Z

probably worth adding a test for this, and checking the test fails without the PR and passes with

ArduCopter/mode_auto.cpp

khancyr · 2024-04-09T18:16:56Z

Added autotest.
added a small change on type + cast that reduce flash usage

tridge · 2024-04-10T07:19:58Z

@khancyr needs python tidy for CI pass (Tools/scripts/run_flake8.py ?)

ArduCopter/mode_auto.cpp

tridge · 2024-04-10T07:20:22Z

@rmackay9 will take a look

rmackay9 · 2024-04-10T07:20:27Z

ArduCopter/mode_auto.cpp

        // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
-        current_alt_cm -= terrain_offset;
+        current_alt_cm -= static_cast<int32_t>(terrain_offset);


I don't think we need this cast do we?

needed to save bytes as current_alt_cm is int now.

keep as float please.

rmackay9 · 2024-04-10T07:20:53Z

ArduCopter/mode_auto.cpp

@@ -324,32 +324,32 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
    // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin
    int32_t alt_target_cm;
    bool alt_target_terrain = false;
-    float current_alt_cm = inertial_nav.get_position_z_up_cm();
+    int32_t current_alt_cm = static_cast<int32_t>(inertial_nav.get_position_z_up_cm());


any reason why we're changing this to an int32?

final use of this is as an int. The only place we need the float version is L335 with the terrain offset. Using directly an int and casting the terrain_offset (in cm , so we don't lose accurracy) save us 16bytes

Keep as float please.

it is useless to keep it as float since all use from this variable are implicitly casting to int32_t, unless you got some WIP on those files

khancyr · 2024-04-10T08:55:15Z

@rmackay9 changes done

…RaisingThrottle USING AUTO_OPTIONS 135 we set auto mode and arm from it if we got a takeoff cmd as first nav cmd. Doing this, the takeoff cmd is loaded when we enter the auto mode and set the target alt relatively to origin https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/mode_auto.cpp#L343 This is never called again. So when we reset the home from the first arming, we don't update the target alt relativly to origin. This leads to a wrong takeoff alt settle due to the alt drift on ground.

rmackay9 · 2024-06-14T23:41:38Z

Hi @khancyr,

Sorry for taking so long to review this.

I've reproduced the issue that you've seen and looked at the solution and while it fixes the problem I think it would be better architecturally to move the fix into the _AutoTakeoff object. The changes would include something like this:

add new members which holds the target altitude and frame (the equivalent of the "Location takeoff_loc" you've added in this PR). Personally I would make that an target_alt_cm and target_alt_type but I guess we could use a Location but we would not use the lat,lon values.
add a new method called something like update_complete_alt() which updates complete_alt_cm and terrain_alt from the above targets
call the update_complete_alt() method from within _AutoTakeoff::run() when "if (!motors->armed() || !copter.ap.auto_armed) {"

What do you think?

khancyr · 2024-06-24T12:16:39Z

@rmackay9 I am ok with the idea to move the check into the _AutoTakeoff::run().
But this will be significant change in the code flow and it also impact Guided mode, this will lead to more testing need and write up more autotest.

I think it will hold this issue for too long until we got something merged. I would conter propose to get this current fix if you agree with it and open an issue for the upgraded version and assign me to it.

lthall · 2024-08-05T23:05:26Z

ArduCopter/mode_auto.cpp

@@ -324,32 +324,32 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
    // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin
    int32_t alt_target_cm;
    bool alt_target_terrain = false;
-    float current_alt_cm = inertial_nav.get_position_z_up_cm();
+    int32_t current_alt_cm = static_cast<int32_t>(inertial_nav.get_position_z_up_cm());


Keep as float please.

lthall · 2024-08-05T23:05:42Z

ArduCopter/mode_auto.cpp

        // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
-        current_alt_cm -= terrain_offset;
+        current_alt_cm -= static_cast<int32_t>(terrain_offset);


keep as float please.

rmackay9 · 2024-08-05T23:07:08Z

Hi, I really think with a bit more effort we could fix this in the way I've described above. Sorry!

khancyr · 2024-08-06T09:39:31Z

Closing then as I cannot spend more time on this. Thanks

khancyr added BUG Copter labels Mar 22, 2024

khancyr requested a review from rmackay9 March 22, 2024 16:53

khancyr force-pushed the feature/#74-takeoff-with-wrong-home branch from 7d63a74 to e144e1d Compare March 28, 2024 16:55

MattKear added the DevCallEU label Apr 3, 2024

peterbarker reviewed Apr 3, 2024

View reviewed changes

ArduCopter/mode_auto.cpp Outdated Show resolved Hide resolved

MattKear reviewed Apr 3, 2024

View reviewed changes

ArduCopter/mode_auto.cpp Outdated Show resolved Hide resolved

tridge removed the DevCallEU label Apr 3, 2024

MattKear reviewed Apr 3, 2024

View reviewed changes

ArduCopter/mode_auto.cpp Show resolved Hide resolved

khancyr force-pushed the feature/#74-takeoff-with-wrong-home branch from 311bf59 to 6d084b7 Compare April 9, 2024 18:16

khancyr added the DevCallEU label Apr 9, 2024

tridge removed the DevCallEU label Apr 10, 2024

rmackay9 reviewed Apr 10, 2024

View reviewed changes

ArduCopter/mode_auto.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Apr 10, 2024

View reviewed changes

khancyr force-pushed the feature/#74-takeoff-with-wrong-home branch from 6d084b7 to 86fd3c7 Compare April 10, 2024 08:54

khancyr added the Partner label Apr 22, 2024

khancyr added 2 commits June 14, 2024 09:38

Copter make current_alt_cm int32_t for flash saving

93f89b2

Tools: add test for auto takeoff relative alt

b2d4523

khancyr force-pushed the feature/#74-takeoff-with-wrong-home branch from 86fd3c7 to b2d4523 Compare June 14, 2024 07:38

andyp1per added the DevCallTopic label Aug 2, 2024

CraigElder removed the DevCallTopic label Aug 5, 2024

lthall suggested changes Aug 5, 2024

View reviewed changes

khancyr closed this Aug 6, 2024

khancyr deleted the feature/#74-takeoff-with-wrong-home branch August 6, 2024 09:39

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Copter: reload takeoff cmd on arm when using AllowTakeOffWithoutRaisi… #26589

Copter: reload takeoff cmd on arm when using AllowTakeOffWithoutRaisi… #26589

khancyr commented Mar 22, 2024

rmackay9 commented Mar 23, 2024

khancyr commented Mar 25, 2024

IamPete1 commented Mar 25, 2024

rmackay9 commented Mar 26, 2024

khancyr commented Mar 26, 2024

khancyr commented Mar 28, 2024 •

edited

Loading

peterbarker commented Apr 2, 2024

tridge commented Apr 3, 2024

khancyr commented Apr 9, 2024

tridge commented Apr 10, 2024

tridge commented Apr 10, 2024

rmackay9 Apr 10, 2024

khancyr Apr 10, 2024

lthall Aug 5, 2024

rmackay9 Apr 10, 2024

khancyr Apr 10, 2024

lthall Aug 5, 2024

khancyr Aug 6, 2024

khancyr commented Apr 10, 2024

rmackay9 commented Jun 14, 2024

khancyr commented Jun 24, 2024

lthall Aug 5, 2024

lthall Aug 5, 2024

rmackay9 commented Aug 5, 2024

khancyr commented Aug 6, 2024

Copter: reload takeoff cmd on arm when using AllowTakeOffWithoutRaisi… #26589

Copter: reload takeoff cmd on arm when using AllowTakeOffWithoutRaisi… #26589

Conversation

khancyr commented Mar 22, 2024

rmackay9 commented Mar 23, 2024

khancyr commented Mar 25, 2024

IamPete1 commented Mar 25, 2024

rmackay9 commented Mar 26, 2024

khancyr commented Mar 26, 2024

khancyr commented Mar 28, 2024 • edited Loading

peterbarker commented Apr 2, 2024

tridge commented Apr 3, 2024

khancyr commented Apr 9, 2024

tridge commented Apr 10, 2024

tridge commented Apr 10, 2024

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

khancyr commented Apr 10, 2024

rmackay9 commented Jun 14, 2024

khancyr commented Jun 24, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rmackay9 commented Aug 5, 2024

khancyr commented Aug 6, 2024

khancyr commented Mar 28, 2024 •

edited

Loading