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