Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add ekf velocity scaler to posvel controller 

Also set auto-yaw-mode to yaw-hold only when posvel controller is
started instead of on every iteration.
added GUIDED_POSVEL_TIMEOUT_MS definition.
removed deprecated trigger_xy call.
Also use wp_nav.get_speed_xy in place of get_speed_param
This commit is contained in:
Randy Mackay 2015-01-07 12:33:54 +09:00
parent 474456e349
commit be5bf91e92
1 changed files with 16 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduCopter/control_guided.pde
View File

@ -8,6 +8,8 @@
# 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
static Vector3f posvel_pos_target_cm;
static Vector3f posvel_vel_target_cms;
static uint32_t posvel_update_time_ms;
@ -96,20 +98,23 @@ static void guided_posvel_control_start()
pos_control.init_xy_controller();
pos_control.set_speed_xy(wp_nav.get_speed_param());
// set speed and acceleration from wpnav's speed and acceleration
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
const Vector3f& curr_pos = inertial_nav.get_position();
const Vector3f& curr_vel = inertial_nav.get_velocity();
// set target position and velocity to current position and velocity
pos_control.set_xy_target(curr_pos.x, curr_pos.y);
pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
// set vertical speed and acceleration
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());
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise guided mode's spline controller
@ -218,6 +223,7 @@ static void guided_run()
break;
case Guided_PosVel:
// run position-velocity controller
guided_posvel_control_run();
break;
}
@ -321,9 +327,6 @@ static void guided_posvel_control_run()
// process pilot's yaw input
float target_yaw_rate = 0;
// pilot control over yaw only for now
set_auto_yaw_mode(AUTO_YAW_HOLD);
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
@ -332,20 +335,22 @@ static void guided_posvel_control_run()
}
}
//after 3 seconds, zero velocity
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if(tnow-posvel_update_time_ms > 3000 && !posvel_vel_target_cms.is_zero()) {
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
posvel_vel_target_cms.zero();
}
// advance position target using velocity target
posvel_pos_target_cm += posvel_vel_target_cms * G_Dt;
// send position and velocity targets to position controller
pos_control.set_pos_target(posvel_pos_target_cm);
pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
pos_control.update_xy_controller(true);
// run position controller
pos_control.update_xy_controller(true, ekfNavVelGainScaler);
pos_control.update_z_controller();
pos_control.trigger_xy();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {