mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-31 13:08:34 -04:00
41 lines
1.1 KiB
C++
41 lines
1.1 KiB
C++
|
#include "mode.h"
|
||
|
|
#include "Plane.h"
|
||
|
|
|
||
|
|
bool ModeCruise::_enter()
|
||
|
|
{
|
||
|
|
plane.throttle_allows_nudging = false;
|
||
|
|
plane.auto_throttle_mode = true;
|
||
|
|
plane.auto_navigation_mode = false;
|
||
|
|
plane.cruise_state.locked_heading = false;
|
||
|
|
plane.cruise_state.lock_timer_ms = 0;
|
||
|
|
|
||
|
|
// for ArduSoar soaring_controller
|
||
|
|
plane.g2.soaring_controller.init_cruising();
|
||
|
|
|
||
|
|
plane.set_target_altitude_current();
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
void ModeCruise::update()
|
||
|
|
{
|
||
|
|
/*
|
||
|
|
in CRUISE mode we use the navigation code to control
|
||
|
|
roll when heading is locked. Heading becomes unlocked on
|
||
|
|
any aileron or rudder input
|
||
|
|
*/
|
||
|
|
if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) {
|
||
|
|
plane.cruise_state.locked_heading = false;
|
||
|
|
plane.cruise_state.lock_timer_ms = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!plane.cruise_state.locked_heading) {
|
||
|
|
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
|
||
|
|
plane.update_load_factor();
|
||
|
|
} else {
|
||
|
|
plane.calc_nav_roll();
|
||
|
|
}
|
||
|
|
plane.update_fbwb_speed_height();
|
||
|
|
}
|
||
|
|
|