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.
39 lines
1.0 KiB
39 lines
1.0 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 |
|
*/ |
|
plane.guided_WP_loc = plane.current_loc; |
|
|
|
if (plane.quadplane.guided_mode_enabled()) { |
|
/* |
|
if using Q_GUIDED_MODE then project forward by the stopping distance |
|
*/ |
|
plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), |
|
plane.quadplane.stopping_distance()); |
|
} |
|
plane.set_guided_WP(); |
|
return true; |
|
} |
|
|
|
void ModeGuided::update() |
|
{ |
|
if (plane.auto_state.vtol_loiter && plane.quadplane.available()) { |
|
plane.quadplane.guided_update(); |
|
} else { |
|
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); |
|
}
|
|
|