ardupilot/ArduCopter/control_guided.pde

165 lines
5.3 KiB
Plaintext

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_guided.pde - init and run calls for guided flight mode
*/
#ifndef GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM
# define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 500 // point nose at target if it is more than 5m away
#endif
static bool guided_pilot_yaw_override_yaw = false;
// guided_init - initialise guided controller
static bool guided_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
guided_pos_control_start();
return true;
}else{
return false;
}
}
// initialise guided mode's position controller
void guided_pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude
// To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude?
Vector3f stopping_point;
stopping_point.z = inertial_nav.get_altitude();
wp_nav.get_wp_stopping_point_xy(stopping_point);
wp_nav.set_wp_destination(stopping_point);
guided_pilot_yaw_override_yaw = false;
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
// initialise guided mode's velocity controller
void guided_vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Velocity;
// initialize vertical speeds and leash lengths
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 velocity controller
pos_control.init_vel_controller_xyz();
}
// guided_set_destination - sets guided mode's target destination
static void guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
wp_nav.set_wp_destination(destination);
}
// guided_set_velocity - sets guided mode's target velocity
static void guided_set_velocity(const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
guided_vel_control_start();
}
// set position controller velocity target
pos_control.set_desired_velocity(velocity);
}
// guided_run - runs the guided controller
// should be called at 100hz or more
static void guided_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// To-Do: reset waypoint controller?
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
// To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
return;
}
// run position or velocity controller
if (guided_mode == Guided_WP) {
guided_pos_control_run();
} else {
// must be velocity controller
guided_vel_control_run();
}
}
// guided_pos_control_run - runs the guided position controller
// called from guided_run
static void guided_pos_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
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) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
}
}
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
static void guided_vel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
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) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// call velocity controller which includes z axis controller
pos_control.update_vel_controller_xyz();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.angle_ef_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
}
}