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
84 lines
2.9 KiB
Plaintext
84 lines
2.9 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
|
* control_circle.pde - init and run calls for circle flight mode
|
|
*/
|
|
|
|
// circle_init - initialise circle controller flight mode
|
|
static bool circle_init(bool ignore_checks)
|
|
{
|
|
if ((position_ok() && inertial_nav.get_filter_status().flags.horiz_pos_abs) || ignore_checks) {
|
|
circle_pilot_yaw_override = false;
|
|
|
|
// initialize speeds and accelerations
|
|
pos_control.set_speed_xy(wp_nav.get_speed_xy());
|
|
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
|
|
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 circle controller including setting the circle center based on vehicle speed
|
|
circle_nav.init();
|
|
|
|
return true;
|
|
}else{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// circle_run - runs the circle flight mode
|
|
// should be called at 100hz or more
|
|
static void circle_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 || ap.land_complete) {
|
|
// To-Do: add some initialisation of position controllers
|
|
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) {
|
|
// get pilot's desired yaw rate
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
|
if (target_yaw_rate != 0) {
|
|
circle_pilot_yaw_override = true;
|
|
}
|
|
|
|
// 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();
|
|
}
|
|
}
|
|
|
|
// run circle controller
|
|
circle_nav.update();
|
|
|
|
// call attitude controller
|
|
if (circle_pilot_yaw_override) {
|
|
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
|
|
}else{
|
|
attitude_control.angle_ef_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
|
|
}
|
|
|
|
// 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();
|
|
}
|