mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
2784f8fa7f
So instead of updating plane.guided_WP_loc and then calling set_guided_WP(void) to copy that state into plane.next_WP_loc we pass the new location in the call to set_guided_WP(const Location &loc). avoidance was the only place which was not entirely over-writing plane.guided_WP_loc. However, plane.next_WP_loc was updated to be the current location when we entered guided mode. If we update the horizontal/vertical avoidance now it is relative to the current location, not the guided wp location, which could be quite important.
58 lines
1.3 KiB
C++
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;
|
|
}
|