2014-01-28 09:44:37 -04:00
|
|
|
/// -*- 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
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
static Vector3f posvel_pos_target_cm;
|
|
|
|
static Vector3f posvel_vel_target_cms;
|
|
|
|
static uint32_t posvel_update_time_ms;
|
|
|
|
|
2014-10-13 05:41:48 -03:00
|
|
|
struct Guided_Limit {
|
|
|
|
uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
|
|
|
|
float alt_min_cm; // lower altitude limit in cm above home (0 = no limit)
|
|
|
|
float alt_max_cm; // upper altitude limit in cm above home (0 = no limit)
|
|
|
|
float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
|
|
|
|
uint32_t start_time;// system time in milliseconds that control was handed to the external computer
|
|
|
|
Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit
|
|
|
|
} guided_limit;
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// guided_init - initialise guided controller
|
|
|
|
static bool guided_init(bool ignore_checks)
|
|
|
|
{
|
2015-01-02 07:43:50 -04:00
|
|
|
if (position_ok() || ignore_checks) {
|
2014-07-06 04:43:29 -03:00
|
|
|
// initialise yaw
|
|
|
|
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
2014-06-02 06:06:11 -03:00
|
|
|
// start in position control mode
|
|
|
|
guided_pos_control_start();
|
2014-01-28 09:44:37 -04:00
|
|
|
return true;
|
|
|
|
}else{
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
|
|
|
|
// guided_takeoff_start - initialises waypoint controller to implement take-off
|
|
|
|
static void guided_takeoff_start(float final_alt)
|
|
|
|
{
|
|
|
|
guided_mode = Guided_TakeOff;
|
|
|
|
|
|
|
|
// initialise wpnav destination
|
|
|
|
Vector3f target_pos = inertial_nav.get_position();
|
|
|
|
target_pos.z = final_alt;
|
|
|
|
wp_nav.set_wp_destination(target_pos);
|
|
|
|
|
|
|
|
// initialise yaw
|
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
|
|
|
|
|
|
// tell motors to do a slow start
|
|
|
|
motors.slow_start(true);
|
|
|
|
}
|
|
|
|
|
2014-06-02 06:06:11 -03:00
|
|
|
// initialise guided mode's position controller
|
2014-11-14 17:23:53 -04:00
|
|
|
static void guided_pos_control_start()
|
2014-06-02 06:06:11 -03:00
|
|
|
{
|
|
|
|
// 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);
|
|
|
|
|
2014-07-06 04:43:29 -03:00
|
|
|
// initialise yaw
|
|
|
|
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// initialise guided mode's velocity controller
|
2014-11-14 17:23:53 -04:00
|
|
|
static void guided_vel_control_start()
|
2014-06-02 06:06:11 -03:00
|
|
|
{
|
|
|
|
// 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();
|
|
|
|
}
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// initialise guided mode's posvel controller
|
|
|
|
static void guided_posvel_control_start()
|
|
|
|
{
|
|
|
|
// set guided_mode to velocity controller
|
|
|
|
guided_mode = Guided_PosVel;
|
|
|
|
|
|
|
|
pos_control.init_xy_controller();
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// set speed and acceleration from wpnav's speed and acceleration
|
|
|
|
pos_control.set_speed_xy(wp_nav.get_speed_xy());
|
2014-11-25 16:15:45 -04:00
|
|
|
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
|
|
|
|
|
|
|
|
const Vector3f& curr_pos = inertial_nav.get_position();
|
|
|
|
const Vector3f& curr_vel = inertial_nav.get_velocity();
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// set target position and velocity to current position and velocity
|
2014-11-25 16:15:45 -04:00
|
|
|
pos_control.set_xy_target(curr_pos.x, curr_pos.y);
|
|
|
|
pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// set vertical speed and acceleration
|
2014-11-25 16:15:45 -04:00
|
|
|
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
|
|
|
|
pos_control.set_accel_z(wp_nav.get_accel_z());
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// pilot always controls yaw
|
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
2014-11-25 16:15:45 -04:00
|
|
|
}
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// guided_set_destination - sets guided mode's target destination
|
|
|
|
static void guided_set_destination(const Vector3f& destination)
|
|
|
|
{
|
2014-06-02 06:06:11 -03:00
|
|
|
// ensure we are in position control mode
|
|
|
|
if (guided_mode != Guided_WP) {
|
|
|
|
guided_pos_control_start();
|
|
|
|
}
|
|
|
|
|
2014-05-23 02:26:41 -03:00
|
|
|
wp_nav.set_wp_destination(destination);
|
2014-01-28 09:44:37 -04:00
|
|
|
}
|
|
|
|
|
2014-06-02 06:06:11 -03:00
|
|
|
// 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
|
2014-06-11 23:53:02 -03:00
|
|
|
pos_control.set_desired_velocity(velocity);
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// set guided mode posvel target
|
|
|
|
static void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) {
|
|
|
|
// check we are in velocity control mode
|
|
|
|
if (guided_mode != Guided_PosVel) {
|
|
|
|
guided_posvel_control_start();
|
|
|
|
}
|
|
|
|
|
|
|
|
posvel_update_time_ms = millis();
|
|
|
|
posvel_pos_target_cm = destination;
|
|
|
|
posvel_vel_target_cms = velocity;
|
|
|
|
|
|
|
|
pos_control.set_pos_target(posvel_pos_target_cm);
|
|
|
|
}
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// 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?
|
2014-06-06 00:04:34 -03:00
|
|
|
attitude_control.relax_bf_rate_controller();
|
2014-06-06 02:45:49 -03:00
|
|
|
attitude_control.set_yaw_target_to_current_heading();
|
2014-01-28 09:44:37 -04:00
|
|
|
attitude_control.set_throttle_out(0, false);
|
|
|
|
// To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
// call the correct auto controller
|
|
|
|
switch (guided_mode) {
|
|
|
|
|
|
|
|
case Guided_TakeOff:
|
|
|
|
// run takeoff controller
|
|
|
|
guided_takeoff_run();
|
|
|
|
break;
|
|
|
|
|
|
|
|
case Guided_WP:
|
|
|
|
// run position controller
|
2014-06-02 06:06:11 -03:00
|
|
|
guided_pos_control_run();
|
2014-08-04 11:11:23 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case Guided_Velocity:
|
|
|
|
// run velocity controller
|
2014-06-02 06:06:11 -03:00
|
|
|
guided_vel_control_run();
|
2014-11-14 17:26:45 -04:00
|
|
|
break;
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
case Guided_PosVel:
|
2015-01-06 23:33:54 -04:00
|
|
|
// run position-velocity controller
|
2014-11-25 16:15:45 -04:00
|
|
|
guided_posvel_control_run();
|
|
|
|
break;
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
// guided_takeoff_run - takeoff in guided mode
|
|
|
|
// called by guided_run at 100hz or more
|
|
|
|
static void guided_takeoff_run()
|
|
|
|
{
|
|
|
|
// if not auto armed set throttle to zero and exit immediately
|
|
|
|
if(!ap.auto_armed) {
|
2014-09-29 03:26:54 -03:00
|
|
|
// initialise wpnav targets
|
|
|
|
wp_nav.shift_wp_origin_to_current_pos();
|
2014-08-04 11:11:23 -03:00
|
|
|
// reset attitude control targets
|
|
|
|
attitude_control.relax_bf_rate_controller();
|
|
|
|
attitude_control.set_yaw_target_to_current_heading();
|
|
|
|
attitude_control.set_throttle_out(0, false);
|
|
|
|
// tell motors to do a slow start
|
|
|
|
motors.slow_start(true);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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();
|
|
|
|
|
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
|
2014-06-02 06:06:11 -03:00
|
|
|
// guided_pos_control_run - runs the guided position controller
|
|
|
|
// called from guided_run
|
|
|
|
static void guided_pos_control_run()
|
|
|
|
{
|
2014-01-28 09:44:37 -04:00
|
|
|
// 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
|
2014-02-13 22:56:46 -04:00
|
|
|
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
2014-01-28 09:44:37 -04:00
|
|
|
}else{
|
|
|
|
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
2014-06-02 06:06:11 -03:00
|
|
|
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
|
2014-11-15 23:06:05 -04:00
|
|
|
pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
|
2014-06-02 06:06:11 -03:00
|
|
|
|
|
|
|
// 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);
|
2014-01-28 09:44:37 -04:00
|
|
|
}
|
|
|
|
}
|
2014-10-13 05:41:48 -03:00
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// guided_posvel_control_run - runs the guided spline controller
|
|
|
|
// called from guided_run
|
|
|
|
static void guided_posvel_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);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// set velocity to zero if no updates received for 3 seconds
|
2014-11-25 16:15:45 -04:00
|
|
|
uint32_t tnow = millis();
|
2015-01-06 23:33:54 -04:00
|
|
|
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
|
2014-11-25 16:15:45 -04:00
|
|
|
posvel_vel_target_cms.zero();
|
|
|
|
}
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// advance position target using velocity target
|
2014-11-25 16:15:45 -04:00
|
|
|
posvel_pos_target_cm += posvel_vel_target_cms * G_Dt;
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// send position and velocity targets to position controller
|
2014-11-25 16:15:45 -04:00
|
|
|
pos_control.set_pos_target(posvel_pos_target_cm);
|
|
|
|
pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
|
2015-01-06 23:33:54 -04:00
|
|
|
|
|
|
|
// run position controller
|
2015-02-06 03:59:35 -04:00
|
|
|
pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler);
|
2014-11-25 16:15:45 -04:00
|
|
|
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(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);
|
|
|
|
}
|
|
|
|
}
|
2014-11-14 17:26:45 -04:00
|
|
|
|
2014-10-13 05:41:48 -03:00
|
|
|
// Guided Limit code
|
|
|
|
|
|
|
|
// guided_limit_clear - clear/turn off guided limits
|
|
|
|
static void guided_limit_clear()
|
|
|
|
{
|
|
|
|
guided_limit.timeout_ms = 0;
|
|
|
|
guided_limit.alt_min_cm = 0.0f;
|
|
|
|
guided_limit.alt_max_cm = 0.0f;
|
|
|
|
guided_limit.horiz_max_cm = 0.0f;
|
|
|
|
}
|
|
|
|
|
|
|
|
// guided_limit_set - set guided timeout and movement limits
|
|
|
|
static void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
|
|
|
|
{
|
|
|
|
guided_limit.timeout_ms = timeout_ms;
|
|
|
|
guided_limit.alt_min_cm = alt_min_cm;
|
|
|
|
guided_limit.alt_max_cm = alt_max_cm;
|
|
|
|
guided_limit.horiz_max_cm = horiz_max_cm;
|
|
|
|
}
|
|
|
|
|
|
|
|
// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
|
|
|
|
// only called from AUTO mode's auto_nav_guided_start function
|
|
|
|
static void guided_limit_init_time_and_pos()
|
|
|
|
{
|
|
|
|
// initialise start time
|
|
|
|
guided_limit.start_time = hal.scheduler->millis();
|
|
|
|
|
|
|
|
// initialise start position from current position
|
|
|
|
guided_limit.start_pos = inertial_nav.get_position();
|
|
|
|
}
|
|
|
|
|
|
|
|
// guided_limit_check - returns true if guided mode has breached a limit
|
|
|
|
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
|
|
|
|
static bool guided_limit_check()
|
|
|
|
{
|
|
|
|
// check if we have passed the timeout
|
|
|
|
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// get current location
|
|
|
|
const Vector3f& curr_pos = inertial_nav.get_position();
|
|
|
|
|
|
|
|
// check if we have gone below min alt
|
|
|
|
if ((guided_limit.alt_min_cm != 0.0f) && (curr_pos.z < guided_limit.alt_min_cm)) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if we have gone above max alt
|
|
|
|
if ((guided_limit.alt_max_cm != 0.0f) && (curr_pos.z > guided_limit.alt_max_cm)) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if we have gone beyond horizontal limit
|
|
|
|
if (guided_limit.horiz_max_cm > 0.0f) {
|
|
|
|
float horiz_move = pv_get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
|
|
|
|
if (horiz_move > guided_limit.horiz_max_cm) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// if we got this far we must be within limits
|
|
|
|
return false;
|
|
|
|
}
|