Copter: guided mode velocity controller

This commit is contained in:
Randy Mackay 2014-06-02 18:06:11 +09:00
parent b027c71491
commit f5640dadbf
4 changed files with 121 additions and 18 deletions

View File

@ -512,6 +512,11 @@ static uint8_t land_state; // records state of land (flying to loca
////////////////////////////////////////////////////////////////////////////////
static AutoMode auto_mode; // controls which auto controller is run
////////////////////////////////////////////////////////////////////////////////
// Guided
////////////////////////////////////////////////////////////////////////////////
static GuidedMode guided_mode; // controls which controller is run (pos or vel)
////////////////////////////////////////////////////////////////////////////////
// RTL
////////////////////////////////////////////////////////////////////////////////

View File

@ -856,7 +856,7 @@ static bool verify_yaw()
// do_guided - start guided mode
static bool do_guided(const AP_Mission::Mission_Command& cmd)
{
Vector3f pos; // target location
Vector3f pos_or_vel; // target location or velocity
// only process guided waypoint if we are in guided mode
if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
@ -868,8 +868,17 @@ static bool do_guided(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_WAYPOINT:
// set wp_nav's destination
pos = pv_location_to_vector(cmd.content.location);
guided_set_destination(pos);
pos_or_vel = pv_location_to_vector(cmd.content.location);
guided_set_destination(pos_or_vel);
return true;
break;
case MAV_CMD_NAV_VELOCITY:
// set target velocity
pos_or_vel.x = cmd.content.nav_velocity.x * 100.0f;
pos_or_vel.y = cmd.content.nav_velocity.y * 100.0f;
pos_or_vel.z = cmd.content.nav_velocity.z * 100.0f;
guided_set_velocity(pos_or_vel);
return true;
break;

View File

@ -14,29 +14,60 @@ static bool guided_pilot_yaw_override_yaw = false;
static bool guided_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude
// To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude?
Vector3f stopping_point;
stopping_point.z = inertial_nav.get_altitude();
wp_nav.get_wp_stopping_point_xy(stopping_point);
wp_nav.set_wp_destination(stopping_point);
guided_pilot_yaw_override_yaw = false;
// initialise yaw to hold at current heading (reset to point at waypoint in guided_set_destination)
set_auto_yaw_mode(AUTO_YAW_HOLD);
// start in position control mode
guided_pos_control_start();
return true;
}else{
return false;
}
}
// initialise guided mode's position controller
void guided_pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude
// To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude?
Vector3f stopping_point;
stopping_point.z = inertial_nav.get_altitude();
wp_nav.get_wp_stopping_point_xy(stopping_point);
wp_nav.set_wp_destination(stopping_point);
guided_pilot_yaw_override_yaw = false;
// initialise yaw to hold at current heading (reset to point at waypoint in guided_set_destination)
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise guided mode's velocity controller
void guided_vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Velocity;
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise velocity controller
pos_control.init_vel_controller_xyz();
// To-Do: set yaw override and auto_yaw_mode?
}
// guided_set_destination - sets guided mode's target destination
static void guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
wp_nav.set_wp_destination(destination);
if (!guided_pilot_yaw_override_yaw) {
// get default yaw mode
@ -44,6 +75,18 @@ static void guided_set_destination(const Vector3f& destination)
}
}
// guided_set_velocity - sets guided mode's target velocity
static void guided_set_velocity(const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
guided_vel_control_start();
}
// set position controller velocity target
pos_control.set_vel_target(velocity);
}
// guided_run - runs the guided controller
// should be called at 100hz or more
static void guided_run()
@ -58,6 +101,19 @@ static void guided_run()
return;
}
// run position or velocity controller
if (guided_mode == Guided_WP) {
guided_pos_control_run();
} else {
// must be velocity controller
guided_vel_control_run();
}
}
// guided_pos_control_run - runs the guided position controller
// called from guided_run
static void guided_pos_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
@ -80,6 +136,33 @@ static void guided_run()
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
}
}
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
static void guided_vel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
if (target_yaw_rate != 0) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// call velocity controller which includes z axis controller
pos_control.update_vel_controller_xyz();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.angle_ef_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
}
}

View File

@ -185,6 +185,12 @@ enum AutoMode {
Auto_NavGuided
};
// Guided modes
enum GuidedMode {
Guided_WP,
Guided_Velocity
};
// RTL states
enum RTLState {
InitialClimb,