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

Copter: use rangefinder to takeoff altitude in guided mode #13768

Merged
merged 2 commits into from Apr 23, 2020

Conversation

tatsuy
Copy link
Contributor

@tatsuy tatsuy commented Mar 9, 2020

This PR allows to use rangefinder when takeoff in Guided mode if WPNAV_RFND_USE = 1 and a rangefinder is connected.
The terrain alt with RangeFinder will continue even after takeoff is over and switched to POS control.

I tested it in SITL. Specified 2m in takeoff.

Before
image

After
image

We support terrain follwoing in guided mode. (#13582)
But takeoff in guided mode use relative altitude only before.

SET_POSITION_TARGET_GLOBAL_INT has the field of MAV_FRAME.
But, MAV_CMD_NAV_TAKEOFF command doesn't have it.
https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_TAKEOFF

There is also a method of selecting relative or terrain by parameter like RTL_ALT_TYPE.
But I decided to use terrain_alt automatically if WPNAV_RFND_USE = 1 and a rangefinder is connected because I don't want to increase the number of parameters.

@tatsuy
Copy link
Contributor Author

tatsuy commented Mar 24, 2020

rebased

@tatsuy tatsuy force-pushed the pr-takeoff-guided-terrain branch 2 times, most recently from 5ef067d to 0cc2f0e Compare March 26, 2020 02:32
@tatsuy tatsuy force-pushed the pr-takeoff-guided-terrain branch from 0cc2f0e to 26522b0 Compare April 2, 2020 05:54
@tatsuy
Copy link
Contributor Author

tatsuy commented Apr 2, 2020

I tested this using real copter.

@tatsuy
Copy link
Contributor Author

tatsuy commented Apr 9, 2020

I haven't used terrain alt (SRTM data) because it's just takeoff and don't move anywhere.
The error of SRTM data is a little dangerous in takeoff at low altitude such as 2m or less.
So this PR uses either RangeFinder or AVOBE_HOME.

@rmackay9 Please review this

@tatsuy
Copy link
Contributor Author

tatsuy commented Apr 10, 2020

I have also confirmed that #9010 is maintained.

Simple test :
guided
takeoff 20
wait alt = 20
setyaw 42 0 0

set_destination(target);
Location dest_loc(wp_nav->get_wp_destination());
Location::AltFrame frame = Location::AltFrame::ABOVE_HOME;
if (wp_nav->rangefinder_used_and_healthy() && wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER) {
Copy link
Contributor

Choose a reason for hiding this comment

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

I worry a bit that re-doing this decision making could lead to a different decision than was originally made at takeoff. For example if the rangefinder became healthy during the takeoff the decision could change. This could happen if the rangefinder was out-of-range-low while on the ground. I think a simpler approach would be:

  • call set_destination(target) and fill in all the arguments including the final "terrain_alt" using AC_WPNav::origin_and_destination_are_terrain_alt().
  • call Mode:Guided::set_destination(Location) and provide it the location from AC_WPNav::get_wp_destination(Location& destination).

I'd probably recommend the first option.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Thank you for the review.
I overlooked the changes during takeoff.

Copy link
Contributor

Choose a reason for hiding this comment

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

Sorry to be annoying about this but could we use the 1st approach which involves calling AC_WPNav::origin_and_destination_are_terrain_alt()? The complexity and number of lines should be much reduced.. it should shrink down to just two lines I think.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I've tried to change the following.

dest_loc.set_alt_cm(dest_loc.alt, wp_nav->origin_and_destination_are_terrain_alt() ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_HOME);
set_destination(dest_loc);

The target altitude seems to change a little when using AVOBE_HOME.
I tested like this.

mode guided
arm throttle
takeoff 10

dest_loc.alt = 985
dest_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm) = 1000

I think that I have to use get_alt_cm.
This error increases to nearly 1m when using GND_ALT_OFFSET.

@tatsuy
Copy link
Contributor Author

tatsuy commented Apr 11, 2020

I've tested these scenarios.

  • enable rangefinder
  1. takeoff alt <= RNGFNDx_MAX_CM
    use ABOVE_TERRAIN
  2. takeoff alt > RNGFNDx_MAX_CM
    use ABOVE_HOME
  3. In case of the rangefinder was out-of-range-low while on the ground
    use ABOVE_HOME
  4. In case of the RangeFinder becomes unavailable during Takeoff
    Failsafe: Terrain data missing -> RTL
  • disable rangefinder
    use ABOVE_HOME

@rmackay9
Copy link
Contributor

I've put a commit at the end of this branch which simplifies it a bit: https://github.com/rmackay9/rmackay9-ardupilot/commits/tatsuy-guided-takeoff-terr. Once I've completed testing I think this might be good to include this change because it's less code so easier to maintain.

@tatsuy
Copy link
Contributor Author

tatsuy commented Apr 23, 2020

@rmackay9
Thank you for the sample code.
Without RangeFinder, it worked with your code.
But, there is an altitude error when using RangeFinder.

test:
arm throttle
takeoff 3

The target altitude has changed to 3.4m.
https://youtu.be/ZRIMdJfDhtk
(In this test, I added send_text message in ModeGuided::set_destination function.)

@rmackay9
Copy link
Contributor

@tatsuy,

Thanks for testing, I've found and fixed the issue. The problem was that ModeGuided::set_destination() calls AC_WPNav::set_wp_destination twice. Because we were passing in the destination as a "const Vector3f&" the intermediate destination was getting changed.

@tatsuy
Copy link
Contributor Author

tatsuy commented Apr 23, 2020

It worked!
I renamed dest back to target.

I've tested these again.

  1. disable rangefinder (RNGFND1_TYPE=0)
    ABOVE_HOME
  2. WPNAV_RFND_USE=0
    ABOVE_HOME
  3. takeoff alt <= RNGFNDx_MAX_CM
    ABOVE_TERRAIN
  4. takeoff alt > RNGFNDx_MAX_CM
    ABOVE_HOME
  5. In case of the rangefinder was out-of-range-low while on the ground
    ABOVE_HOME
  6. In case of the RangeFinder becomes unavailable during Takeoff
    ABOVE_TERRAIN -> ABOVE_HOME

In the last case, the alt source changes on the ground and after Takeoff, but I think it's good.

@rmackay9
Copy link
Contributor

Great, I'll merge after it passes travis

@rmackay9 rmackay9 merged commit dcbbd04 into ArduPilot:master Apr 23, 2020
@tatsuy tatsuy deleted the pr-takeoff-guided-terrain branch April 23, 2020 12:01
@tatsuy
Copy link
Contributor Author

tatsuy commented Apr 23, 2020

Thank you very much for your advice!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants