mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 07:58:28 -04:00
199dc3454d
position_ok uses the EKF's filter status if the EKF is being used otherwise it falls back to the GPS based checks used by inertial nav
104 lines
3.6 KiB
Plaintext
104 lines
3.6 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
|
* control_loiter.pde - init and run calls for loiter flight mode
|
|
*/
|
|
|
|
// loiter_init - initialise loiter controller
|
|
static bool loiter_init(bool ignore_checks)
|
|
{
|
|
if (position_ok() || ignore_checks) {
|
|
|
|
// set target to current position
|
|
wp_nav.init_loiter_target();
|
|
|
|
// initialize vertical speed and accelerationj
|
|
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
|
pos_control.set_accel_z(g.pilot_accel_z);
|
|
|
|
// initialise altitude target to stopping point
|
|
pos_control.set_target_to_stopping_point_z();
|
|
|
|
return true;
|
|
}else{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// loiter_run - runs the loiter controller
|
|
// should be called at 100hz or more
|
|
static void loiter_run()
|
|
{
|
|
float target_yaw_rate = 0;
|
|
float target_climb_rate = 0;
|
|
|
|
// if not auto armed set throttle to zero and exit immediately
|
|
if(!ap.auto_armed || !inertial_nav.get_filter_status().flags.horiz_pos_abs) {
|
|
wp_nav.init_loiter_target();
|
|
attitude_control.relax_bf_rate_controller();
|
|
attitude_control.set_yaw_target_to_current_heading();
|
|
attitude_control.set_throttle_out(0, false);
|
|
pos_control.set_alt_target_to_current_alt();
|
|
return;
|
|
}
|
|
|
|
// process pilot inputs
|
|
if (!failsafe.radio) {
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// process pilot's roll and pitch input
|
|
wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in);
|
|
|
|
// get pilot's desired yaw rate
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
|
|
|
// get pilot desired climb rate
|
|
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
|
|
|
|
// check for pilot requested take-off
|
|
if (ap.land_complete && target_climb_rate > 0) {
|
|
// indicate we are taking off
|
|
set_land_complete(false);
|
|
// clear i term when we're taking off
|
|
set_throttle_takeoff();
|
|
}
|
|
} else {
|
|
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
|
wp_nav.clear_pilot_desired_acceleration();
|
|
}
|
|
|
|
// relax loiter target if we might be landed
|
|
if (land_complete_maybe()) {
|
|
wp_nav.loiter_soften_for_landing();
|
|
}
|
|
|
|
// when landed reset targets and output zero throttle
|
|
if (ap.land_complete) {
|
|
wp_nav.init_loiter_target();
|
|
attitude_control.relax_bf_rate_controller();
|
|
attitude_control.set_yaw_target_to_current_heading();
|
|
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
|
|
attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
|
|
pos_control.set_alt_target_to_current_alt();
|
|
}else{
|
|
// run loiter controller
|
|
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
|
|
|
// call attitude controller
|
|
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
|
|
|
// body-frame rate controller is run directly from 100hz loop
|
|
|
|
// run altitude controller
|
|
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
|
|
// if sonar is ok, use surface tracking
|
|
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
|
}
|
|
|
|
// update altitude target and call position controller
|
|
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
|
pos_control.update_z_controller();
|
|
}
|
|
}
|