From c233f114bd10d8753d8b46ed62eeadcc859a4f25 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 22 Feb 2022 14:36:10 +0900 Subject: [PATCH] AR_PosControl: fixup logging to record desired and target velocity and acceleration --- libraries/APM_Control/AR_PosControl.cpp | 92 ++++++++++++------------- libraries/APM_Control/AR_PosControl.h | 12 ++-- 2 files changed, 53 insertions(+), 51 deletions(-) diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index 4317915439..fad162d756 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -135,43 +135,43 @@ void AR_PosControl::update(float dt) _pid_vel.set_dt(dt); // calculate position error and convert to desired velocity - Vector2f des_vel_NE; + _vel_target.zero(); if (_pos_target_valid) { Vector2p pos_target = _pos_target; - des_vel_NE = _p_pos.update_all(pos_target.x, pos_target.y, curr_pos_NE); + _vel_target = _p_pos.update_all(pos_target.x, pos_target.y, curr_pos_NE); } // calculation velocity error - if (_vel_target_valid) { + if (_vel_desired_valid) { // add target velocity to desired velocity from position error - des_vel_NE += _vel_target; + _vel_target += _vel_desired; } // limit velocity to maximum speed - des_vel_NE.limit_length(get_speed_max()); + _vel_target.limit_length(get_speed_max()); // Limit the velocity to prevent fence violations bool backing_up = false; AC_Avoid *avoid = AP::ac_avoid(); if (avoid != nullptr) { - Vector3f vel_3d_cms{des_vel_NE.x * 100.0f, des_vel_NE.y * 100.0f, 0.0f}; + Vector3f vel_3d_cms{_vel_target.x * 100.0f, _vel_target.y * 100.0f, 0.0f}; const float accel_max_cmss = MIN(_accel_max, _lat_accel_max) * 100.0; avoid->adjust_velocity(vel_3d_cms, backing_up, _p_pos.kP(), accel_max_cmss, _p_pos.kP(), accel_max_cmss, dt); - des_vel_NE.x = vel_3d_cms.x * 0.01; - des_vel_NE.y = vel_3d_cms.y * 0.01; + _vel_target.x = vel_3d_cms.x * 0.01; + _vel_target.y = vel_3d_cms.y * 0.01; } // calculate desired acceleration // To-Do: fixup _limit_vel used below - Vector2f des_accel_NE = _pid_vel.update_all(des_vel_NE, curr_vel_NED.xy(), _limit_vel); - if (_accel_target_valid) { - des_accel_NE += _accel_target; + _accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), _limit_vel); + if (_accel_desired_valid) { + _accel_target += _accel_desired; } // convert desired acceleration to desired forward-back speed, desired lateral speed and desired turn rate // rotate acceleration into body frame using current heading - const Vector2f des_accel_FR = AP::ahrs().earth_to_body2D(des_accel_NE); + const Vector2f accel_target_FR = AP::ahrs().earth_to_body2D(_accel_target); // calculate minimum turn speed which is the max speed the vehicle could turn through the corner // given the vehicle's turn radius and half its max lateral acceleration @@ -179,22 +179,22 @@ void AR_PosControl::update(float dt) float turn_speed_min = MAX(safe_sqrt(_atc.get_turn_lat_accel_max() * 0.5 * _turn_radius), 0); // rotate target velocity from earth-frame to body frame - const Vector2f des_vel_FR = AP::ahrs().earth_to_body2D(des_vel_NE); + const Vector2f vel_target_FR = AP::ahrs().earth_to_body2D(_vel_target); // desired speed is normally the forward component (only) of the target velocity // but we do not let it fall below the minimum turn speed unless the vehicle is slowing down - const float abs_des_speed_min = MIN(des_vel_NE.length(), turn_speed_min); + const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min); float des_speed; if (_reversed != backing_up) { // if reversed or backing up desired speed will be negative - des_speed = MIN(-abs_des_speed_min, des_vel_FR.x); + des_speed = MIN(-abs_des_speed_min, vel_target_FR.x); } else { - des_speed = MAX(abs_des_speed_min, des_vel_FR.x); + des_speed = MAX(abs_des_speed_min, vel_target_FR.x); } _desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt); // calculate turn rate from desired lateral acceleration - _desired_lat_accel = des_accel_FR.y; + _desired_lat_accel = accel_target_FR.y; _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed); } @@ -242,8 +242,10 @@ bool AR_PosControl::init() _pos_target.y = pos_NE.y; // set target velocity and acceleration - _vel_target = vel_NED.xy(); - _accel_target = AP::ahrs().get_accel_ef_blended().xy(); + _vel_desired = vel_NED.xy(); + _vel_target.zero(); + _accel_desired = AP::ahrs().get_accel_ef_blended().xy(); + _accel_target.zero(); // clear reversed setting _reversed = false; @@ -260,37 +262,37 @@ bool AR_PosControl::init() void AR_PosControl::input_pos_target(const Vector2p &pos, float dt) { // adjust target position, velocity and acceleration forward by dt - update_pos_vel_accel_xy(_pos_target, _vel_target, _accel_target, dt, Vector2f(), Vector2f(), Vector2f()); + update_pos_vel_accel_xy(_pos_target, _vel_desired, _accel_desired, dt, Vector2f(), Vector2f(), Vector2f()); // call shape_pos_vel_accel_xy to pull target towards final destination Vector2f vel; Vector2f accel; const float accel_max = MIN(_accel_max, _lat_accel_max); - shape_pos_vel_accel_xy(pos, vel, accel, _pos_target, _vel_target, _accel_target, + shape_pos_vel_accel_xy(pos, vel, accel, _pos_target, _vel_desired, _accel_desired, _speed_max, accel_max, _jerk_max, dt, false); - // set flags so update will consume target position, velocity and acceleration + // set flags so update will consume target position, desired velocity and desired acceleration _pos_target_valid = true; - _vel_target_valid = true; - _accel_target_valid = true; + _vel_desired_valid = true; + _accel_desired_valid = true; } -// set position, velocity and acceleration targets +// set target position, desired velocity and acceleration. These should be from an externally created path and are not "input shaped" void AR_PosControl::set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel) { _pos_target = pos; - _vel_target = vel; - _accel_target = accel; + _vel_desired = vel; + _accel_desired = accel; _pos_target_valid = true; - _vel_target_valid = true; - _accel_target_valid = true; + _vel_desired_valid = true; + _accel_desired_valid = true; } // returns desired velocity vector (i.e. feed forward) in cm/s in lat and lon direction Vector2f AR_PosControl::get_desired_velocity() const { - if (_vel_target_valid) { - return _vel_target; + if (_vel_desired_valid) { + return _vel_desired; } return Vector2f(); } @@ -298,8 +300,8 @@ Vector2f AR_PosControl::get_desired_velocity() const // return desired acceleration vector in m/s in lat and lon direction Vector2f AR_PosControl::get_desired_accel() const { - if (_accel_target_valid) { - return _accel_target; + if (_accel_desired_valid) { + return _accel_desired; } return Vector2f(); } @@ -333,28 +335,26 @@ void AR_PosControl::write_log() } // get acceleration - const Vector3f curr_accel_NED;// = AP::ahrs().get_accel_ef_blended; + const Vector3f curr_accel_NED = AP::ahrs().get_accel_ef_blended() * 100.0; - // convert position, velocity and accel targets to required format + // convert position to required format Vector2f pos_target_2d_cm = get_pos_target().tofloat() * 100.0; - Vector2f vel_target_2d_cm = get_desired_velocity() * 100.0; - Vector2f accel_target_2d_cm = get_desired_accel() * 100.0; AP::logger().Write_PSCN(pos_target_2d_cm.x, // position target curr_pos_NED.x * 100.0, // position - 0.0, // desired velocity - vel_target_2d_cm.x, // target velocity + _vel_desired.x * 100.0, // desired velocity + _vel_target.x * 100.0, // target velocity curr_vel_NED.x * 100.0, // velocity - 0.0, // desired accel - accel_target_2d_cm.x, // target accel + _accel_desired.x * 100.0, // desired accel + _accel_target.x * 100.0, // target accel curr_accel_NED.x); // accel AP::logger().Write_PSCE(pos_target_2d_cm.y, // position target curr_pos_NED.y * 100.0, // position - 0.0, // desired velocity - vel_target_2d_cm.y, // target velocity + _vel_desired.y * 100.0, // desired velocity + _vel_target.y * 100.0, // target velocity curr_vel_NED.y * 100.0, // velocity - 0.0, // desired accel - accel_target_2d_cm.y, // target accel + _accel_desired.y * 100.0, // desired accel + _accel_target.y * 100.0, // target accel curr_accel_NED.y); // accel } @@ -382,7 +382,7 @@ void AR_PosControl::handle_ekf_xy_reset() if (!AP::ahrs().get_velocity_NED(vel_NED)) { return; } - _vel_target = vel_NED.xy() + _pid_vel.get_error(); + _vel_desired = vel_NED.xy() + _pid_vel.get_error(); _ekf_xy_reset_ms = reset_ms; } diff --git a/libraries/APM_Control/AR_PosControl.h b/libraries/APM_Control/AR_PosControl.h index ed0d65853f..87ed932426 100644 --- a/libraries/APM_Control/AR_PosControl.h +++ b/libraries/APM_Control/AR_PosControl.h @@ -43,7 +43,7 @@ public: // init should be called once before starting to use these methods void input_pos_target(const Vector2p &pos, float dt); - // set position, velocity and acceleration targets. These should be from an externally created path and are not "input shaped" + // set target position, desired velocity and acceleration. These should be from an externally created path and are not "input shaped" void set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel); // get outputs for forward-back speed (in m/s), lateral speed (in m/s) and turn rate (in rad/sec) @@ -54,10 +54,10 @@ public: // get position target const Vector2p& get_pos_target() const { return _pos_target; } - // returns desired velocity vector (i.e. feed forward) in m/s in lat and lon direction + // returns desired velocity vector (i.e. feed forward) in m/s in NE frame Vector2f get_desired_velocity() const; - // return desired acceleration vector in m/s in lat and lon direction + // return desired acceleration vector in m/s in NE frame Vector2f get_desired_accel() const; /// get position error as a vector from the current position to the target position @@ -96,11 +96,13 @@ private: // position and velocity targets Vector2p _pos_target; // position target as an offset (in meters) from the EKF origin + Vector2f _vel_desired; // desired velocity in m/s in NE frame. This is the "feed forward" provided by SCurves Vector2f _vel_target; // velocity target in m/s in NE frame + Vector2f _accel_desired; // desired accel in m/s/s in NE frame. This is the "feed forward" provided by SCurves Vector2f _accel_target; // accel target in m/s/s in NE frame bool _pos_target_valid; // true if _pos_target is valid - bool _vel_target_valid; // true if _vel_target is valid - bool _accel_target_valid; // true if _accel_target is valid + bool _vel_desired_valid; // true if _vel_desired is valid + bool _accel_desired_valid; // true if _accel_desired is valid // variables for navigation uint32_t _last_update_ms; // system time of last call to update