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