Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: use rangefinder to takeoff altitude in guided mode

This commit is contained in:
Tatsuya Yamaguchi 2020-03-09 19:30:47 +09:00 committed by Randy Mackay
parent c4b5d47e1f
commit 29dcbd1398
1 changed files with 23 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/mode_guided.cpp
View File

@ -53,7 +53,17 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
// initialise wpnav destination
Location target_loc = copter.current_loc;
target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME);
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 &&
takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
// can't takeoff downwards
if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
return false;
}
frame = Location::AltFrame::ABOVE_TERRAIN;
}
target_loc.set_alt_cm(takeoff_alt_cm, frame);
if (!wp_nav->set_wp_destination(target_loc)) {
// failure to set destination can only be because of missing terrain data
@ -374,8 +384,18 @@ void ModeGuided::takeoff_run()
copter.landinggear.retract_after_takeoff();
// switch to position control mode but maintain current target
const Vector3f& target = wp_nav->get_wp_destination();
set_destination(target);
Location dest_loc(wp_nav->get_wp_destination());
Location::AltFrame frame = wp_nav->origin_and_destination_are_terrain_alt()
? Location::AltFrame::ABOVE_TERRAIN
: Location::AltFrame::ABOVE_HOME;
int32_t target_alt_cm;
if (dest_loc.get_alt_cm(frame, target_alt_cm)) {
dest_loc.set_alt_cm(target_alt_cm, frame);
set_destination(dest_loc);
} else {
const Vector3f& target = wp_nav->get_wp_destination();
set_destination(target);
}
}
}