Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: write_log checks xy and z controllers are active

Browse Source 
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
This commit is contained in:
Randy Mackay 2021-05-25 13:35:23 +09:00 committed by Andrew Tridgell
parent 86f39d354f
commit d9c68031fa
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1081,19 +1081,22 @@ void AC_PosControl::standby_xyz_reset()
init_ekf_xy_reset(); init_ekf_xy_reset();
} }
// write log to dataflash // write PSC and/or PSCZ logs
void AC_PosControl::write_log() void AC_PosControl::write_log()
{ {
if (is_active_xy()) {
float accel_x, accel_y; float accel_x, accel_y;
lean_angles_to_accel_xy(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);
}
if (is_active_z()) {
AP::logger().Write_PSCZ(get_pos_target_cm().z, _inav.get_position().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, 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()); _accel_desired.z, get_accel_target_cmss().z, get_z_accel_cmss(), _attitude_control.get_throttle_in());
}
} }
/// ///
/// private methods /// private methods
/// ///

1
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -310,6 +310,7 @@ public:
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s // 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; Vector3f lean_angles_to_accel(const Vector3f& att_target_euler) const;
// write PSC and/or PSCZ logs
void write_log(); void write_log();
// provide feedback on whether arming would be a good idea right now: // provide feedback on whether arming would be a good idea right now: