Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: align GCS PID with logged

Browse Source
This commit is contained in:
Iampete1 2020-05-17 18:15:59 +01:00 committed by Andrew Tridgell
parent aa657626f2
commit 13d6a887fd
1 changed files with 6 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/GCS_Mavlink.cpp
View File

@ -186,7 +186,6 @@ int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const
*/
void GCS_MAVLINK_Copter::send_pid_tuning()
{
const Vector3f &gyro = AP::ahrs().get_gyro();
static const PID_TUNING_AXIS axes[] = {
PID_TUNING_ROLL,
PID_TUNING_PITCH,
@ -201,23 +200,18 @@ void GCS_MAVLINK_Copter::send_pid_tuning()
return;
}
const AP_Logger::PID_Info *pid_info = nullptr;
float achieved;
switch (axes[i]) {
case PID_TUNING_ROLL:
pid_info = &copter.attitude_control->get_rate_roll_pid().get_pid_info();
achieved = degrees(gyro.x);
break;
case PID_TUNING_PITCH:
pid_info = &copter.attitude_control->get_rate_pitch_pid().get_pid_info();
achieved = degrees(gyro.y);
break;
case PID_TUNING_YAW:
pid_info = &copter.attitude_control->get_rate_yaw_pid().get_pid_info();
achieved = degrees(gyro.z);
break;
case PID_TUNING_ACCZ:
pid_info = &copter.pos_control->get_accel_z_pid().get_pid_info();
achieved = -(AP::ahrs().get_accel_ef_blended().z + GRAVITY_MSS);
break;
default:
continue;
@ -225,12 +219,12 @@ void GCS_MAVLINK_Copter::send_pid_tuning()
if (pid_info != nullptr) {
mavlink_msg_pid_tuning_send(chan,
axes[i],
pid_info->target*0.01f,
achieved,
pid_info->FF*0.01f,
pid_info->P*0.01f,
pid_info->I*0.01f,
pid_info->D*0.01f);
pid_info->target,
pid_info->actual,
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
}
}
}