ardupilot/ArduPlane/mode_guided.cpp

58 lines
1.3 KiB
C++

#include "mode.h"
#include "Plane.h"
bool ModeGuided::_enter()
{
plane.guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
Location loc{plane.current_loc};
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.guided_mode_enabled()) {
/*
if using Q_GUIDED_MODE then project forward by the stopping distance
*/
loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()),
plane.quadplane.stopping_distance());
}
#endif
plane.set_guided_WP(loc);
return true;
}
void ModeGuided::update()
{
#if HAL_QUADPLANE_ENABLED
if (plane.auto_state.vtol_loiter && plane.quadplane.available()) {
plane.quadplane.guided_update();
return;
}
#endif
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
}
void ModeGuided::navigate()
{
// Zero indicates to use WP_LOITER_RAD
plane.update_loiter(0);
}
bool ModeGuided::handle_guided_request(Location target_loc)
{
// add home alt if needed
if (target_loc.relative_alt) {
target_loc.alt += plane.home.alt;
target_loc.relative_alt = 0;
}
plane.set_guided_WP(target_loc);
return true;
}