/// 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(); }
/// 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_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; // initialise ekf xy reset handler init_ekf_xy_reset(); }