mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
Copter: use ahrs singleton to log ATT, POS and AHRS2
This commit is contained in:
parent
8948d736c1
commit
45d080bc55
@ -72,7 +72,7 @@ void Copter::Log_Write_Attitude()
|
|||||||
{
|
{
|
||||||
Vector3f targets = attitude_control->get_att_target_euler_cd();
|
Vector3f targets = attitude_control->get_att_target_euler_cd();
|
||||||
targets.z = wrap_360_cd(targets.z);
|
targets.z = wrap_360_cd(targets.z);
|
||||||
logger.Write_Attitude(ahrs, targets);
|
logger.Write_Attitude(targets);
|
||||||
logger.Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
logger.Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
||||||
if (should_log(MASK_LOG_PID)) {
|
if (should_log(MASK_LOG_PID)) {
|
||||||
logger.Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
|
logger.Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
|
||||||
@ -86,11 +86,11 @@ void Copter::Log_Write_Attitude()
|
|||||||
void Copter::Log_Write_EKF_POS()
|
void Copter::Log_Write_EKF_POS()
|
||||||
{
|
{
|
||||||
AP::ahrs_navekf().Log_Write();
|
AP::ahrs_navekf().Log_Write();
|
||||||
logger.Write_AHRS2(ahrs);
|
logger.Write_AHRS2();
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
sitl.Log_Write_SIMSTATE();
|
sitl.Log_Write_SIMSTATE();
|
||||||
#endif
|
#endif
|
||||||
logger.Write_POS(ahrs);
|
logger.Write_POS();
|
||||||
}
|
}
|
||||||
|
|
||||||
struct PACKED log_MotBatt {
|
struct PACKED log_MotBatt {
|
||||||
|
Loading…
Reference in New Issue
Block a user