/// init_xy_controller - initialise the xy controller
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
/// this does not update the xy target
void AC_PosControl::init_xy_controller()
{
// reset xy controller to first step
_xy_step = 0;
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
// initialise I terms from lean angles
if (!_flags.keep_xy_I_terms) {
// reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pid_rate_lat.set_integrator(_accel_target.x);
_pid_rate_lon.set_integrator(_accel_target.y);
} else {
// reset keep_xy_I_term flag in case it has been set
_flags.keep_xy_I_terms = false;
}
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
// update update time
_last_update_xy_ms = hal.scheduler->millis();
}
/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
void AC_PosControl::init_vel_controller_xyz()
{
// force the xy velocity controller to run immediately
_vel_xyz_step = 3;
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
// reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pid_rate_lat.set_integrator(_accel_target.x);
_pid_rate_lon.set_integrator(_accel_target.y);
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
// set target position in xy axis
const Vector3f& curr_pos = _inav.get_position();
set_xy_target(curr_pos.x, curr_pos.y);
// move current vehicle velocity into feed forward velocity
const Vector3f& curr_vel = _inav.get_velocity();
set_desired_velocity_xy(curr_vel.x, curr_vel.y);
// record update time
_last_update_vel_xyz_ms = hal.scheduler->millis();
}
/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
void AC_PosControl::init_vel_controller_xyz()
{
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
_pid_vel_xy.reset_filter();
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pid_vel_xy.set_integrator(_accel_target);
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_accel_to_lean_xy = true;
// set target position
const Vector3f& curr_pos = _inav.get_position();
set_xy_target(curr_pos.x, curr_pos.y);
set_alt_target(curr_pos.z);
// move current vehicle velocity into feed forward velocity
const Vector3f& curr_vel = _inav.get_velocity();
set_desired_velocity(curr_vel);
// set vehicle acceleration to zero
set_desired_accel_xy(0.0f, 0.0f);
// initialise ekf reset handlers
init_ekf_xy_reset();
init_ekf_z_reset();
}
void AC_PosControl::write_log()
{
const Vector3f &pos_target = get_pos_target();
const Vector3f &vel_target = get_vel_target();
const Vector3f &accel_target = get_accel_target();
const Vector3f &position = _inav.get_position();
const Vector3f &velocity = _inav.get_velocity();
float accel_x, accel_y;
lean_angles_to_accel(accel_x, accel_y);
AP::logger().Write("PSC",
"TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY",
"smmmmnnnnoooo",
"F000000000000",
"Qffffffffffff",
AP_HAL::micros64(),
double(pos_target.x * 0.01f),
double(pos_target.y * 0.01f),
double(position.x * 0.01f),
double(position.y * 0.01f),
double(vel_target.x * 0.01f),
double(vel_target.y * 0.01f),
double(velocity.x * 0.01f),
double(velocity.y * 0.01f),
double(accel_target.x * 0.01f),
double(accel_target.y * 0.01f),
double(accel_x * 0.01f),
double(accel_y * 0.01f));
}
/// init_xy_controller - initialise the xy controller
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
/// this does not update the xy target
void AC_PosControl::init_xy_controller(bool reset_I)
{
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
// initialise I terms from lean angles
if (reset_I) {
// reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pi_vel_xy.set_integrator(_accel_target);
}
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
_flags.reset_accel_to_lean_xy = true;
}
/// init_xy_controller - initialise the xy controller
/// this should be called after setting the position target and the desired velocity and acceleration
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
/// this does not update the xy target
void AC_PosControl::init_xy_controller()
{
// set roll, pitch lean angle targets to current attitude
// todo: this should probably be based on the desired attitude not the current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
// initialise I terms from lean angles
_pid_vel_xy.reset_filter();
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pid_vel_xy.set_integrator(_accel_target-_accel_desired);
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_accel_to_lean_xy = true;
// initialise ekf xy reset handler
init_ekf_xy_reset();
}
/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
void AC_PosControl::init_vel_controller_xyz()
{
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
// reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pi_vel_xy.set_integrator(_accel_target);
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
_flags.reset_accel_to_lean_xy = true;
// set target position in xy axis
const Vector3f& curr_pos = _inav.get_position();
set_xy_target(curr_pos.x, curr_pos.y);
// move current vehicle velocity into feed forward velocity
const Vector3f& curr_vel = _inav.get_velocity();
set_desired_velocity_xy(curr_vel.x, curr_vel.y);
}
