Browse Source

Global: rename desired to target in PID info

master
Leonard Hall 6 years ago committed by Randy Mackay
parent
commit
157f786adf
  1. 12
      APMrover2/GCS_Mavlink.cpp
  2. 4
      AntennaTracker/GCS_Mavlink.cpp
  3. 2
      ArduCopter/GCS_Mavlink.cpp
  4. 2
      ArduPlane/GCS_Mavlink.cpp
  5. 4
      ArduPlane/quadplane.cpp
  6. 8
      ArduSub/GCS_Mavlink.cpp
  7. 2
      libraries/APM_Control/AP_PitchController.cpp
  8. 2
      libraries/APM_Control/AP_RollController.cpp
  9. 2
      libraries/APM_Control/AP_SteerController.cpp
  10. 2
      libraries/APM_Control/AP_YawController.h
  11. 2
      libraries/APM_Control/AR_AttitudeControl.cpp
  12. 2
      libraries/AP_Landing/AP_Landing_Deepstall.cpp
  13. 2
      libraries/AP_Logger/AP_Logger.h
  14. 2
      libraries/AP_Logger/LogFile.cpp
  15. 2
      libraries/AP_Logger/LogStructure.h
  16. 2
      libraries/PID/PID.cpp

12
APMrover2/GCS_Mavlink.cpp

@ -192,7 +192,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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,

4
AntennaTracker/GCS_Mavlink.cpp

@ -103,7 +103,7 @@ void GCS_MAVLINK_Tracker::send_pid_tuning() @@ -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() @@ -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,

2
ArduCopter/GCS_Mavlink.cpp

@ -223,7 +223,7 @@ void GCS_MAVLINK_Copter::send_pid_tuning() @@ -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,

2
ArduPlane/GCS_Mavlink.cpp

@ -321,7 +321,7 @@ void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info, @@ -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,

4
ArduPlane/quadplane.cpp

@ -805,8 +805,8 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) @@ -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);
}
}

8
ArduSub/GCS_Mavlink.cpp

@ -156,7 +156,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() @@ -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() @@ -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() @@ -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() @@ -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,

2
libraries/APM_Control/AP_PitchController.cpp

@ -186,7 +186,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool @@ -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) {

2
libraries/APM_Control/AP_RollController.cpp

@ -152,7 +152,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool @@ -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;

2
libraries/APM_Control/AP_SteerController.cpp

@ -139,7 +139,7 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) @@ -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

2
libraries/APM_Control/AP_YawController.h

@ -13,7 +13,7 @@ public: @@ -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;
}

2
libraries/APM_Control/AR_AttitudeControl.cpp

@ -613,7 +613,7 @@ float AR_AttitudeControl::get_desired_pitch() const @@ -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

2
libraries/AP_Landing/AP_Landing_Deepstall.cpp

@ -448,7 +448,7 @@ void AP_Landing_Deepstall::Log(void) const { @@ -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,

2
libraries/AP_Logger/AP_Logger.h

@ -283,7 +283,7 @@ public: @@ -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;

2
libraries/AP_Logger/LogFile.cpp

@ -795,7 +795,7 @@ void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info) @@ -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,

2
libraries/AP_Logger/LogStructure.h

@ -724,7 +724,7 @@ struct PACKED log_Attitude { @@ -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;

2
libraries/PID/PID.cpp

@ -105,7 +105,7 @@ float PID::get_pid(float error, float scaler) @@ -105,7 +105,7 @@ float PID::get_pid(float error, float scaler)
output += _integrator;
}
_pid_info.desired = output;
_pid_info.target = output;
return output;
}

Loading…
Cancel
Save