You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
57 lines
1.3 KiB
57 lines
1.3 KiB
#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; |
|
}
|
|
|