mirror of https://github.com/ArduPilot/ardupilot
38 lines
752 B
C++
38 lines
752 B
C++
#include "mode.h"
|
|
#include "Plane.h"
|
|
|
|
#if SOARING_ENABLED == ENABLED
|
|
|
|
bool ModeThermal::_enter()
|
|
{
|
|
if (!plane.g2.soaring_controller.is_active()) {
|
|
return false;
|
|
}
|
|
|
|
plane.throttle_allows_nudging = true;
|
|
plane.auto_throttle_mode = true;
|
|
plane.auto_navigation_mode = true;
|
|
plane.do_loiter_at_location();
|
|
plane.loiter_angle_reset();
|
|
|
|
plane.g2.soaring_controller.init_thermalling();
|
|
plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path
|
|
|
|
return true;
|
|
}
|
|
|
|
void ModeThermal::update()
|
|
{
|
|
plane.calc_nav_roll();
|
|
plane.calc_nav_pitch();
|
|
plane.calc_throttle();
|
|
}
|
|
|
|
void ModeThermal::navigate()
|
|
{
|
|
// Zero indicates to use WP_LOITER_RAD
|
|
plane.update_loiter(0);
|
|
}
|
|
|
|
#endif
|