From d9c68031fa700511034a5bba271a14bcfe0f387b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 25 May 2021 13:35:23 +0900 Subject: [PATCH] AC_PosControl: write_log checks xy and z controllers are active This allows write_log to be write PSCZ messages for modes with onlyi vertical control (e.g. althold) without also logging out-of-date PSC messages --- .../AC_AttitudeControl/AC_PosControl.cpp | 19 +++++++++++-------- libraries/AC_AttitudeControl/AC_PosControl.h | 1 + 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0920b4df28..5bf72e7c19 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1081,19 +1081,22 @@ void AC_PosControl::standby_xyz_reset() init_ekf_xy_reset(); } -// write log to dataflash +// write PSC and/or PSCZ logs void AC_PosControl::write_log() { - float accel_x, accel_y; - lean_angles_to_accel_xy(accel_x, accel_y); + if (is_active_xy()) { + float accel_x, accel_y; + lean_angles_to_accel_xy(accel_x, accel_y); + AP::logger().Write_PSC(get_pos_target_cm(), _inav.get_position(), get_vel_target_cms(), _inav.get_velocity(), get_accel_target_cmss(), accel_x, accel_y); + } - AP::logger().Write_PSC(get_pos_target_cm(), _inav.get_position(), get_vel_target_cms(), _inav.get_velocity(), get_accel_target_cmss(), accel_x, accel_y); - AP::logger().Write_PSCZ(get_pos_target_cm().z, _inav.get_position().z, - get_vel_desired_cms().z, get_vel_target_cms().z, _inav.get_velocity().z, - _accel_desired.z, get_accel_target_cmss().z, get_z_accel_cmss(), _attitude_control.get_throttle_in()); + if (is_active_z()) { + AP::logger().Write_PSCZ(get_pos_target_cm().z, _inav.get_position().z, + get_vel_desired_cms().z, get_vel_target_cms().z, _inav.get_velocity().z, + _accel_desired.z, get_accel_target_cmss().z, get_z_accel_cmss(), _attitude_control.get_throttle_in()); + } } - /// /// private methods /// diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 59a687bfbd..fc8fb1b9c5 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -310,6 +310,7 @@ public: // lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s Vector3f lean_angles_to_accel(const Vector3f& att_target_euler) const; + // write PSC and/or PSCZ logs void write_log(); // provide feedback on whether arming would be a good idea right now: