From 71e2b756afd57d1fd9c7a883bd5331c1d2485dd6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 10 Sep 2024 11:52:38 +0100 Subject: [PATCH] AC_AttitudeControl: introduce ANG log message for high resolution attitude logging Move RATE message to AC_AttitudeControl_Logging.cpp --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 50 ------------- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 + .../AC_AttitudeControl_Logging.cpp | 75 +++++++++++++++++++ libraries/AC_AttitudeControl/LogStructure.h | 29 ++++++- 4 files changed, 105 insertions(+), 52 deletions(-) create mode 100644 libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index a42e031af0..ef3317fe6b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -1,9 +1,7 @@ #include "AC_AttitudeControl.h" -#include "AC_PosControl.h" #include #include #include -#include extern const AP_HAL::HAL& hal; @@ -1263,51 +1261,3 @@ void AC_AttitudeControl::get_rpy_srate(float &roll_srate, float &pitch_srate, fl pitch_srate = get_rate_pitch_pid().get_pid_info().slew_rate; yaw_srate = get_rate_yaw_pid().get_pid_info().slew_rate; } - -#if HAL_LOGGING_ENABLED -// 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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index cb54ac9205..70271ac9cc 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -434,6 +434,9 @@ public: // write RATE message void Write_Rate(const AC_PosControl &pos_control) const; + // write ANG message + void Write_ANG() const; + // User settable parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp new file mode 100644 index 0000000000..80d7512742 --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp @@ -0,0 +1,75 @@ +#include + +#if HAL_LOGGING_ENABLED + +#include "AC_AttitudeControl.h" +#include "AC_PosControl.h" +#include +#include +#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 diff --git a/libraries/AC_AttitudeControl/LogStructure.h b/libraries/AC_AttitudeControl/LogStructure.h index 8290ec3d09..0155e7e650 100644 --- a/libraries/AC_AttitudeControl/LogStructure.h +++ b/libraries/AC_AttitudeControl/LogStructure.h @@ -5,7 +5,8 @@ #define LOG_IDS_FROM_AC_ATTITUDECONTROL \ LOG_PSCN_MSG, \ LOG_PSCE_MSG, \ - LOG_PSCD_MSG + LOG_PSCD_MSG, \ + LOG_ANG_MSG // @LoggerMessage: PSCN // @Description: Position Control North @@ -92,6 +93,28 @@ struct PACKED log_Rate { float throttle_slew; }; +// @LoggerMessage: ANG +// @Description: Attitude control attitude +// @Field: TimeUS: Timestamp of the current Attitude loop +// @Field: DesRoll: vehicle desired roll +// @Field: Roll: achieved vehicle roll +// @Field: DesPitch: vehicle desired pitch +// @Field: Pitch: achieved vehicle pitch +// @Field: DesYaw: vehicle desired yaw +// @Field: Yaw: achieved vehicle yaw +// @Field: Dt: attitude delta time +struct PACKED log_ANG { + LOG_PACKET_HEADER; + uint64_t time_us; + float control_roll; + float roll; + float control_pitch; + float pitch; + float control_yaw; + float yaw; + float sensor_dt; +}; + #define PSCx_FMT "Qffffffff" #define PSCx_UNITS "smmnnnooo" #define PSCx_MULTS "F00000000" @@ -104,4 +127,6 @@ struct PACKED log_Rate { { LOG_PSCD_MSG, sizeof(log_PSCx), \ "PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \ { LOG_RATE_MSG, sizeof(log_Rate), \ - "RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true } + "RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }, \ + { LOG_ANG_MSG, sizeof(log_ANG),\ + "ANG", "Qfffffff", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,Dt", "sddddhhs", "F0000000" , true }