2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
/*
|
2016-07-25 15:45:29 -03: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
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates
|
2015-10-10 09:41:42 -03:00
|
|
|
#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
|
2015-01-06 23:33:54 -04:00
|
|
|
|
2016-07-21 04:14:21 -03:00
|
|
|
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
|
|
|
|
2015-10-08 08:13:53 -03:00
|
|
|
struct {
|
|
|
|
uint32_t update_time_ms;
|
|
|
|
float roll_cd;
|
|
|
|
float pitch_cd;
|
|
|
|
float yaw_cd;
|
2016-08-02 14:20:57 -03:00
|
|
|
float yaw_rate_cds;
|
2015-10-08 08:13:53 -03:00
|
|
|
float climb_rate_cms;
|
2016-08-02 14:20:57 -03:00
|
|
|
bool use_yaw_rate;
|
2017-07-10 09:51:43 -03:00
|
|
|
} static guided_angle_state;
|
2015-10-08 08:13:53 -03:00
|
|
|
|
2014-10-13 05:41:48 -03:00
|
|
|
struct Guided_Limit {
|
|
|
|
uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
|
|
|
|
float alt_min_cm; // lower altitude limit in cm above home (0 = no limit)
|
|
|
|
float alt_max_cm; // upper altitude limit in cm above home (0 = no limit)
|
|
|
|
float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
|
|
|
|
uint32_t start_time;// system time in milliseconds that control was handed to the external computer
|
|
|
|
Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit
|
|
|
|
} guided_limit;
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// guided_init - initialise guided controller
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeGuided::init(bool ignore_checks)
|
2014-01-28 09:44:37 -04:00
|
|
|
{
|
2018-02-07 22:21:09 -04:00
|
|
|
if (copter.position_ok() || ignore_checks) {
|
2014-07-06 04:43:29 -03:00
|
|
|
// initialise yaw
|
2018-02-07 22:21:09 -04:00
|
|
|
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
|
2014-06-02 06:06:11 -03:00
|
|
|
// start in position control mode
|
2016-03-21 23:13:34 -03:00
|
|
|
pos_control_start();
|
2014-01-28 09:44:37 -04:00
|
|
|
return true;
|
|
|
|
}else{
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
|
|
|
|
// guided_takeoff_start - initialises waypoint controller to implement take-off
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
|
2014-08-04 11:11:23 -03:00
|
|
|
{
|
|
|
|
guided_mode = Guided_TakeOff;
|
2016-03-11 04:16:26 -04:00
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
// initialise wpnav destination
|
2018-02-07 22:21:09 -04:00
|
|
|
Location_Class target_loc = copter.current_loc;
|
2016-04-28 07:53:58 -03:00
|
|
|
target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
|
2016-03-11 04:16:26 -04:00
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!wp_nav->set_wp_destination(target_loc)) {
|
2016-04-22 08:46:36 -03:00
|
|
|
// failure to set destination can only be because of missing terrain data
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
2016-04-22 08:46:36 -03:00
|
|
|
// failure is propagated to GCS with NAK
|
2016-03-11 04:16:26 -04:00
|
|
|
return false;
|
|
|
|
}
|
2014-08-04 11:11:23 -03:00
|
|
|
|
|
|
|
// initialise yaw
|
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
|
|
|
2015-07-01 22:37:12 -03:00
|
|
|
// clear i term when we're taking off
|
|
|
|
set_throttle_takeoff();
|
2016-03-11 04:16:26 -04:00
|
|
|
|
2016-08-15 00:57:38 -03:00
|
|
|
// get initial alt for WP_NAVALT_MIN
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.auto_takeoff_set_start_alt();
|
2016-03-21 23:13:34 -03:00
|
|
|
|
2016-03-11 04:16:26 -04:00
|
|
|
return true;
|
2014-08-04 11:11:23 -03:00
|
|
|
}
|
|
|
|
|
2014-06-02 06:06:11 -03:00
|
|
|
// initialise guided mode's position controller
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::pos_control_start()
|
2014-06-02 06:06:11 -03:00
|
|
|
{
|
|
|
|
// set to position control mode
|
|
|
|
guided_mode = Guided_WP;
|
|
|
|
|
|
|
|
// initialise waypoint and spline controller
|
2017-01-09 03:31:26 -04:00
|
|
|
wp_nav->wp_and_spline_init();
|
2014-06-02 06:06:11 -03:00
|
|
|
|
2017-04-27 01:44:08 -03:00
|
|
|
// initialise wpnav to stopping point
|
2014-06-02 06:06:11 -03:00
|
|
|
Vector3f stopping_point;
|
2017-04-27 01:44:08 -03:00
|
|
|
wp_nav->get_wp_stopping_point(stopping_point);
|
2016-04-22 08:46:36 -03:00
|
|
|
|
|
|
|
// no need to check return status because terrain data is not used
|
2017-01-09 03:31:26 -04:00
|
|
|
wp_nav->set_wp_destination(stopping_point, false);
|
2014-06-02 06:06:11 -03:00
|
|
|
|
2014-07-06 04:43:29 -03:00
|
|
|
// initialise yaw
|
2018-02-07 22:21:09 -04:00
|
|
|
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// initialise guided mode's velocity controller
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::vel_control_start()
|
2014-06-02 06:06:11 -03:00
|
|
|
{
|
|
|
|
// set guided_mode to velocity controller
|
|
|
|
guided_mode = Guided_Velocity;
|
|
|
|
|
2017-04-27 01:21:31 -03:00
|
|
|
// initialise horizontal speed, acceleration and jerk
|
|
|
|
pos_control->set_speed_xy(wp_nav->get_speed_xy());
|
|
|
|
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
|
|
|
|
pos_control->set_jerk_xy_to_default();
|
|
|
|
|
|
|
|
// initialize vertical speeds and acceleration
|
2017-11-08 09:25:53 -04:00
|
|
|
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_accel_z(g.pilot_accel_z);
|
2014-06-02 06:06:11 -03:00
|
|
|
|
|
|
|
// initialise velocity controller
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->init_vel_controller_xyz();
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// initialise guided mode's posvel controller
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::posvel_control_start()
|
2014-11-25 16:15:45 -04:00
|
|
|
{
|
|
|
|
// set guided_mode to velocity controller
|
|
|
|
guided_mode = Guided_PosVel;
|
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->init_xy_controller();
|
2014-11-25 16:15:45 -04:00
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// set speed and acceleration from wpnav's speed and acceleration
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_speed_xy(wp_nav->get_speed_xy());
|
|
|
|
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
|
|
|
|
pos_control->set_jerk_xy_to_default();
|
2014-11-25 16:15:45 -04:00
|
|
|
|
|
|
|
const Vector3f& curr_pos = inertial_nav.get_position();
|
|
|
|
const Vector3f& curr_vel = inertial_nav.get_velocity();
|
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// set target position and velocity to current position and velocity
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_xy_target(curr_pos.x, curr_pos.y);
|
|
|
|
pos_control->set_desired_velocity_xy(curr_vel.x, curr_vel.y);
|
2014-11-25 16:15:45 -04:00
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// set vertical speed and acceleration
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
|
|
|
|
pos_control->set_accel_z(wp_nav->get_accel_z());
|
2014-11-25 16:15:45 -04:00
|
|
|
|
2015-01-06 23:33:54 -04:00
|
|
|
// pilot always controls yaw
|
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
2014-11-25 16:15:45 -04:00
|
|
|
}
|
|
|
|
|
2015-10-08 08:13:53 -03:00
|
|
|
// initialise guided mode's angle controller
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::angle_control_start()
|
2015-10-08 08:13:53 -03:00
|
|
|
{
|
|
|
|
// set guided_mode to velocity controller
|
|
|
|
guided_mode = Guided_Angle;
|
|
|
|
|
|
|
|
// set vertical speed and acceleration
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
|
|
|
|
pos_control->set_accel_z(wp_nav->get_accel_z());
|
2015-10-08 08:13:53 -03:00
|
|
|
|
2015-10-27 10:06:50 -03:00
|
|
|
// initialise position and desired velocity
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!pos_control->is_active_z()) {
|
|
|
|
pos_control->set_alt_target_to_current_alt();
|
|
|
|
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
2016-10-14 09:28:32 -03:00
|
|
|
}
|
2015-10-08 08:13:53 -03:00
|
|
|
|
|
|
|
// initialise targets
|
|
|
|
guided_angle_state.update_time_ms = millis();
|
2018-02-07 22:21:09 -04:00
|
|
|
guided_angle_state.roll_cd = copter.ahrs.roll_sensor;
|
2015-10-08 08:13:53 -03:00
|
|
|
guided_angle_state.pitch_cd = ahrs.pitch_sensor;
|
|
|
|
guided_angle_state.yaw_cd = ahrs.yaw_sensor;
|
|
|
|
guided_angle_state.climb_rate_cms = 0.0f;
|
2016-08-02 14:20:57 -03:00
|
|
|
guided_angle_state.yaw_rate_cds = 0.0f;
|
|
|
|
guided_angle_state.use_yaw_rate = false;
|
2015-10-08 08:13:53 -03:00
|
|
|
|
|
|
|
// pilot always controls yaw
|
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
|
|
}
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// guided_set_destination - sets guided mode's target destination
|
2016-04-28 04:35:29 -03:00
|
|
|
// Returns true if the fence is enabled and guided waypoint is within the fence
|
|
|
|
// else return false if the waypoint is outside the fence
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
2014-01-28 09:44:37 -04:00
|
|
|
{
|
2014-06-02 06:06:11 -03:00
|
|
|
// ensure we are in position control mode
|
|
|
|
if (guided_mode != Guided_WP) {
|
2016-03-21 23:13:34 -03:00
|
|
|
pos_control_start();
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
|
2016-04-28 04:35:29 -03:00
|
|
|
#if AC_FENCE == ENABLED
|
2016-05-19 05:35:44 -03:00
|
|
|
// reject destination if outside the fence
|
2016-07-02 05:15:05 -03:00
|
|
|
Location_Class dest_loc(destination);
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.fence.check_destination_within_fence(dest_loc)) {
|
|
|
|
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
2016-04-28 04:35:29 -03:00
|
|
|
// failure is propagated to GCS with NAK
|
|
|
|
return false;
|
|
|
|
}
|
2016-05-19 05:35:44 -03:00
|
|
|
#endif
|
2016-04-28 04:35:29 -03:00
|
|
|
|
2017-06-22 12:20:14 -03:00
|
|
|
// set yaw state
|
2016-03-21 23:13:34 -03:00
|
|
|
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
2017-06-22 12:20:14 -03:00
|
|
|
|
2016-05-19 05:35:44 -03:00
|
|
|
// no need to check return status because terrain data is not used
|
2017-01-09 03:31:26 -04:00
|
|
|
wp_nav->set_wp_destination(destination, false);
|
2016-01-10 22:41:28 -04:00
|
|
|
|
|
|
|
// log target
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
|
2016-04-28 04:35:29 -03:00
|
|
|
return true;
|
2014-01-28 09:44:37 -04:00
|
|
|
}
|
|
|
|
|
2016-03-11 04:16:26 -04:00
|
|
|
// sets guided mode's target from a Location object
|
|
|
|
// returns false if destination could not be set (probably caused by missing terrain data)
|
2016-04-28 04:35:29 -03:00
|
|
|
// or if the fence is enabled and guided waypoint is outside the fence
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
2016-03-11 04:16:26 -04:00
|
|
|
{
|
|
|
|
// ensure we are in position control mode
|
|
|
|
if (guided_mode != Guided_WP) {
|
2016-03-21 23:13:34 -03:00
|
|
|
pos_control_start();
|
2016-03-11 04:16:26 -04:00
|
|
|
}
|
|
|
|
|
2016-04-28 04:35:29 -03:00
|
|
|
#if AC_FENCE == ENABLED
|
2016-05-19 05:35:44 -03:00
|
|
|
// 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
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.fence.check_destination_within_fence(dest_loc)) {
|
|
|
|
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
2016-07-02 05:15:05 -03:00
|
|
|
// failure is propagated to GCS with NAK
|
|
|
|
return false;
|
2016-05-19 05:35:44 -03:00
|
|
|
}
|
2016-04-28 04:35:29 -03:00
|
|
|
#endif
|
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!wp_nav->set_wp_destination(dest_loc)) {
|
2016-05-19 05:35:44 -03:00
|
|
|
// failure to set destination can only be because of missing terrain data
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
2016-04-22 08:46:36 -03:00
|
|
|
// failure is propagated to GCS with NAK
|
2016-03-11 04:16:26 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2017-06-22 12:20:14 -03:00
|
|
|
// set yaw state
|
2016-03-21 23:13:34 -03:00
|
|
|
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
2017-06-22 12:20:14 -03:00
|
|
|
|
2016-03-11 04:16:26 -04:00
|
|
|
// log target
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
|
2016-03-11 04:16:26 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2014-06-02 06:06:11 -03:00
|
|
|
// guided_set_velocity - sets guided mode's target velocity
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
2014-06-02 06:06:11 -03:00
|
|
|
{
|
|
|
|
// check we are in velocity control mode
|
|
|
|
if (guided_mode != Guided_Velocity) {
|
2016-03-21 23:13:34 -03:00
|
|
|
vel_control_start();
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
|
2017-06-22 12:20:14 -03:00
|
|
|
// set yaw state
|
2016-03-21 23:13:34 -03:00
|
|
|
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
2017-06-22 12:20:14 -03:00
|
|
|
|
2016-07-20 08:45:11 -03:00
|
|
|
// record velocity target
|
|
|
|
guided_vel_target_cms = velocity;
|
2015-08-11 20:36:42 -03:00
|
|
|
vel_update_time_ms = millis();
|
|
|
|
|
2016-01-10 22:41:28 -04:00
|
|
|
// log target
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// set guided mode posvel target
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::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)
|
2017-07-11 02:06:18 -03:00
|
|
|
{
|
2014-11-25 16:15:45 -04:00
|
|
|
// check we are in velocity control mode
|
|
|
|
if (guided_mode != Guided_PosVel) {
|
2016-03-21 23:13:34 -03:00
|
|
|
posvel_control_start();
|
2014-11-25 16:15:45 -04:00
|
|
|
}
|
|
|
|
|
2017-11-27 08:21:44 -04:00
|
|
|
#if AC_FENCE == ENABLED
|
|
|
|
// reject destination if outside the fence
|
|
|
|
Location_Class dest_loc(destination);
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.fence.check_destination_within_fence(dest_loc)) {
|
|
|
|
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
2017-11-27 08:21:44 -04:00
|
|
|
// failure is propagated to GCS with NAK
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2017-06-22 12:20:14 -03:00
|
|
|
// set yaw state
|
2016-03-21 23:13:34 -03:00
|
|
|
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
2017-06-22 12:20:14 -03:00
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
posvel_update_time_ms = millis();
|
2016-07-21 04:14:21 -03:00
|
|
|
guided_pos_target_cm = destination;
|
|
|
|
guided_vel_target_cms = velocity;
|
2014-11-25 16:15:45 -04:00
|
|
|
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.pos_control->set_pos_target(guided_pos_target_cm);
|
2016-01-10 22:41:28 -04:00
|
|
|
|
|
|
|
// log target
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
|
2017-11-27 08:21:44 -04:00
|
|
|
return true;
|
2014-11-25 16:15:45 -04:00
|
|
|
}
|
|
|
|
|
2015-10-08 08:13:53 -03:00
|
|
|
// set guided mode angle target
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
|
2015-10-08 08:13:53 -03:00
|
|
|
{
|
|
|
|
// check we are in velocity control mode
|
|
|
|
if (guided_mode != Guided_Angle) {
|
2016-03-21 23:13:34 -03:00
|
|
|
angle_control_start();
|
2015-10-08 08:13:53 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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;
|
2016-04-27 06:35:18 -03:00
|
|
|
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
|
2016-08-02 14:20:57 -03:00
|
|
|
guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;
|
|
|
|
guided_angle_state.use_yaw_rate = use_yaw_rate;
|
2015-10-08 08:13:53 -03:00
|
|
|
|
|
|
|
guided_angle_state.climb_rate_cms = climb_rate_cms;
|
|
|
|
guided_angle_state.update_time_ms = millis();
|
2016-01-10 22:41:28 -04:00
|
|
|
|
2016-08-02 06:25:28 -03:00
|
|
|
// interpret positive climb rate as triggering take-off
|
2017-01-09 03:31:26 -04:00
|
|
|
if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.set_auto_armed(true);
|
2016-08-02 06:25:28 -03:00
|
|
|
}
|
|
|
|
|
2016-01-10 22:41:28 -04:00
|
|
|
// log target
|
2018-02-07 22:21:09 -04:00
|
|
|
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, guided_angle_state.climb_rate_cms));
|
2015-10-08 08:13:53 -03:00
|
|
|
}
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// guided_run - runs the guided controller
|
|
|
|
// should be called at 100hz or more
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::run()
|
2014-01-28 09:44:37 -04:00
|
|
|
{
|
2014-08-04 11:11:23 -03:00
|
|
|
// call the correct auto controller
|
|
|
|
switch (guided_mode) {
|
|
|
|
|
|
|
|
case Guided_TakeOff:
|
|
|
|
// run takeoff controller
|
2016-03-21 23:13:34 -03:00
|
|
|
takeoff_run();
|
2014-08-04 11:11:23 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case Guided_WP:
|
|
|
|
// run position controller
|
2016-03-21 23:13:34 -03:00
|
|
|
pos_control_run();
|
2014-08-04 11:11:23 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case Guided_Velocity:
|
|
|
|
// run velocity controller
|
2016-03-21 23:13:34 -03:00
|
|
|
vel_control_run();
|
2014-11-14 17:26:45 -04:00
|
|
|
break;
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
case Guided_PosVel:
|
2015-01-06 23:33:54 -04:00
|
|
|
// run position-velocity controller
|
2016-03-21 23:13:34 -03:00
|
|
|
posvel_control_run();
|
2014-11-25 16:15:45 -04:00
|
|
|
break;
|
2015-10-08 08:13:53 -03:00
|
|
|
|
|
|
|
case Guided_Angle:
|
|
|
|
// run angle controller
|
2016-03-21 23:13:34 -03:00
|
|
|
angle_control_run();
|
2015-10-08 08:13:53 -03:00
|
|
|
break;
|
2014-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
// guided_takeoff_run - takeoff in guided mode
|
|
|
|
// called by guided_run at 100hz or more
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::takeoff_run()
|
2014-08-04 11:11:23 -03:00
|
|
|
{
|
2017-02-07 05:50:08 -04:00
|
|
|
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
2017-02-07 05:50:08 -04:00
|
|
|
// initialise wpnav targets
|
|
|
|
wp_nav->shift_wp_origin_to_current_pos();
|
2017-12-28 22:36:18 -04:00
|
|
|
zero_throttle_and_relax_ac();
|
2017-02-07 05:50:08 -04:00
|
|
|
// clear i term when we're taking off
|
|
|
|
set_throttle_takeoff();
|
2015-10-20 03:40:24 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
// process pilot's yaw input
|
|
|
|
float target_yaw_rate = 0;
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.failsafe.radio) {
|
2014-08-04 11:11:23 -03: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());
|
2014-08-04 11:11:23 -03:00
|
|
|
}
|
|
|
|
|
2017-02-07 05:50:08 -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:10:45 -04:00
|
|
|
// set motors to full range
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
2016-01-13 03:10:45 -04:00
|
|
|
|
2014-08-04 11:11:23 -03:00
|
|
|
// run waypoint controller
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
|
2014-08-04 11:11:23 -03:00
|
|
|
|
|
|
|
// call z-axis position controller (wpnav should have already updated it's alt target)
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.pos_control->update_z_controller();
|
2014-08-04 11:11:23 -03:00
|
|
|
|
2016-06-04 23:37:55 -03:00
|
|
|
// call attitude controller
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.auto_takeoff_attitude_run(target_yaw_rate);
|
2014-08-04 11:11:23 -03:00
|
|
|
}
|
|
|
|
|
2014-06-02 06:06:11 -03:00
|
|
|
// guided_pos_control_run - runs the guided position controller
|
|
|
|
// called from guided_run
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::pos_control_run()
|
2014-06-02 06:06:11 -03:00
|
|
|
{
|
2015-08-11 06:34:26 -03:00
|
|
|
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
2016-11-17 01:47:41 -04:00
|
|
|
zero_throttle_and_relax_ac();
|
2015-08-11 06:34:26 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// process pilot's yaw input
|
|
|
|
float target_yaw_rate = 0;
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.failsafe.radio) {
|
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)) {
|
2014-01-28 09:44:37 -04:00
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-01-13 03:10:45 -04:00
|
|
|
// set motors to full range
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
2016-01-13 03:10:45 -04:00
|
|
|
|
2014-01-28 09:44:37 -04:00
|
|
|
// run waypoint controller
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
|
2014-01-28 09:44:37 -04:00
|
|
|
|
|
|
|
// call z-axis position controller (wpnav should have already updated it's alt target)
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->update_z_controller();
|
2014-01-28 09:44:37 -04:00
|
|
|
|
|
|
|
// call attitude controller
|
2018-02-07 22:21:09 -04:00
|
|
|
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
|
2014-01-28 09:44:37 -04:00
|
|
|
// roll & pitch from waypoint controller, yaw rate from pilot
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
2017-07-10 09:51:43 -03:00
|
|
|
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
|
|
|
|
// roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
|
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
|
2017-06-22 12:20:14 -03:00
|
|
|
} else {
|
2017-07-10 09:51:43 -03:00
|
|
|
// roll, pitch from waypoint controller, yaw heading from GCS or 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-06-02 06:06:11 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// guided_vel_control_run - runs the guided velocity controller
|
|
|
|
// called from guided_run
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::vel_control_run()
|
2014-06-02 06:06:11 -03:00
|
|
|
{
|
2015-08-11 06:34:26 -03:00
|
|
|
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
2015-08-11 06:34:26 -03:00
|
|
|
// initialise velocity controller
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->init_vel_controller_xyz();
|
2016-11-17 01:47:41 -04:00
|
|
|
zero_throttle_and_relax_ac();
|
2015-08-11 06:34:26 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-06-02 06:06:11 -03:00
|
|
|
// process pilot's yaw input
|
|
|
|
float target_yaw_rate = 0;
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.failsafe.radio) {
|
2014-06-02 06:06:11 -03: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)) {
|
2014-06-02 06:06:11 -03:00
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-01-13 03:10:45 -04:00
|
|
|
// set motors to full range
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
2016-01-13 03:10:45 -04:00
|
|
|
|
2017-07-11 02:39:53 -03:00
|
|
|
// set velocity to zero and stop rotating if no updates received for 3 seconds
|
2015-08-11 20:36:42 -03:00
|
|
|
uint32_t tnow = millis();
|
2017-07-11 02:39:53 -03:00
|
|
|
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
|
|
|
if (!pos_control->get_desired_velocity().is_zero()) {
|
2016-03-21 23:13:34 -03:00
|
|
|
set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
|
2017-07-11 02:39:53 -03:00
|
|
|
}
|
|
|
|
if (auto_yaw_mode == AUTO_YAW_RATE) {
|
|
|
|
set_auto_yaw_rate(0.0f);
|
|
|
|
}
|
2016-07-20 08:45:11 -03:00
|
|
|
} else {
|
2016-03-21 23:13:34 -03:00
|
|
|
set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
|
2015-08-11 20:36:42 -03:00
|
|
|
}
|
|
|
|
|
2015-11-17 02:51:25 -04:00
|
|
|
// call velocity controller which includes z axis controller
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
|
2014-06-02 06:06:11 -03:00
|
|
|
|
|
|
|
// call attitude controller
|
2018-02-07 22:21:09 -04:00
|
|
|
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
|
2014-06-02 06:06:11 -03:00
|
|
|
// roll & pitch from waypoint controller, yaw rate from pilot
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
2017-07-10 09:51:43 -03:00
|
|
|
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
|
|
|
|
// roll & pitch from velocity controller, yaw rate from mavlink command or mission item
|
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
|
2017-06-22 12:20:14 -03:00
|
|
|
} else {
|
2017-07-10 09:51:43 -03:00
|
|
|
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
|
|
|
|
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
2014-01-28 09:44:37 -04:00
|
|
|
}
|
|
|
|
}
|
2014-10-13 05:41:48 -03:00
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// guided_posvel_control_run - runs the guided spline controller
|
|
|
|
// called from guided_run
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::posvel_control_run()
|
2014-11-25 16:15:45 -04:00
|
|
|
{
|
2015-08-11 06:34:26 -03:00
|
|
|
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
2015-08-11 06:34:26 -03:00
|
|
|
// set target position and velocity to current position and velocity
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_pos_target(inertial_nav.get_position());
|
|
|
|
pos_control->set_desired_velocity(Vector3f(0,0,0));
|
2016-11-17 01:47:41 -04:00
|
|
|
zero_throttle_and_relax_ac();
|
2015-08-11 06:34:26 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// process pilot's yaw input
|
|
|
|
float target_yaw_rate = 0;
|
|
|
|
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.failsafe.radio) {
|
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)) {
|
2014-11-25 16:15:45 -04:00
|
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-01-13 03:10:45 -04:00
|
|
|
// set motors to full range
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
2016-01-13 03:10:45 -04:00
|
|
|
|
2017-07-11 02:39:53 -03:00
|
|
|
// 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();
|
2017-07-11 02:39:53 -03:00
|
|
|
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
2016-07-21 04:14:21 -03:00
|
|
|
guided_vel_target_cms.zero();
|
2017-07-11 02:39:53 -03:00
|
|
|
if (auto_yaw_mode == AUTO_YAW_RATE) {
|
|
|
|
set_auto_yaw_rate(0.0f);
|
|
|
|
}
|
2014-11-25 16:15:45 -04:00
|
|
|
}
|
|
|
|
|
2015-04-13 22:51:16 -03:00
|
|
|
// calculate dt
|
2017-01-09 03:31:26 -04:00
|
|
|
float dt = pos_control->time_since_last_xy_update();
|
2014-11-25 16:15:45 -04:00
|
|
|
|
2015-04-13 22:51:16 -03:00
|
|
|
// update at poscontrol update rate
|
2017-01-09 03:31:26 -04:00
|
|
|
if (dt >= pos_control->get_dt_xy()) {
|
2015-04-13 22:51:16 -03:00
|
|
|
// sanity check dt
|
|
|
|
if (dt >= 0.2f) {
|
|
|
|
dt = 0.0f;
|
|
|
|
}
|
2015-01-06 23:33:54 -04:00
|
|
|
|
2015-04-13 22:51:16 -03:00
|
|
|
// advance position target using velocity target
|
2016-07-21 04:14:21 -03:00
|
|
|
guided_pos_target_cm += guided_vel_target_cms * dt;
|
2015-04-13 22:51:16 -03:00
|
|
|
|
|
|
|
// send position and velocity targets to position controller
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_pos_target(guided_pos_target_cm);
|
|
|
|
pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
|
2015-04-13 22:51:16 -03:00
|
|
|
|
|
|
|
// run position controller
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
|
2015-04-13 22:51:16 -03:00
|
|
|
}
|
2014-11-25 16:15:45 -04:00
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->update_z_controller();
|
2015-04-22 16:04:27 -03:00
|
|
|
|
2014-11-25 16:15:45 -04:00
|
|
|
// call attitude controller
|
2018-02-07 22:21:09 -04:00
|
|
|
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
|
2014-11-25 16:15:45 -04:00
|
|
|
// roll & pitch from waypoint controller, yaw rate from pilot
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
2017-07-10 09:51:43 -03:00
|
|
|
} 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_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
|
2017-06-22 12:20:14 -03:00
|
|
|
} else {
|
2017-07-10 09:51:43 -03:00
|
|
|
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
|
|
|
|
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
2014-11-25 16:15:45 -04:00
|
|
|
}
|
|
|
|
}
|
2014-11-14 17:26:45 -04:00
|
|
|
|
2015-10-08 08:13:53 -03:00
|
|
|
// guided_angle_control_run - runs the guided angle controller
|
|
|
|
// called from guided_run
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::angle_control_run()
|
2015-10-08 08:13:53 -03:00
|
|
|
{
|
|
|
|
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
|
2016-11-17 01:47:41 -04:00
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->set_yaw_target_to_current_heading();
|
2015-10-08 08:13:53 -03:00
|
|
|
#endif
|
2016-11-17 01:47:41 -04:00
|
|
|
zero_throttle_and_relax_ac();
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->relax_alt_hold_controllers(0.0f);
|
2015-10-08 08:13:53 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// constrain desired lean angles
|
|
|
|
float roll_in = guided_angle_state.roll_cd;
|
|
|
|
float pitch_in = guided_angle_state.pitch_cd;
|
2016-04-16 06:58:46 -03:00
|
|
|
float total_in = norm(roll_in, pitch_in);
|
2018-02-07 22:21:09 -04:00
|
|
|
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);
|
2015-10-08 08:13:53 -03:00
|
|
|
if (total_in > angle_max) {
|
|
|
|
float ratio = angle_max / total_in;
|
|
|
|
roll_in *= ratio;
|
|
|
|
pitch_in *= ratio;
|
|
|
|
}
|
|
|
|
|
|
|
|
// wrap yaw request
|
2016-04-27 06:35:18 -03:00
|
|
|
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
|
2016-08-02 14:20:57 -03:00
|
|
|
float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
|
2015-10-08 08:13:53 -03:00
|
|
|
|
|
|
|
// constrain climb rate
|
2017-01-09 03:31:26 -04:00
|
|
|
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_speed_down()), wp_nav->get_speed_up());
|
2015-10-08 08:13:53 -03:00
|
|
|
|
2017-01-05 02:24:02 -04:00
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
|
|
|
|
|
2015-10-08 08:13:53 -03:00
|
|
|
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
|
|
|
|
uint32_t tnow = millis();
|
2015-10-10 09:41:42 -03:00
|
|
|
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
|
2015-10-08 08:13:53 -03:00
|
|
|
roll_in = 0.0f;
|
|
|
|
pitch_in = 0.0f;
|
|
|
|
climb_rate_cms = 0.0f;
|
2016-08-02 14:20:57 -03:00
|
|
|
yaw_rate_in = 0.0f;
|
2015-10-08 08:13:53 -03:00
|
|
|
}
|
|
|
|
|
2016-01-13 03:10:45 -04:00
|
|
|
// set motors to full range
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
2016-01-13 03:10:45 -04:00
|
|
|
|
2015-10-08 08:13:53 -03:00
|
|
|
// call attitude controller
|
2016-08-02 14:20:57 -03:00
|
|
|
if (guided_angle_state.use_yaw_rate) {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
|
2016-08-02 14:20:57 -03:00
|
|
|
} else {
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
|
2016-08-02 14:20:57 -03:00
|
|
|
}
|
2015-10-08 08:13:53 -03:00
|
|
|
|
|
|
|
// call position controller
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
|
|
|
|
pos_control->update_z_controller();
|
2015-10-08 08:13:53 -03:00
|
|
|
}
|
|
|
|
|
2016-07-20 08:45:11 -03:00
|
|
|
// helper function to update position controller's desired velocity while respecting acceleration limits
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
|
2016-07-20 08:45:11 -03:00
|
|
|
{
|
|
|
|
// get current desired velocity
|
2017-01-09 03:31:26 -04:00
|
|
|
Vector3f curr_vel_des = pos_control->get_desired_velocity();
|
2016-07-20 08:45:11 -03:00
|
|
|
|
|
|
|
// get change in desired velocity
|
|
|
|
Vector3f vel_delta = vel_des - curr_vel_des;
|
|
|
|
|
|
|
|
// limit xy change
|
|
|
|
float vel_delta_xy = safe_sqrt(sq(vel_delta.x)+sq(vel_delta.y));
|
2017-01-09 03:31:26 -04:00
|
|
|
float vel_delta_xy_max = G_Dt * pos_control->get_accel_xy();
|
2016-07-20 08:45:11 -03:00
|
|
|
float ratio_xy = 1.0f;
|
|
|
|
if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) {
|
|
|
|
ratio_xy = vel_delta_xy_max / vel_delta_xy;
|
|
|
|
}
|
2016-07-24 23:47:44 -03:00
|
|
|
curr_vel_des.x += (vel_delta.x * ratio_xy);
|
|
|
|
curr_vel_des.y += (vel_delta.y * ratio_xy);
|
2016-07-20 08:45:11 -03:00
|
|
|
|
|
|
|
// limit z change
|
2017-01-09 03:31:26 -04:00
|
|
|
float vel_delta_z_max = G_Dt * pos_control->get_accel_z();
|
2016-07-24 23:40:08 -03:00
|
|
|
curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max);
|
2016-07-20 08:45:11 -03:00
|
|
|
|
2017-01-02 20:36:26 -04:00
|
|
|
#if AC_AVOID_ENABLED
|
2016-07-24 23:47:44 -03:00
|
|
|
// limit the velocity to prevent fence violations
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
|
2017-11-22 10:49:55 -04:00
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
|
2017-01-02 20:36:26 -04:00
|
|
|
#endif
|
2016-07-24 23:47:44 -03:00
|
|
|
|
2016-07-20 08:45:11 -03:00
|
|
|
// update position controller with new target
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->set_desired_velocity(curr_vel_des);
|
2016-07-20 08:45:11 -03:00
|
|
|
}
|
|
|
|
|
2017-07-10 09:51:43 -03:00
|
|
|
// helper function to set yaw state and targets
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
|
2017-06-22 12:20:14 -03:00
|
|
|
{
|
2017-07-10 09:51:43 -03:00
|
|
|
if (use_yaw) {
|
2017-07-11 02:06:18 -03:00
|
|
|
set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle);
|
2017-07-10 09:51:43 -03:00
|
|
|
} else if (use_yaw_rate) {
|
|
|
|
set_auto_yaw_rate(yaw_rate_cds);
|
|
|
|
}
|
2017-06-22 12:20:14 -03:00
|
|
|
}
|
|
|
|
|
2014-10-13 05:41:48 -03:00
|
|
|
// Guided Limit code
|
|
|
|
|
|
|
|
// guided_limit_clear - clear/turn off guided limits
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::limit_clear()
|
2014-10-13 05:41:48 -03:00
|
|
|
{
|
|
|
|
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
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
|
2014-10-13 05:41:48 -03:00
|
|
|
{
|
|
|
|
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
|
2017-12-10 23:51:13 -04:00
|
|
|
void Copter::ModeGuided::limit_init_time_and_pos()
|
2014-10-13 05:41:48 -03:00
|
|
|
{
|
|
|
|
// initialise start time
|
2015-11-19 23:04:45 -04:00
|
|
|
guided_limit.start_time = AP_HAL::millis();
|
2014-10-13 05:41:48 -03:00
|
|
|
|
|
|
|
// 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
|
2017-12-10 23:51:13 -04:00
|
|
|
bool Copter::ModeGuided::limit_check()
|
2014-10-13 05:41:48 -03:00
|
|
|
{
|
|
|
|
// 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)) {
|
2014-10-13 05:41:48 -03:00
|
|
|
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)) {
|
2014-10-13 05:41:48 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if we have gone beyond horizontal limit
|
|
|
|
if (guided_limit.horiz_max_cm > 0.0f) {
|
2017-12-03 17:07:39 -04:00
|
|
|
float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
|
2014-10-13 05:41:48 -03:00
|
|
|
if (horiz_move > guided_limit.horiz_max_cm) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// if we got this far we must be within limits
|
|
|
|
return false;
|
|
|
|
}
|
2017-12-05 21:28:32 -04:00
|
|
|
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
uint32_t Copter::ModeGuided::wp_distance() const
|
2017-12-05 21:28:32 -04:00
|
|
|
{
|
|
|
|
switch(mode()) {
|
|
|
|
case Guided_WP:
|
|
|
|
return wp_nav->get_wp_distance_to_destination();
|
|
|
|
break;
|
|
|
|
case Guided_PosVel:
|
|
|
|
return pos_control->get_distance_to_target();
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-12-10 23:51:13 -04:00
|
|
|
int32_t Copter::ModeGuided::wp_bearing() const
|
2017-12-05 21:28:32 -04:00
|
|
|
{
|
|
|
|
switch(mode()) {
|
|
|
|
case Guided_WP:
|
|
|
|
return wp_nav->get_wp_bearing_to_destination();
|
|
|
|
break;
|
|
|
|
case Guided_PosVel:
|
|
|
|
return pos_control->get_bearing_to_target();
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|