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
Conversation
506647e
to
9ed2750
Compare
9ed2750
to
04dde27
Compare
rebased |
5ef067d
to
0cc2f0e
Compare
0cc2f0e
to
26522b0
Compare
I tested this using real copter. |
I haven't used terrain alt (SRTM data) because it's just takeoff and don't move anywhere. @rmackay9 Please review this |
I have also confirmed that #9010 is maintained. Simple test : |
ArduCopter/mode_guided.cpp
Outdated
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) { |
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 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.
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.
Thank you for the review.
I overlooked the changes during takeoff.
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.
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.
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 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.
26522b0
to
c809ac3
Compare
I've tested these scenarios.
|
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. |
@rmackay9 test: The target altitude has changed to 3.4m. |
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. |
c809ac3
to
065d715
Compare
It worked! I've tested these again.
In the last case, the alt source changes on the ground and after Takeoff, but I think it's good. |
Great, I'll merge after it passes travis |
Thank you very much for your advice! |
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
After
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.