看了下loiter 控制器,且ArduCopter 的loiter mode 不是很稳定,优化一波。
// loiter_init - initialise loiter controller
bool Copter::ModeLoiter::init(bool ignore_checks)
{
if (copter.position_ok() || ignore_checks) {
// set target to current position
loiter_nav->init_target();
// initialise position and desired velocity
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());
}
return true;
}else{
return false;
}
}
// 从当前位置和速度,初始化位置和前馈速度。
void AC_Loiter::init_target()
{
const Vector3f& curr_pos = _inav.get_position();
const Vector3f& curr_vel = _inav.get_velocity();
sanity_check_params();
// 初始化位置控制器的速度和加速度
_pos_control.set_speed_xy(LOITER_VEL_CORRECTION_MAX);
_pos_control.set_accel_xy(_accel_cmss);
_pos_control.set_leash_length_xy(LOITER_POS_CORRECTION_MAX);
// 基于当前速度和人工阻力初始化期望加速度
float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f));
_predicted_accel.x = pilot_acceleration_max*curr_vel.x/_speed_cms;
_predicted_accel.y = pilot_acceleration_max*curr_vel.y/_speed_cms;
_desired_accel = _predicted_accel;
// this should be the current roll and pitch angle.
_predicted_euler_angle.x = radians(_attitude_control.get_att_target_euler_cd().x*0.01f);
_predicted_euler_angle.y = radians(_attitude_control.get_att_target_euler_cd().y*0.01f);
// set target position
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
// set vehicle velocity and acceleration to current state
_pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
_pos_control.set_desired_accel_xy(_desired_accel.x, _desired_accel.y);
// initialise position controller
_pos_control.init_xy_controller();
}