diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index bb5687fe69..732c20c33c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -334,12 +334,12 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, /// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The time constant also defines the time taken to achieve the maximum acceleration. /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. -void AC_PosControl::input_pos_vel_accel_xyz(const Vector3f& pos) +void AC_PosControl::input_pos_vel_accel_xyz(const Vector3p& pos) { // check for ekf xy position reset handle_ekf_xy_reset(); handle_ekf_z_reset(); - Vector3f dest_vector = pos - _pos_target; + Vector3f dest_vector = (pos - _pos_target).tofloat(); // calculated increased maximum acceleration if over speed float accel_z_cmss = _accel_max_z_cmss; @@ -359,7 +359,7 @@ void AC_PosControl::input_pos_vel_accel_xyz(const Vector3f& pos) float vel_max_z_cms = 0.0f; if (is_positive(dest_vector.length_squared()) ) { dest_vector.normalize(); - float dest_vector_xy_length = Vector2f{dest_vector.x, dest_vector.y}.length(); + float dest_vector_xy_length = dest_vector.xy().length(); float vel_max_cms = kinematic_limit(dest_vector, _vel_max_xy_cms, _vel_max_up_cms, _vel_max_down_cms); vel_max_xy_cms = vel_max_cms * dest_vector_xy_length; @@ -367,19 +367,18 @@ void AC_PosControl::input_pos_vel_accel_xyz(const Vector3f& pos) } - Vector3f vel; - Vector3f accel; - shape_pos_vel_accel_xy(pos.xy(), vel.xy(), accel.xy(), _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), - vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt); + Vector2f vel; + Vector2f accel; + shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), + vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt); float posz = pos.z; - shape_pos_vel_accel(posz, vel.z, accel.z, + shape_pos_vel_accel(posz, 0, 0, _pos_target.z, _vel_desired.z, _accel_desired.z, vel_max_z_cms, _vel_max_down_cms, _vel_max_up_cms, -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss, _tc_z_s, _dt); } - /// /// Lateral position controller /// @@ -506,7 +505,7 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel) /// The pos and vel are projected forwards in time based on a time step of dt and acceleration accel. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The function alters the pos and vel to be the kinematic path based on accel -void AC_PosControl::input_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel) +void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel) { // check for ekf xy position reset handle_ekf_xy_reset(); @@ -514,7 +513,7 @@ void AC_PosControl::input_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const V update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), - _vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt); + _vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt); update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f()); } @@ -814,7 +813,9 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss, _tc_z_s, _dt); - update_pos_vel_accel(pos, vel, accel, _dt, 0); + postype_t posp = pos; + update_pos_vel_accel(posp, vel, accel, _dt, 0); + pos = posp; } /// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm @@ -850,7 +851,9 @@ void AC_PosControl::update_z_controller() const float curr_alt = _inav.get_position().z; // calculate the target velocity correction - _vel_target.z = _p_pos_z.update_all(_pos_target.z, curr_alt, _limit.pos_down, _limit.pos_up); + float pos_target_zf = _pos_target.z; + _vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up); + _pos_target.z = pos_target_zf; // add feed forward component _vel_target.z += _vel_desired.z; @@ -908,7 +911,7 @@ void AC_PosControl::update_z_controller() /// /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration -void AC_PosControl::get_stopping_point_z_cm(float &stopping_point) const +void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const { const float curr_pos_z = _inav.get_position().z; float curr_vel_z = _inav.get_velocity().z; @@ -937,7 +940,7 @@ float AC_PosControl::get_lean_angle_max_cd() const } /// set position, velocity and acceleration targets -void AC_PosControl::set_pos_vel_accel(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel) +void AC_PosControl::set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel) { _pos_target = pos; _vel_desired = vel; @@ -945,7 +948,7 @@ void AC_PosControl::set_pos_vel_accel(const Vector3f& pos, const Vector3f& vel, } /// set position, velocity and acceleration targets -void AC_PosControl::set_pos_vel_accel_xy(const Vector2f& pos, const Vector2f& vel, const Vector2f& accel) +void AC_PosControl::set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel) { _pos_target.x = pos.x; _pos_target.y = pos.y; @@ -983,10 +986,10 @@ Vector3f AC_PosControl::get_thrust_vector() const /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// function does not change the z axis -void AC_PosControl::get_stopping_point_xy_cm(Vector2f &stopping_point) const +void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const { const Vector3f curr_pos = _inav.get_position(); - stopping_point = curr_pos.xy(); + stopping_point = curr_pos.xy().topostype(); float kP = _p_pos_xy.kP(); Vector3f curr_vel = _inav.get_velocity(); @@ -1018,7 +1021,7 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2f &stopping_point) const /// get_bearing_to_target_cd - get bearing to target position in centi-degrees int32_t AC_PosControl::get_bearing_to_target_cd() const { - return get_bearing_cd(_inav.get_position(), _pos_target); + return get_bearing_cd(_inav.get_position(), _pos_target.tofloat()); } @@ -1048,7 +1051,7 @@ void AC_PosControl::standby_xyz_reset() _pid_accel_z.set_integrator(0.0f); // Set the target position to the current pos. - _pos_target = _inav.get_position(); + _pos_target = _inav.get_position().topostype(); // Set _pid_vel_xy integrator and derivative to zero. _pid_vel_xy.reset_filter(); @@ -1063,7 +1066,7 @@ void AC_PosControl::write_log() if (is_active_xy()) { float accel_x, accel_y; lean_angles_to_accel_xy(accel_x, accel_y); - AP::logger().Write_PSC(get_pos_target_cm(), _inav.get_position(), get_vel_target_cms(), _inav.get_velocity(), get_accel_target_cmss(), accel_x, accel_y); + AP::logger().Write_PSC(get_pos_target_cm().tofloat(), _inav.get_position(), get_vel_target_cms(), _inav.get_velocity(), get_accel_target_cmss(), accel_x, accel_y); } if (is_active_z()) { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 668c585574..17afd3d0f5 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -54,7 +54,7 @@ public: /// input_pos_vel_accel_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. - void input_pos_vel_accel_xyz(const Vector3f& pos); + void input_pos_vel_accel_xyz(const Vector3p& pos); /// /// Lateral position controller @@ -96,7 +96,7 @@ public: /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. /// The function alters the pos and vel to be the kinematic path based on accel - void input_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel); + void input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel); // is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times bool is_active_xy() const; @@ -190,8 +190,8 @@ public: /// /// set commanded position (cm), velocity (cm/s) and acceleration (cm/s/s) inputs when the path is created externally. - void set_pos_vel_accel(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel); - void set_pos_vel_accel_xy(const Vector2f& pos, const Vector2f& vel, const Vector2f& accel); + void set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel); + void set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel); /// Position @@ -200,7 +200,7 @@ public: void set_pos_target_xy_cm(float pos_x, float pos_y) { _pos_target.x = pos_x; _pos_target.y = pos_y; } /// get_pos_target_cm - returns the position target in NEU cm from home - const Vector3f& get_pos_target_cm() const { return _pos_target; } + const Vector3p& get_pos_target_cm() const { return _pos_target; } /// set_pos_target_z_cm - set altitude target in cm above home void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; } @@ -209,13 +209,13 @@ public: float get_pos_target_z_cm() const { return _pos_target.z; } /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration - void get_stopping_point_xy_cm(Vector2f &stopping_point) const; + void get_stopping_point_xy_cm(Vector2p &stopping_point) const; /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration - void get_stopping_point_z_cm(float &stopping_point) const; + void get_stopping_point_z_cm(postype_t &stopping_point) const; /// get_pos_error_cm - get position error vector between the current and target position - const Vector3f get_pos_error_cm() const { return _pos_target - _inav.get_position(); } + const Vector3f get_pos_error_cm() const { return (_pos_target - _inav.get_position().topostype()).tofloat(); } /// get_pos_error_xy_cm - get the length of the position error vector in the xy plane float get_pos_error_xy_cm() const { return norm(_pos_target.x - _inav.get_position().x, _pos_target.y - _inav.get_position().y); } @@ -396,7 +396,7 @@ protected: float _yaw_rate_target; // desired yaw rate in centi-degrees per second calculated by position controller // position controller internal variables - Vector3f _pos_target; // target location in NEU cm from home + Vector3p _pos_target; // target location in NEU cm from home Vector3f _vel_desired; // desired velocity in NEU cm/s Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward)