ardupilot/ArduCopter/mode_guided.cpp

809 lines
29 KiB
C++
Raw Normal View History

#include "Copter.h"
#if MODE_GUIDED_ENABLED == ENABLED
2014-01-28 09:44:37 -04:00
/*
* Init and run calls for guided flight mode
2014-01-28 09:44:37 -04:00
*/
#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
#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates
#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
static Vector3f guided_pos_target_cm; // position target (used by posvel controller only)
static Vector3f guided_vel_target_cms; // velocity target (used by velocity controller and posvel controller)
static uint32_t posvel_update_time_ms; // system time of last target update to posvel controller (i.e. position and velocity update)
static uint32_t vel_update_time_ms; // system time of last target update to velocity controller
2014-11-25 16:15:45 -04:00
struct {
uint32_t update_time_ms;
float roll_cd;
float pitch_cd;
float yaw_cd;
float yaw_rate_cds;
float climb_rate_cms; // climb rate in cms. Used if use_thrust is false
float thrust; // thrust from -1 to 1. Used if use_thrust is true
bool use_yaw_rate;
bool use_thrust;
} static guided_angle_state;
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
bool ModeGuided::init(bool ignore_checks)
2014-01-28 09:44:37 -04:00
{
// start in position control mode
pos_control_start();
send_notification = false;
return true;
2014-01-28 09:44:37 -04:00
}
// guided_run - runs the guided controller
// should be called at 100hz or more
void ModeGuided::run()
{
// call the correct auto controller
switch (guided_mode) {
case SubMode::TakeOff:
// run takeoff controller
takeoff_run();
break;
case SubMode::WP:
// run position controller
pos_control_run();
if (send_notification && wp_nav->reached_wp_destination()) {
send_notification = false;
gcs().send_mission_item_reached_message(0);
}
break;
case SubMode::Velocity:
// run velocity controller
vel_control_run();
break;
case SubMode::PosVel:
// run position-velocity controller
posvel_control_run();
break;
case SubMode::Angle:
// run angle controller
angle_control_run();
break;
}
}
bool ModeGuided::allows_arming(AP_Arming::Method method) const
{
// always allow arming from the ground station
if (method == AP_Arming::Method::MAVLINK) {
return true;
}
// optionally allow arming from the transmitter
return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0;
};
2017-12-12 06:09:48 -04:00
// do_user_takeoff_start - initialises waypoint controller to implement take-off
bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
{
guided_mode = SubMode::TakeOff;
// initialise wpnav destination
Location target_loc = copter.current_loc;
Location::AltFrame frame = Location::AltFrame::ABOVE_HOME;
if (wp_nav->rangefinder_used_and_healthy() &&
wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
// can't takeoff downwards
if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
return false;
}
frame = Location::AltFrame::ABOVE_TERRAIN;
}
target_loc.set_alt_cm(takeoff_alt_cm, frame);
if (!wp_nav->set_wp_destination_loc(target_loc)) {
// failure to set destination can only be because of missing terrain data
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
// initialise yaw
auto_yaw.set_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();
return true;
}
// initialise guided mode's position controller
void ModeGuided::pos_control_start()
{
// set to position control mode
guided_mode = SubMode::WP;
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
// initialise wpnav to stopping point
Vector3f stopping_point;
wp_nav->get_wp_stopping_point(stopping_point);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(stopping_point, false);
// initialise yaw
auto_yaw.set_mode_to_default(false);
}
// initialise guided mode's velocity controller
void ModeGuided::vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = SubMode::Velocity;
2017-11-17 01:10:54 -04:00
// initialise horizontal speed, acceleration
2021-05-11 01:42:02 -03:00
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
2021-05-19 11:07:38 -03:00
// set vertical speed and acceleration limits
2021-05-11 01:42:02 -03:00
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
2021-05-19 11:07:38 -03:00
// initialise the position controller
2021-05-11 01:42:02 -03:00
pos_control->init_z_controller();
pos_control->init_xy_controller();
}
2014-11-25 16:15:45 -04:00
// initialise guided mode's posvel controller
void ModeGuided::posvel_control_start()
2014-11-25 16:15:45 -04:00
{
// set guided_mode to velocity controller
guided_mode = SubMode::PosVel;
2014-11-25 16:15:45 -04:00
2021-05-11 01:42:02 -03:00
// initialise horizontal speed, acceleration
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
2014-11-25 16:15:45 -04:00
2021-05-19 11:07:38 -03:00
// set vertical speed and acceleration limits
2021-05-11 01:42:02 -03:00
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
2014-11-25 16:15:45 -04:00
2021-05-19 11:07:38 -03:00
// initialise the position controller
2021-05-11 01:42:02 -03:00
pos_control->init_z_controller();
pos_control->init_xy_controller();
2014-11-25 16:15:45 -04:00
// pilot always controls yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
2014-11-25 16:15:45 -04:00
}
bool ModeGuided::is_taking_off() const
{
return guided_mode == SubMode::TakeOff;
}
// initialise guided mode's angle controller
void ModeGuided::angle_control_start()
{
// set guided_mode to velocity controller
guided_mode = SubMode::Angle;
2021-05-19 11:07:38 -03:00
// set vertical speed and acceleration limits
2021-05-11 01:42:02 -03:00
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
2021-05-19 11:07:38 -03:00
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
2021-05-11 01:42:02 -03:00
pos_control->init_z_controller();
}
// initialise targets
guided_angle_state.update_time_ms = millis();
guided_angle_state.roll_cd = ahrs.roll_sensor;
guided_angle_state.pitch_cd = ahrs.pitch_sensor;
guided_angle_state.yaw_cd = ahrs.yaw_sensor;
guided_angle_state.climb_rate_cms = 0.0f;
guided_angle_state.yaw_rate_cds = 0.0f;
guided_angle_state.use_yaw_rate = false;
// pilot always controls yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
2014-01-28 09:44:37 -04:00
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt)
2014-01-28 09:44:37 -04:00
{
#if AC_FENCE == ENABLED
// reject destination if outside the fence
const Location dest_loc(destination, terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
if (!copter.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// ensure we are in position control mode
if (guided_mode != SubMode::WP) {
pos_control_start();
}
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(destination, terrain_alt);
2016-01-10 22:41:28 -04:00
// log target
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
send_notification = true;
return true;
2014-01-28 09:44:37 -04:00
}
bool ModeGuided::get_wp(Location& destination)
{
if (guided_mode != SubMode::WP) {
return false;
}
2019-06-05 07:47:32 -03:00
return wp_nav->get_oa_wp_destination(destination);
}
// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
if (!copter.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// ensure we are in position control mode
if (guided_mode != SubMode::WP) {
pos_control_start();
}
if (!wp_nav->set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// log target
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
send_notification = true;
return true;
}
// guided_set_velocity - sets guided mode's target velocity
void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
{
// check we are in velocity control mode
if (guided_mode != SubMode::Velocity) {
vel_control_start();
}
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// record velocity target
guided_vel_target_cms = velocity;
vel_update_time_ms = millis();
2016-01-10 22:41:28 -04:00
// log target
if (log_request) {
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
}
}
2014-11-25 16:15:45 -04:00
// set guided mode posvel target
bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
#if AC_FENCE == ENABLED
// reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
if (!copter.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// check we are in velocity control mode
if (guided_mode != SubMode::PosVel) {
posvel_control_start();
}
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
2014-11-25 16:15:45 -04:00
posvel_update_time_ms = millis();
guided_pos_target_cm = destination;
guided_vel_target_cms = velocity;
2014-11-25 16:15:45 -04:00
2021-05-11 01:42:02 -03:00
copter.pos_control->set_pos_vel_accel(guided_pos_target_cm, guided_vel_target_cms, Vector3f());
2016-01-10 22:41:28 -04:00
// log target
copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
return true;
2014-11-25 16:15:45 -04:00
}
// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
bool ModeGuided::set_attitude_target_provides_thrust() const
{
return ((copter.g2.guided_options.get() & uint32_t(Options::SetAttitudeTarget_ThrustAsThrust)) != 0);
}
// set guided mode angle target and climbrate
void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust)
{
// check we are in velocity control mode
if (guided_mode != SubMode::Angle) {
angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;
guided_angle_state.use_yaw_rate = use_yaw_rate;
guided_angle_state.use_thrust = use_thrust;
if (use_thrust) {
guided_angle_state.thrust = climb_rate_cms_or_thrust;
guided_angle_state.climb_rate_cms = 0.0f;
} else {
guided_angle_state.thrust = 0.0f;
guided_angle_state.climb_rate_cms = climb_rate_cms_or_thrust;
}
guided_angle_state.update_time_ms = millis();
2016-01-10 22:41:28 -04:00
// log target
copter.Log_Write_GuidedTarget(guided_mode,
2016-01-10 22:41:28 -04:00
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust));
}
// guided_takeoff_run - takeoff in guided mode
// called by guided_run at 100hz or more
void ModeGuided::takeoff_run()
{
auto_takeoff_run();
if (wp_nav->reached_wp_destination()) {
#if LANDING_GEAR_ENABLED == ENABLED
// optionally retract landing gear
copter.landinggear.retract_after_takeoff();
#endif
// switch to position control mode but maintain current target
const Vector3f target = wp_nav->get_wp_destination();
set_destination(target, false, 0, false, 0, false, wp_nav->origin_and_destination_are_terrain_alt());
}
}
// guided_pos_control_run - runs the guided position controller
// called from guided_run
void ModeGuided::pos_control_run()
{
2014-01-28 09:44:37 -04:00
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
2014-01-28 09:44:37 -04:00
// 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)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
2014-01-28 09:44:37 -04:00
}
}
// if not armed set throttle to zero and exit immediately
2019-04-08 05:15:57 -03:00
if (is_disarmed_or_landed()) {
make_safe_spool_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
2014-01-28 09:44:37 -04:00
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
2014-01-28 09:44:37 -04:00
2021-05-19 11:07:38 -03:00
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control->update_z_controller();
2014-01-28 09:44:37 -04:00
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
2014-01-28 09:44:37 -04:00
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
}
}
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void ModeGuided::vel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// 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)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// landed with positive desired climb rate, initiate takeoff
if (motors->armed() && copter.ap.auto_armed && copter.ap.land_complete && is_positive(guided_vel_target_cms.z)) {
zero_throttle_and_relax_ac();
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
set_land_complete(false);
set_throttle_takeoff();
}
return;
}
// if not armed set throttle to zero and exit immediately
2019-04-08 05:15:57 -03:00
if (is_disarmed_or_landed()) {
make_safe_spool_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
2021-05-11 01:42:02 -03:00
if (!pos_control->get_vel_desired_cms().is_zero()) {
set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
}
if (auto_yaw.mode() == AUTO_YAW_RATE) {
auto_yaw.set_rate(0.0f);
}
} else {
set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
}
// call velocity controller which includes z axis controller
2021-05-11 01:42:02 -03:00
pos_control->update_xy_controller();
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_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
2014-01-28 09:44:37 -04:00
}
}
2014-11-25 16:15:45 -04:00
// guided_posvel_control_run - runs the guided spline controller
// called from guided_run
void ModeGuided::posvel_control_run()
2014-11-25 16:15:45 -04:00
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
2014-11-25 16:15:45 -04:00
// 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)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
2014-11-25 16:15:45 -04:00
}
}
// if not armed set throttle to zero and exit immediately
2019-04-08 05:15:57 -03:00
if (is_disarmed_or_landed()) {
make_safe_spool_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set velocity to zero and stop rotating if no updates received for 3 seconds
2014-11-25 16:15:45 -04:00
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
guided_vel_target_cms.zero();
if (auto_yaw.mode() == AUTO_YAW_RATE) {
auto_yaw.set_rate(0.0f);
}
2014-11-25 16:15:45 -04:00
}
// advance position target using velocity target
2021-05-19 11:07:38 -03:00
guided_pos_target_cm += guided_vel_target_cms * pos_control->get_dt();
// send position and velocity targets to position controller
2021-06-21 04:22:48 -03:00
pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), Vector2f());
pos_control->input_pos_vel_accel_z(guided_pos_target_cm.z, guided_vel_target_cms.z, 0);
2014-11-25 16:15:45 -04:00
// run position controllers
pos_control->update_xy_controller();
pos_control->update_z_controller();
2014-11-25 16:15:45 -04:00
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
2014-11-25 16:15:45 -04:00
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
2014-11-25 16:15:45 -04:00
}
}
2014-11-14 17:26:45 -04:00
// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void ModeGuided::angle_control_run()
{
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
float yaw_rate_in = guided_angle_state.yaw_rate_cds;
float climb_rate_cms = 0.0f;
if (!guided_angle_state.use_thrust) {
// constrain climb rate
climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up());
// get avoidance adjusted climb rate
climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
}
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
roll_in = 0.0f;
pitch_in = 0.0f;
climb_rate_cms = 0.0f;
yaw_rate_in = 0.0f;
guided_angle_state.use_thrust = false;
}
// interpret positive climb rate or thrust as triggering take-off
const bool positive_thrust_or_climbrate = is_positive(guided_angle_state.use_thrust ? guided_angle_state.thrust : climb_rate_cms);
if (motors->armed() && positive_thrust_or_climbrate) {
copter.set_auto_armed(true);
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && !positive_thrust_or_climbrate)) {
make_safe_spool_down();
return;
}
// TODO: use get_alt_hold_state
// landed with positive desired climb rate, takeoff
if (copter.ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
zero_throttle_and_relax_ac();
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
set_land_complete(false);
set_throttle_takeoff();
}
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// call attitude controller
if (guided_angle_state.use_yaw_rate) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in);
} else {
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
}
// call position controller
if (guided_angle_state.use_thrust) {
attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt);
} else {
2021-05-11 01:42:02 -03:00
pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false);
pos_control->update_z_controller();
}
}
// helper function to update position controller's desired velocity while respecting acceleration limits
void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
{
// get current desired velocity
2021-05-11 01:42:02 -03:00
Vector3f curr_vel_des = vel_des;
#if AC_AVOID_ENABLED
2020-12-06 08:48:21 -04:00
// limit the velocity for obstacle/fence avoidance
2021-05-11 01:42:02 -03:00
copter.avoid.adjust_velocity(curr_vel_des, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt);
#endif
// update position controller with new target
2021-06-21 04:22:48 -03:00
pos_control->input_vel_accel_xy(curr_vel_des.xy(), Vector2f());
pos_control->input_vel_accel_z(curr_vel_des.z, 0, false);
}
// helper function to set yaw state and targets
void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{
if (use_yaw) {
auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle);
} else if (use_yaw_rate) {
auto_yaw.set_rate(yaw_rate_cds);
}
}
// returns true if pilot's yaw input should be used to adjust vehicle's heading
bool ModeGuided::use_pilot_yaw(void) const
{
return (copter.g2.guided_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0;
}
// Guided Limit code
// guided_limit_clear - clear/turn off guided limits
void ModeGuided::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
void ModeGuided::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
void ModeGuided::limit_init_time_and_pos()
{
// initialise start time
guided_limit.start_time = AP_HAL::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
bool ModeGuided::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
2015-05-04 23:34:21 -03:00
if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) {
return true;
}
// check if we have gone above max alt
2015-05-04 23:34:21 -03:00
if (!is_zero(guided_limit.alt_max_cm) && (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 = 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;
}
uint32_t ModeGuided::wp_distance() const
{
switch(guided_mode) {
case SubMode::WP:
return wp_nav->get_wp_distance_to_destination();
break;
case SubMode::PosVel:
2021-05-11 01:42:02 -03:00
return pos_control->get_pos_error_xy_cm();
break;
default:
return 0;
}
}
int32_t ModeGuided::wp_bearing() const
{
switch(guided_mode) {
case SubMode::WP:
return wp_nav->get_wp_bearing_to_destination();
break;
case SubMode::PosVel:
2021-05-11 01:42:02 -03:00
return pos_control->get_bearing_to_target_cd();
break;
case SubMode::TakeOff:
case SubMode::Velocity:
case SubMode::Angle:
// these do not have bearings
return 0;
}
// compiler guarantees we don't get here
return 0.0;
}
2018-05-23 12:34:24 -03:00
float ModeGuided::crosstrack_error() const
2018-05-23 12:34:24 -03:00
{
switch (guided_mode) {
case SubMode::WP:
2018-05-23 12:34:24 -03:00
return wp_nav->crosstrack_error();
case SubMode::TakeOff:
case SubMode::Velocity:
case SubMode::PosVel:
case SubMode::Angle:
// no track to have a crosstrack to
2018-05-23 12:34:24 -03:00
return 0;
}
// compiler guarantees we don't get here
return 0;
2018-05-23 12:34:24 -03:00
}
#endif