/// 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; // 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); }