Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
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

19
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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
///

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
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: