mirror of https://github.com/ArduPilot/ardupilot
76 lines
2.7 KiB
C++
76 lines
2.7 KiB
C++
#include <AP_Logger/AP_Logger_config.h>
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
|
|
#include "AC_AttitudeControl.h"
|
|
#include "AC_PosControl.h"
|
|
#include <AP_Logger/AP_Logger.h>
|
|
#include <AP_Scheduler/AP_Scheduler.h>
|
|
#include "LogStructure.h"
|
|
|
|
// Write an ANG packet
|
|
void AC_AttitudeControl::Write_ANG() const
|
|
{
|
|
Vector3f targets = get_att_target_euler_rad() * RAD_TO_DEG;
|
|
|
|
const struct log_ANG pkt{
|
|
LOG_PACKET_HEADER_INIT(LOG_ANG_MSG),
|
|
time_us : AP::scheduler().get_loop_start_time_us(),
|
|
control_roll : targets.x,
|
|
roll : degrees(_ahrs.roll),
|
|
control_pitch : targets.y,
|
|
pitch : degrees(_ahrs.pitch),
|
|
control_yaw : wrap_360(targets.z),
|
|
yaw : wrap_360(degrees(_ahrs.yaw)),
|
|
sensor_dt : AP::scheduler().get_last_loop_time_s()
|
|
};
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
}
|
|
|
|
// Write a rate packet
|
|
void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const
|
|
{
|
|
const Vector3f &rate_targets = rate_bf_targets();
|
|
const Vector3f &accel_target = pos_control.get_accel_target_cmss();
|
|
const Vector3f &gyro_rate = _rate_gyro;
|
|
const struct log_Rate pkt_rate{
|
|
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
|
|
time_us : _rate_gyro_time_us,
|
|
control_roll : degrees(rate_targets.x),
|
|
roll : degrees(gyro_rate.x),
|
|
roll_out : _motors.get_roll()+_motors.get_roll_ff(),
|
|
control_pitch : degrees(rate_targets.y),
|
|
pitch : degrees(gyro_rate.y),
|
|
pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(),
|
|
control_yaw : degrees(rate_targets.z),
|
|
yaw : degrees(gyro_rate.z),
|
|
yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(),
|
|
control_accel : (float)accel_target.z,
|
|
accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f),
|
|
accel_out : _motors.get_throttle(),
|
|
throttle_slew : _motors.get_throttle_slew_rate()
|
|
};
|
|
AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
|
|
|
|
/*
|
|
log P/PD gain scale if not == 1.0
|
|
*/
|
|
const Vector3f &scale = get_last_angle_P_scale();
|
|
const Vector3f &pd_scale = _pd_scale_used;
|
|
if (scale != AC_AttitudeControl::VECTORF_111 || pd_scale != AC_AttitudeControl::VECTORF_111) {
|
|
const struct log_ATSC pkt_ATSC {
|
|
LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG),
|
|
time_us : _rate_gyro_time_us,
|
|
scaleP_x : scale.x,
|
|
scaleP_y : scale.y,
|
|
scaleP_z : scale.z,
|
|
scalePD_x : pd_scale.x,
|
|
scalePD_y : pd_scale.y,
|
|
scalePD_z : pd_scale.z,
|
|
};
|
|
AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));
|
|
}
|
|
}
|
|
|
|
#endif // HAL_LOGGING_ENABLED
|