From 157f786adfc5a699970652881f9eed0ae9c79b63 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 27 Jun 2019 19:01:52 +0930 Subject: [PATCH] Global: rename desired to target in PID info --- APMrover2/GCS_Mavlink.cpp | 12 ++++++------ AntennaTracker/GCS_Mavlink.cpp | 4 ++-- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/quadplane.cpp | 4 ++-- ArduSub/GCS_Mavlink.cpp | 8 ++++---- libraries/APM_Control/AP_PitchController.cpp | 2 +- libraries/APM_Control/AP_RollController.cpp | 2 +- libraries/APM_Control/AP_SteerController.cpp | 2 +- libraries/APM_Control/AP_YawController.h | 2 +- libraries/APM_Control/AR_AttitudeControl.cpp | 2 +- libraries/AP_Landing/AP_Landing_Deepstall.cpp | 2 +- libraries/AP_Logger/AP_Logger.h | 2 +- libraries/AP_Logger/LogFile.cpp | 2 +- libraries/AP_Logger/LogStructure.h | 2 +- libraries/PID/PID.cpp | 2 +- 16 files changed, 26 insertions(+), 26 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 85fe98cfb0..9d23bfb5c0 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -192,7 +192,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() if (g.gcs_pid_mask & 1) { pid_info = &g2.attitude_control.get_steering_rate_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER, - degrees(pid_info->desired), + degrees(pid_info->target), degrees(ahrs.get_yaw_rate_earth()), pid_info->FF, pid_info->P, @@ -209,7 +209,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() float speed = 0.0f; g2.attitude_control.get_forward_speed(speed); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, - pid_info->desired, + pid_info->target, speed, 0, pid_info->P, @@ -224,7 +224,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() if (g.gcs_pid_mask & 4) { pid_info = &g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, - degrees(pid_info->desired), + degrees(pid_info->target), degrees(ahrs.pitch), pid_info->FF, pid_info->P, @@ -239,7 +239,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() if (g.gcs_pid_mask & 8) { pid_info = &g2.wheel_rate_control.get_pid(0).get_pid_info(); mavlink_msg_pid_tuning_send(chan, 7, - pid_info->desired, + pid_info->target, pid_info->actual, pid_info->FF, pid_info->P, @@ -254,7 +254,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() if (g.gcs_pid_mask & 16) { pid_info = &g2.wheel_rate_control.get_pid(1).get_pid_info(); mavlink_msg_pid_tuning_send(chan, 8, - pid_info->desired, + pid_info->target, pid_info->actual, pid_info->FF, pid_info->P, @@ -269,7 +269,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() if (g.gcs_pid_mask & 32) { pid_info = &g2.attitude_control.get_sailboat_heel_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, 9, - pid_info->desired, + pid_info->target, pid_info->actual, pid_info->FF, pid_info->P, diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 14b4ec5176..0a32fd6e6f 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -103,7 +103,7 @@ void GCS_MAVLINK_Tracker::send_pid_tuning() const AP_Logger::PID_Info *pid_info; pid_info = &g.pidPitch2Srv.get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, - pid_info->desired, + pid_info->target, pid_info->actual, pid_info->FF, pid_info->P, @@ -119,7 +119,7 @@ void GCS_MAVLINK_Tracker::send_pid_tuning() const AP_Logger::PID_Info *pid_info; pid_info = &g.pidYaw2Srv.get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, - pid_info->desired, + pid_info->target, pid_info->actual, pid_info->FF, pid_info->P, diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index e7dcb70b80..d32ee6ee4f 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -223,7 +223,7 @@ void GCS_MAVLINK_Copter::send_pid_tuning() if (pid_info != nullptr) { mavlink_msg_pid_tuning_send(chan, axes[i], - pid_info->desired*0.01f, + pid_info->target*0.01f, achieved, pid_info->FF*0.01f, pid_info->P*0.01f, diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 01ac2a9f6a..162d286152 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -321,7 +321,7 @@ void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info, return; } mavlink_msg_pid_tuning_send(chan, axis, - pid_info->desired, + pid_info->target, achieved, pid_info->FF, pid_info->P, diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a0a63fedb9..a0499a1795 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -805,8 +805,8 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) yaw_rate_cds); } else { // use the fixed wing desired rates - float roll_rate = plane.rollController.get_pid_info().desired; - float pitch_rate = plane.pitchController.get_pid_info().desired; + float roll_rate = plane.rollController.get_pid_info().target; + float pitch_rate = plane.pitchController.get_pid_info().target; attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds); } } diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index cdbe75abf0..081cba6dc1 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -156,7 +156,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() if (g.gcs_pid_mask & 1) { const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL, - pid_info.desired*0.01f, + pid_info.target*0.01f, degrees(gyro.x), pid_info.FF*0.01f, pid_info.P*0.01f, @@ -169,7 +169,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() if (g.gcs_pid_mask & 2) { const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, - pid_info.desired*0.01f, + pid_info.target*0.01f, degrees(gyro.y), pid_info.FF*0.01f, pid_info.P*0.01f, @@ -182,7 +182,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() if (g.gcs_pid_mask & 4) { const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, - pid_info.desired*0.01f, + pid_info.target*0.01f, degrees(gyro.z), pid_info.FF*0.01f, pid_info.P*0.01f, @@ -195,7 +195,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() if (g.gcs_pid_mask & 8) { const AP_Logger::PID_Info &pid_info = sub.pos_control.get_accel_z_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, - pid_info.desired*0.01f, + pid_info.target*0.01f, -(ahrs.get_accel_ef_blended().z + GRAVITY_MSS), pid_info.FF*0.01f, pid_info.P*0.01f, diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 1f48d8f6bb..fdf6fc982e 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -186,7 +186,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool _pid_info.FF = desired_rate * k_ff * scaler; _pid_info.D = rate_error * gains.D * scaler; _last_out = _pid_info.D + _pid_info.FF + _pid_info.P; - _pid_info.desired = desired_rate; + _pid_info.target = desired_rate; _pid_info.actual = achieved_rate; if (autotune.running && aspeed > aparm.airspeed_min) { diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 805e845375..d0a94064cc 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -152,7 +152,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool _pid_info.D = rate_error * gains.D * scaler; _pid_info.P = desired_rate * kp_ff * scaler; _pid_info.FF = desired_rate * k_ff * scaler; - _pid_info.desired = desired_rate; + _pid_info.target = desired_rate; _pid_info.actual = achieved_rate; _last_out = _pid_info.FF + _pid_info.P + _pid_info.D; diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp index 01391873d3..3c6d2e32cf 100644 --- a/libraries/APM_Control/AP_SteerController.cpp +++ b/libraries/APM_Control/AP_SteerController.cpp @@ -139,7 +139,7 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) // equation for a ground vehicle. It returns steering as an angle from -45 to 45 float scaler = 1.0f / speed; - _pid_info.desired = desired_rate; + _pid_info.target = desired_rate; // Calculate the steering rate error (deg/sec) and apply gain scaler // We do this in earth frame to allow for rover leaning over in hard corners diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 46a04c71d4..611483c4f2 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -13,7 +13,7 @@ public: , _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); - _pid_info.desired = 0; + _pid_info.target = 0; _pid_info.FF = 0; _pid_info.P = 0; } diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 26507ac139..4161cdb972 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -613,7 +613,7 @@ float AR_AttitudeControl::get_desired_pitch() const return 0.0f; } - return _pitch_to_throttle_pid.get_pid_info().desired; + return _pitch_to_throttle_pid.get_pid_info().target; } // Sailboat heel(roll) angle contorller release sail to keep at maximum heel angle diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index 3c1d05e9f2..d54deb4dc6 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -448,7 +448,7 @@ void AP_Landing_Deepstall::Log(void) const { constrain_float(predicted_travel_distance * 1e2f, (float)INT16_MIN, (float)INT16_MAX) : 0), l1_i : stage >= DEEPSTALL_STAGE_LAND ? L1_xtrack_i : 0.0f, loiter_sum_cd : stage >= DEEPSTALL_STAGE_ESTIMATE_WIND ? loiter_sum_cd : 0, - desired : pid_info.desired, + desired : pid_info.target, P : pid_info.P, I : pid_info.I, D : pid_info.D, diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 1133ad3ff9..8e588fb6c6 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -283,7 +283,7 @@ public: // This structure provides information on the internal member data of a PID for logging purposes struct PID_Info { - float desired; + float target; float actual; float P; float I; diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 61fb9e734b..f78394f4e6 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -795,7 +795,7 @@ void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info) struct log_PID pkt = { LOG_PACKET_HEADER_INIT(msg_type), time_us : AP_HAL::micros64(), - desired : info.desired, + target : info.target, actual : info.actual, P : info.P, I : info.I, diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 4961a13d3c..471e952c2a 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -724,7 +724,7 @@ struct PACKED log_Attitude { struct PACKED log_PID { LOG_PACKET_HEADER; uint64_t time_us; - float desired; + float target; float actual; float P; float I; diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index e65203b0de..141afcd05f 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -105,7 +105,7 @@ float PID::get_pid(float error, float scaler) output += _integrator; } - _pid_info.desired = output; + _pid_info.target = output; return output; }