From 790a1ddf7a318ddcc66f6f054e476232be45bf08 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Sep 2024 21:18:52 +0900 Subject: [PATCH] AC_PosControl: PSCx gets offsets --- .../AC_AttitudeControl/AC_PosControl.cpp | 19 +++++----- libraries/AC_AttitudeControl/AC_PosControl.h | 16 ++++++--- .../AC_PosControl_Logging.cpp | 35 ++++++++++++++----- libraries/AC_AttitudeControl/LogStructure.h | 32 +++++++++++++---- 4 files changed, 75 insertions(+), 27 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 9a02e4d1cfd4c8..cbc371a34e04d4 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1322,18 +1322,19 @@ void AC_PosControl::write_log() if (is_active_xy()) { float accel_x, accel_y; lean_angles_to_accel_xy(accel_x, accel_y); - Write_PSCN(get_pos_target_cm().x, _inav.get_position_neu_cm().x, - get_vel_desired_cms().x, get_vel_target_cms().x, _inav.get_velocity_neu_cms().x, - _accel_desired.x, get_accel_target_cmss().x, accel_x); - Write_PSCE(get_pos_target_cm().y, _inav.get_position_neu_cm().y, - get_vel_desired_cms().y, get_vel_target_cms().y, _inav.get_velocity_neu_cms().y, - _accel_desired.y, get_accel_target_cmss().y, accel_y); + + Write_PSCN(get_pos_target_cm().x, _inav.get_position_neu_cm().x, _pos_offset_target.x, _pos_offset.x, + get_vel_desired_cms().x, get_vel_target_cms().x, _inav.get_velocity_neu_cms().x, _vel_offset_target.x, _vel_offset.x, + _accel_desired.x, get_accel_target_cmss().x, accel_x, _accel_offset.x); + Write_PSCE(get_pos_target_cm().y, _inav.get_position_neu_cm().y, _pos_offset_target.y, _pos_offset.y, + get_vel_desired_cms().y, get_vel_target_cms().y, _inav.get_velocity_neu_cms().y, _vel_offset_target.y, _vel_offset.y, + _accel_desired.y, get_accel_target_cmss().y, accel_y, _accel_offset.y); } if (is_active_z()) { - Write_PSCD(-get_pos_target_cm().z, -_inav.get_position_z_up_cm(), - -get_vel_desired_cms().z, -get_vel_target_cms().z, -_inav.get_velocity_z_up_cms(), - -_accel_desired.z, -get_accel_target_cmss().z, -get_z_accel_cmss()); + Write_PSCD(-get_pos_target_cm().z, -_inav.get_position_z_up_cm(), 0, 0, + -get_vel_desired_cms().z, -get_vel_target_cms().z, -_inav.get_velocity_z_up_cms(), 0, 0, + -_accel_desired.z, -get_accel_target_cmss().z, -get_z_accel_cmss(), 0); } } #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 8e5f21b4b22dea..24637056c8979f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -417,9 +417,15 @@ class AC_PosControl static const struct AP_Param::GroupInfo var_info[]; - static void Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); - static void Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); - static void Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); + static void Write_PSCN(float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset); + static void Write_PSCE(float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset); + static void Write_PSCD(float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset); protected: @@ -534,6 +540,8 @@ class AC_PosControl private: // convenience method for writing out the identical PSCE, PSCN, PSCD - and // to save bytes - static void Write_PSCx(LogMessages ID, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); + static void Write_PSCx(LogMessages ID, float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset); }; diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp index d02760cdeb62ec..0d53efd54b3a44 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp @@ -8,36 +8,55 @@ #include "LogStructure.h" // a convenience function for writing out the position controller PIDs -void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) +void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset) { const struct log_PSCx pkt{ LOG_PACKET_HEADER_INIT(id), time_us : AP_HAL::micros64(), pos_target : pos_target * 0.01f, pos : pos * 0.01f, + pos_target_offset: pos_target_offset * 0.01f, + pos_offset : pos_offset * 0.01f, vel_desired : vel_desired * 0.01f, vel_target : vel_target * 0.01f, vel : vel * 0.01f, + vel_target_offset: vel_target_offset * 0.01f, + vel_offset : vel_offset * 0.01f, accel_desired : accel_desired * 0.01f, accel_target : accel_target * 0.01f, - accel : accel * 0.01f + accel : accel * 0.01f, + accel_offset : accel_offset * 0.01f }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void AC_PosControl::Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) +void AC_PosControl::Write_PSCN(float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset) { - Write_PSCx(LOG_PSCN_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel); + Write_PSCx(LOG_PSCN_MSG, pos_target, pos, pos_target_offset, pos_offset, + vel_desired, vel_target, vel, vel_target_offset, vel_offset, + accel_desired, accel_target, accel, accel_offset); } -void AC_PosControl::Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) +void AC_PosControl::Write_PSCE(float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset) { - Write_PSCx(LOG_PSCE_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel); + Write_PSCx(LOG_PSCE_MSG, pos_target, pos, pos_target_offset, pos_offset, + vel_desired, vel_target, vel, vel_target_offset, vel_offset, + accel_desired, accel_target, accel, accel_offset); } -void AC_PosControl::Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) +void AC_PosControl::Write_PSCD(float pos_target, float pos, float pos_target_offset, float pos_offset, + float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset, + float accel_desired, float accel_target, float accel, float accel_offset) { - Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel); + Write_PSCx(LOG_PSCD_MSG, pos_target, pos, pos_target_offset, pos_offset, + vel_desired, vel_target, vel, vel_target_offset, vel_offset, + accel_desired, accel_target, accel, accel_offset); } #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AC_AttitudeControl/LogStructure.h b/libraries/AC_AttitudeControl/LogStructure.h index 8290ec3d09f202..e8cf0c79b97883 100644 --- a/libraries/AC_AttitudeControl/LogStructure.h +++ b/libraries/AC_AttitudeControl/LogStructure.h @@ -12,36 +12,51 @@ // @Field: TimeUS: Time since system startup // @Field: TPN: Target position relative to EKF origin // @Field: PN: Position relative to EKF origin +// @Field: TPON: Target position offset North +// @Field: PON: Position offset North // @Field: DVN: Desired velocity North // @Field: TVN: Target velocity North // @Field: VN: Velocity North +// @Field: TVON: Target velocity offset North +// @Field: VON: Velocity offset North // @Field: DAN: Desired acceleration North // @Field: TAN: Target acceleration North // @Field: AN: Acceleration North +// @Field: AON: Acceleration offset North // @LoggerMessage: PSCE // @Description: Position Control East // @Field: TimeUS: Time since system startup // @Field: TPE: Target position relative to EKF origin // @Field: PE: Position relative to EKF origin +// @Field: TPOE: Target position offset East +// @Field: POE: Position offset East // @Field: DVE: Desired velocity East // @Field: TVE: Target velocity East // @Field: VE: Velocity East +// @Field: TVOE: Target velocity offset East +// @Field: VOE: Velocity offset East // @Field: DAE: Desired acceleration East // @Field: TAE: Target acceleration East // @Field: AE: Acceleration East +// @Field: AOE: Acceleration offset East // @LoggerMessage: PSCD // @Description: Position Control Down // @Field: TimeUS: Time since system startup // @Field: TPD: Target position relative to EKF origin // @Field: PD: Position relative to EKF origin +// @Field: TPOD: Target position offset Down +// @Field: POD: Position offset Down // @Field: DVD: Desired velocity Down // @Field: TVD: Target velocity Down // @Field: VD: Velocity Down +// @Field: TVOD: Target velocity offset Down +// @Field: VOD: Velocity offset Down // @Field: DAD: Desired acceleration Down // @Field: TAD: Target acceleration Down // @Field: AD: Acceleration Down +// @Field: AOD: Acceleration offset Down // position controller per-axis logging @@ -50,12 +65,17 @@ struct PACKED log_PSCx { uint64_t time_us; float pos_target; float pos; + float pos_target_offset; + float pos_offset; float vel_desired; float vel_target; float vel; + float vel_target_offset; + float vel_offset; float accel_desired; float accel_target; float accel; + float accel_offset; }; // @LoggerMessage: RATE @@ -92,16 +112,16 @@ struct PACKED log_Rate { float throttle_slew; }; -#define PSCx_FMT "Qffffffff" -#define PSCx_UNITS "smmnnnooo" -#define PSCx_MULTS "F00000000" +#define PSCx_FMT "Qfffffffffffff" +#define PSCx_UNITS "smmmmnnnnnoooo" +#define PSCx_MULTS "F0000000000000" #define LOG_STRUCTURE_FROM_AC_ATTITUDECONTROL \ { LOG_PSCN_MSG, sizeof(log_PSCx), \ - "PSCN", PSCx_FMT, "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", PSCx_UNITS, PSCx_MULTS }, \ + "PSCN", PSCx_FMT, "TimeUS,TPN,PN,TPON,PON,DVN,TVN,VN,TVON,VON,DAN,TAN,AN,AON", PSCx_UNITS, PSCx_MULTS }, \ { LOG_PSCE_MSG, sizeof(log_PSCx), \ - "PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \ + "PSCE", PSCx_FMT, "TimeUS,TPE,PE,TPOE,POE,DVE,TVE,VE,TVOE,VOE,DAE,TAE,AE,AON", PSCx_UNITS, PSCx_MULTS }, \ { LOG_PSCD_MSG, sizeof(log_PSCx), \ - "PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \ + "PSCD", PSCx_FMT, "TimeUS,TPD,PD,TPOD,POD,DVD,TVD,VD,TVOD,VOD,DAD,TAD,AD,AOD", 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 }