ardupilot/ArduCopter/control_auto.pde

645 lines
22 KiB
Plaintext
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_auto.pde - init and run calls for auto flight mode
2014-01-24 22:37:42 -04:00
*
* This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode
* Command execution code (i.e. command_logic.pde) should:
2014-01-27 23:20:39 -04:00
* a) switch to Auto flight mode with set_mode() function. This will cause auto_init to be called
* b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start()
2014-01-24 22:37:42 -04:00
* c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed
* The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call
* correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature
*/
/*
* While in the auto flight mode, navigation or do/now commands can be run.
2014-01-27 23:20:39 -04:00
* Code in this file implements the navigation commands
*/
// auto_init - initialise auto controller
static bool auto_init(bool ignore_checks)
{
if ((GPS_ok() && inertial_nav.position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// clear guided limits
guided_limit_clear();
2014-05-15 04:20:02 -03:00
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
return true;
}else{
return false;
}
}
// auto_run - runs the auto controller
// should be called at 100hz or more
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
static void auto_run()
{
2014-01-24 22:37:42 -04:00
// call the correct auto controller
switch (auto_mode) {
case Auto_TakeOff:
2014-01-24 22:37:42 -04:00
auto_takeoff_run();
break;
case Auto_WP:
case Auto_CircleMoveToEdge:
2014-01-24 22:37:42 -04:00
auto_wp_run();
break;
case Auto_Land:
2014-01-24 22:37:42 -04:00
auto_land_run();
break;
2014-01-27 10:43:48 -04:00
2014-01-27 23:20:39 -04:00
case Auto_RTL:
auto_rtl_run();
break;
2014-01-27 10:43:48 -04:00
case Auto_Circle:
auto_circle_run();
break;
2014-03-22 00:48:54 -03:00
case Auto_Spline:
auto_spline_run();
break;
case Auto_NavGuided:
#if NAV_GUIDED == ENABLED
auto_nav_guided_run();
#endif
break;
case Auto_Loiter:
auto_loiter_run();
break;
2014-01-24 22:37:42 -04:00
}
}
// auto_takeoff_start - initialises waypoint controller to implement take-off
static void auto_takeoff_start(float final_alt)
{
auto_mode = Auto_TakeOff;
2014-01-24 22:37:42 -04:00
// 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-01-24 22:37:42 -04:00
}
// auto_takeoff_run - takeoff in auto mode
// called by auto_run at 100hz or more
static void auto_takeoff_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
2014-01-24 22:37:42 -04:00
// reset attitude control targets
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
2014-01-24 22:37:42 -04:00
attitude_control.set_throttle_out(0, false);
// tell motors to do a slow start
motors.slow_start(true);
2014-01-24 22:37:42 -04:00
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
2014-01-24 22:37:42 -04:00
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-01-24 22:37:42 -04:00
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
static void auto_wp_start(const Vector3f& destination)
{
auto_mode = Auto_WP;
2014-01-24 22:37:42 -04:00
// initialise wpnav
wp_nav.set_wp_destination(destination);
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
2014-01-24 22:37:42 -04:00
}
2014-01-24 22:37:42 -04:00
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
static void auto_wp_run()
{
// if not auto armed set throttle to zero and exit immediately
2014-01-24 22:37:42 -04:00
if(!ap.auto_armed) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
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
2014-01-24 22:37:42 -04:00
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)
2014-03-22 00:48:54 -03: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(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);
}
}
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_destination)
2014-03-22 00:48:54 -03:00
{
auto_mode = Auto_Spline;
// initialise wpnav
wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination);
2014-03-22 00:48:54 -03:00
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
}
// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
static void auto_spline_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
2014-03-22 00:48:54 -03:00
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);
if (target_yaw_rate != 0) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_spline();
// 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()
2014-03-23 08:41:32 -03:00
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
}
2014-01-24 22:37:42 -04:00
}
// auto_land_start - initialises controller to implement a landing
static void auto_land_start()
{
// set target to stopping point
Vector3f stopping_point;
wp_nav.get_loiter_stopping_point_xy(stopping_point);
2014-01-24 22:37:42 -04:00
// call location specific land start function
auto_land_start(stopping_point);
2014-01-24 22:37:42 -04:00
}
// auto_land_start - initialises controller to implement a landing
static void auto_land_start(const Vector3f& destination)
{
auto_mode = Auto_Land;
2014-01-24 22:37:42 -04:00
// initialise loiter target destination
wp_nav.init_loiter_target(destination);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
2014-01-24 22:37:42 -04:00
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// auto_land_run - lands in auto mode
// called by auto_run at 100hz or more
static void auto_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
2014-01-24 22:37:42 -04:00
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
2014-01-24 22:37:42 -04:00
attitude_control.set_throttle_out(0, false);
// set target to current position
wp_nav.init_loiter_target();
2014-01-24 22:37:42 -04:00
return;
}
// relax loiter targets if we might be landed
if (land_complete_maybe()) {
wp_nav.loiter_soften_for_landing();
}
// process pilot's input
2014-01-24 22:37:42 -04:00
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = g.rc_1.control_in;
pitch_control = g.rc_2.control_in;
}
2014-01-24 22:37:42 -04:00
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
}
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter();
2014-01-24 22:37:42 -04:00
// call z-axis position controller
pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt, true);
2014-01-24 22:37:42 -04:00
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-01-27 23:20:39 -04:00
// auto_rtl_start - initialises RTL in AUTO flight mode
static void auto_rtl_start()
{
auto_mode = Auto_RTL;
// call regular rtl flight mode initialisation and ask it to ignore checks
rtl_init(true);
}
// auto_rtl_run - rtl in AUTO flight mode
// called by auto_run at 100hz or more
void auto_rtl_run()
{
// call regular rtl flight mode run function
rtl_run();
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
static void auto_circle_movetoedge_start()
{
// check our distance from edge of circle
Vector3f circle_edge;
circle_nav.get_closest_point_on_circle(circle_edge);
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// initialise wpnav to move to edge of circle
wp_nav.set_wp_destination(circle_edge);
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f &circle_center = circle_nav.get_center();
float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
2014-01-27 10:43:48 -04:00
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
static void auto_circle_start()
2014-01-27 10:43:48 -04:00
{
2014-01-27 23:20:39 -04:00
auto_mode = Auto_Circle;
// initialise circle controller
// center was set in do_circle so initialise with current center
circle_nav.init(circle_nav.get_center());
2014-01-27 10:43:48 -04:00
}
// auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more
void auto_circle_run()
{
// call circle controller
circle_nav.update();
// call z-axis position controller
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angle_ef_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
2014-01-27 10:43:48 -04:00
}
#if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
void auto_nav_guided_start()
{
auto_mode = Auto_NavGuided;
// call regular guided flight mode initialisation
guided_init(true);
// initialise guided start time and position as reference for limit checking
guided_limit_init_time_and_pos();
}
// auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more
void auto_nav_guided_run()
{
// call regular guided flight mode run function
guided_run();
}
#endif // NAV_GUIDED
2014-10-19 22:54:57 -03:00
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool auto_loiter_start()
{
2014-10-19 22:54:57 -03:00
// return failure if GPS is bad
if (!GPS_ok()) {
return false;
}
auto_mode = Auto_Loiter;
Vector3f origin = inertial_nav.get_position();
2014-10-19 22:54:57 -03:00
// calculate stopping point
Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point);
2014-10-19 22:54:57 -03:00
// initialise waypoint controller target to stopping point
wp_nav.set_wp_origin_and_destination(origin, stopping_point);
2014-10-19 22:54:57 -03:00
// hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD);
return true;
}
2014-10-19 22:54:57 -03:00
// auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
void auto_loiter_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
return;
}
2014-10-19 22:54:57 -03:00
// accept pilot input of yaw
float target_yaw_rate = 0;
if(!failsafe.radio) {
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
}
2014-10-19 22:54:57 -03:00
// run waypoint and z-axis postion controller
wp_nav.update_wpnav();
pos_control.update_z_controller();
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t get_default_auto_yaw_mode(bool rtl)
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
}else{
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
break;
}
}
// set_auto_yaw_mode - sets the yaw mode for auto
void set_auto_yaw_mode(uint8_t yaw_mode)
{
// return immediately if no change
if (auto_yaw_mode == yaw_mode) {
return;
}
auto_yaw_mode = yaw_mode;
// perform initialisation
switch (auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_NEXT_WP:
2014-03-23 08:41:32 -03:00
// wpnav will initialise heading when wpnav's set_destination method is called
break;
case AUTO_YAW_ROI:
// point towards a location held in yaw_look_at_WP
yaw_look_at_WP_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading
// caller should set the yaw_look_at_heading
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
yaw_look_ahead_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required
break;
}
}
2014-10-20 23:52:22 -03:00
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
{
// get current yaw target
int32_t curr_yaw_target = attitude_control.angle_ef_targets().z;
// get final angle, 1 = Relative, 0 = Absolute
if (relative_angle == 0) {
// absolute angle
yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
} else {
// relative angle
if (direction < 0) {
angle_deg = -angle_deg;
}
yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
}
// get turn speed
if (turn_rate_dps == 0 ) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
}
// set yaw mode
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
// TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
}
// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
static void set_auto_yaw_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if MOUNT == ENABLED
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
}
#endif // MOUNT == ENABLED
}else{
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) {
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&roi_location);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
// 1: point at next waypoint
// 2: point at a waypoint taken from WP# parameter (2nd parameter?)
// 3: point at a location given by alt, lon, lat parameters
// 4: point at a target given a target id (can't be implemented)
#else
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
#endif // MOUNT == ENABLED
}
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float get_auto_heading(void)
{
switch(auto_yaw_mode) {
case AUTO_YAW_ROI:
// point towards a location held in roi_WP
return get_roi_yaw();
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
return yaw_look_at_heading;
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return get_look_ahead_yaw();
break;
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return initial_armed_bearing;
break;
case AUTO_YAW_LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight
2014-03-23 08:41:32 -03:00
return wp_nav.get_yaw();
break;
}
}