ardupilot/ArduCopter/control_auto.cpp

867 lines
31 KiB
C++
Raw Normal View History

#include "Copter.h"
/*
* 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
bool Copter::auto_init(bool ignore_checks)
{
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
2017-01-05 14:07:14 -04:00
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false;
}
// 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
void Copter::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;
2016-11-17 01:19:22 -04:00
case Auto_NavPayloadPlace:
auto_payload_place_run();
break;
2014-01-24 22:37:42 -04:00
}
}
// auto_takeoff_start - initialises waypoint controller to implement take-off
void Copter::auto_takeoff_start(const Location& dest_loc)
2014-01-24 22:37:42 -04:00
{
auto_mode = Auto_TakeOff;
2014-01-24 22:37:42 -04:00
// convert location to class
Location_Class dest(dest_loc);
// set horizontal target
dest.lat = current_loc.lat;
dest.lng = current_loc.lng;
// get altitude target
int32_t alt_target;
if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) {
// this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
// fall back to altitude above current altitude
alt_target = current_loc.alt + dest.alt;
}
// sanity check target
if (alt_target < current_loc.alt) {
dest.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
}
// Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
if (alt_target < 100) {
dest.set_alt_cm(100, Location_Class::ALT_FRAME_ABOVE_HOME);
}
// set waypoint controller target
if (!wp_nav->set_wp_destination(dest)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
}
2014-01-24 22:37:42 -04:00
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
// clear i term when we're taking off
set_throttle_takeoff();
// get initial alt for WP_NAVALT_MIN
auto_takeoff_set_start_alt();
2014-01-24 22:37:42 -04:00
}
// auto_takeoff_run - takeoff in auto mode
// called by auto_run at 100hz or more
void Copter::auto_takeoff_run()
2014-01-24 22:37:42 -04:00
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
2014-01-24 22:37:42 -04:00
// reset attitude control targets
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
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
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
2014-01-24 22:37:42 -04:00
}
#if FRAME_CONFIG == HELI_FRAME
// helicopters stay in landed state until rotor speed runup has finished
if (motors->rotor_runup_complete()) {
set_land_complete(false);
} else {
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
}
#else
set_land_complete(false);
#endif
2016-01-13 03:09:03 -04:00
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
2016-01-13 03:09:03 -04:00
2014-01-24 22:37:42 -04:00
// run waypoint controller
failsafe_terrain_set_status(wp_nav->update_wpnav());
2014-01-24 22:37:42 -04:00
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
2014-01-24 22:37:42 -04:00
// call attitude controller
auto_takeoff_attitude_run(target_yaw_rate);
2014-01-24 22:37:42 -04:00
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Copter::auto_wp_start(const Vector3f& destination)
2014-01-24 22:37:42 -04:00
{
auto_mode = Auto_WP;
2014-01-24 22:37:42 -04:00
// initialise wpnav (no need to check return status because terrain data is not used)
wp_nav->set_wp_destination(destination, false);
2014-01-24 22:37:42 -04: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));
}
2014-01-24 22:37:42 -04:00
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Copter::auto_wp_start(const Location_Class& dest_loc)
{
auto_mode = Auto_WP;
// send target to waypoint controller
if (!wp_nav->set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
}
// 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
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
void Copter::auto_wp_run()
2014-01-24 22:37:42 -04:00
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
2014-01-24 22:37:42 -04:00
// 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)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
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
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
2015-05-04 23:34:21 -03:00
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
2016-01-13 03:09:03 -04:00
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
2016-01-13 03:09:03 -04:00
// run waypoint controller
failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
2014-03-22 00:48:54 -03:00
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
2014-03-22 00:48:54 -03:00
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain());
2014-03-22 00:48:54 -03:00
}
}
// 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
void Copter::auto_spline_start(const Location_Class& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Location_Class& next_destination)
2014-03-22 00:48:54 -03:00
{
auto_mode = Auto_Spline;
// initialise wpnav
if (!wp_nav->set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
}
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
void Copter::auto_spline_run()
2014-03-22 00:48:54 -03:00
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
2014-03-22 00:48:54 -03:00
// 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)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
2014-03-22 00:48:54 -03:00
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rat
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
2015-05-04 23:34:21 -03:00
if (!is_zero(target_yaw_rate)) {
2014-03-22 00:48:54 -03:00
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
2016-01-13 03:09:03 -04:00
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
2016-01-13 03:09:03 -04:00
2014-03-22 00:48:54 -03:00
// run waypoint controller
wp_nav->update_spline();
2014-03-22 00:48:54 -03:00
// 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->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
2014-01-24 22:37:42 -04:00
}
// auto_land_start - initialises controller to implement a landing
void Copter::auto_land_start()
2014-01-24 22:37:42 -04:00
{
// 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
void Copter::auto_land_start(const Vector3f& destination)
2014-01-24 22:37:42 -04:00
{
auto_mode = Auto_Land;
2014-01-24 22:37:42 -04:00
// initialise loiter target destination
wp_nav->init_loiter_target(destination);
// initialise position and desired velocity
if (!pos_control->is_active_z()) {
pos_control->set_alt_target(inertial_nav.get_altitude());
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_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
void Copter::auto_land_run()
2014-01-24 22:37:42 -04:00
{
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
2016-01-13 03:09:03 -04:00
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
2016-01-13 03:09:03 -04:00
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav->init_loiter_target();
2014-01-24 22:37:42 -04:00
return;
}
2016-01-13 03:09:03 -04:00
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
land_run_horizontal_control();
land_run_vertical_control();
}
2014-01-27 23:20:39 -04:00
// auto_rtl_start - initialises RTL in AUTO flight mode
void Copter::auto_rtl_start()
2014-01-27 23:20:39 -04:00
{
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 Copter::auto_rtl_run()
2014-01-27 23:20:39 -04:00
{
// 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 performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
{
// convert location to vector from ekf origin
Vector3f circle_center_neu;
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error
circle_center_neu = inertial_nav.get_position();
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
}
circle_nav->set_center(circle_center_neu);
// set circle radius
if (!is_zero(radius_m)) {
circle_nav->set_radius(radius_m * 100.0f);
}
// check our distance from edge of circle
Vector3f circle_edge_neu;
circle_nav->get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// convert circle_edge_neu to Location_Class
Location_Class circle_edge(circle_edge_neu);
// convert altitude to same as command
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle
if (!wp_nav->set_wp_destination(circle_edge)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.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);
}
} else {
auto_circle_start();
}
}
2014-01-27 10:43:48 -04:00
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
// assumes that circle_nav object has already been initialised with circle center and radius
void Copter::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
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 Copter::auto_circle_run()
2014-01-27 10:43:48 -04:00
{
// call circle controller
circle_nav->update();
2014-01-27 10:43:48 -04:00
// call z-axis position controller
pos_control->update_z_controller();
2014-01-27 10:43:48 -04:00
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
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 Copter::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 Copter::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 Copter::auto_loiter_start()
{
2014-10-19 22:54:57 -03:00
// return failure if GPS is bad
if (!position_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 Copter::auto_loiter_run()
2014-10-19 22:54:57 -03:00
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
2016-01-13 03:09:03 -04:00
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
2016-01-13 03:09:03 -04:00
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
2014-10-19 22:54:57 -03:00
// accept pilot input of yaw
float target_yaw_rate = 0;
if(!failsafe.radio) {
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
2016-01-13 03:09:03 -04:00
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
2016-01-13 03:09:03 -04:00
2016-03-11 04:15:47 -04:00
// run waypoint and z-axis position controller
failsafe_terrain_set_status(wp_nav->update_wpnav());
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// 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 Copter::get_default_auto_yaw_mode(bool rtl)
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
}else{
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
}
// set_auto_yaw_mode - sets the yaw mode for auto
void Copter::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
void Copter::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->get_att_target_euler_cd().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
2015-05-04 23:34:21 -03:00
if (is_zero(turn_rate_dps)) {
// 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
void Copter::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.has_pan_control()) {
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_target(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 Copter::get_auto_heading(void)
{
switch(auto_yaw_mode) {
case AUTO_YAW_ROI:
// point towards a location held in roi_WP
return get_roi_yaw();
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;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return get_look_ahead_yaw();
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return initial_armed_bearing;
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
return wp_nav->get_yaw();
}
}
2016-11-17 01:19:22 -04:00
// auto_payload_place_start - initialises controller to implement a placing
void Copter::auto_payload_place_start()
{
// set target to stopping point
Vector3f stopping_point;
wp_nav->get_loiter_stopping_point_xy(stopping_point);
2016-11-17 01:19:22 -04:00
// call location specific place start function
auto_payload_place_start(stopping_point);
}
// auto_payload_place_start - initialises controller to implement placement of a load
void Copter::auto_payload_place_start(const Vector3f& destination)
{
auto_mode = Auto_NavPayloadPlace;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
// initialise loiter target destination
wp_nav->init_loiter_target(destination);
2016-11-17 01:19:22 -04:00
// initialise position and desired velocity
if (!pos_control->is_active_z()) {
pos_control->set_alt_target(inertial_nav.get_altitude());
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
2016-11-17 01:19:22 -04:00
}
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
bool Copter::auto_payload_place_run_should_run()
{
// muts be armed
if (!motors->armed()) {
2016-11-17 01:19:22 -04:00
return false;
}
// muts be auto-armed
if (!ap.auto_armed) {
return false;
}
// must not be landed
if (ap.land_complete) {
return false;
}
// interlock must be enabled (i.e. unsafe)
if (!motors->get_interlock()) {
2016-11-17 01:19:22 -04:00
return false;
}
return true;
}
// auto_payload_place_run - places an object in auto mode
// called by auto_run at 100hz or more
void Copter::auto_payload_place_run()
{
if (!auto_payload_place_run_should_run()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
2016-11-17 01:19:22 -04:00
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
2016-11-17 01:19:22 -04:00
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
2016-11-17 01:19:22 -04:00
#endif
// set target to current position
wp_nav->init_loiter_target();
2016-11-17 01:19:22 -04:00
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
2016-11-17 01:19:22 -04:00
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
case PayloadPlaceStateType_Calibrating_Hover_Start:
case PayloadPlaceStateType_Calibrating_Hover:
return auto_payload_place_run_loiter();
case PayloadPlaceStateType_Descending_Start:
case PayloadPlaceStateType_Descending:
return auto_payload_place_run_descend();
case PayloadPlaceStateType_Releasing_Start:
case PayloadPlaceStateType_Releasing:
case PayloadPlaceStateType_Released:
case PayloadPlaceStateType_Ascending_Start:
case PayloadPlaceStateType_Ascending:
case PayloadPlaceStateType_Done:
return auto_payload_place_run_loiter();
}
}
void Copter::auto_payload_place_run_loiter()
{
// loiter...
land_run_horizontal_control();
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
2016-11-17 01:19:22 -04:00
// call attitude controller
const float target_yaw_rate = 0;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
2016-11-17 01:19:22 -04:00
// call position controller
pos_control->update_z_controller();
2016-11-17 01:19:22 -04:00
}
void Copter::auto_payload_place_run_descend()
{
land_run_horizontal_control();
land_run_vertical_control();
}