diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 14d3a1fc7f..d95651fbd0 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -1,7 +1,9 @@ #include "AC_AttitudeControl.h" +#include "AC_PosControl.h" #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -1240,3 +1242,51 @@ 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 b396828258..104720f38a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -428,8 +428,8 @@ public: // purposes void set_PD_scale_mult(const Vector3f &pd_scale) { _pd_scale *= pd_scale; } - // get the value of the PD scale that was used in the last loop, for logging - const Vector3f &get_PD_scale_logging(void) const { return _pd_scale_used; } + // write RATE message + void Write_Rate(const AC_PosControl &pos_control) const; // User settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -487,6 +487,11 @@ protected: AP_Float _land_pitch_mult; AP_Float _land_yaw_mult; + // latest gyro value use by the rate_controller + Vector3f _rate_gyro; + // timestamp of the latest gyro value used by the rate controller + uint64_t _rate_gyro_time_us; + // Intersampling period in seconds float _dt; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 39c9ad277e..4c67892fe0 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -422,7 +422,8 @@ void AC_AttitudeControl_Heli::rate_controller_run() { _ang_vel_body += _sysid_ang_vel_body; - Vector3f gyro_latest = _ahrs.get_gyro_latest(); + _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro_time_us = AP_HAL::micros64(); // call rate controllers and send output to motors object // if using a flybar passthrough roll and pitch directly to motors @@ -430,12 +431,12 @@ void AC_AttitudeControl_Heli::rate_controller_run() _motors.set_roll(_passthrough_roll / 4500.0f); _motors.set_pitch(_passthrough_pitch / 4500.0f); } else { - rate_bf_to_motor_roll_pitch(gyro_latest, _ang_vel_body.x, _ang_vel_body.y); + rate_bf_to_motor_roll_pitch(_rate_gyro, _ang_vel_body.x, _ang_vel_body.y); } if (_flags_heli.tail_passthrough) { _motors.set_yaw(_passthrough_yaw / 4500.0f); } else { - _motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _ang_vel_body.z)); + _motors.set_yaw(rate_target_to_motor_yaw(_rate_gyro.z, _ang_vel_body.z)); } _sysid_ang_vel_body.zero(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 947975a0a1..91378c8bfa 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -449,15 +449,16 @@ void AC_AttitudeControl_Multi::rate_controller_run() _ang_vel_body += _sysid_ang_vel_body; - Vector3f gyro_latest = _ahrs.get_gyro_latest(); + _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro_time_us = AP_HAL::micros64(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); + _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); _motors.set_roll_ff(get_rate_roll_pid().get_ff()); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); + _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); _motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); + _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar); _sysid_ang_vel_body.zero(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index 7f2791c241..e83abb39ae 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -423,10 +423,12 @@ void AC_AttitudeControl_Sub::rate_controller_run() // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration) update_throttle_rpy_mix(); - Vector3f gyro_latest = _ahrs.get_gyro_latest(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll)); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch)); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw)); + _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro_time_us = AP_HAL::micros64(); + + _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll)); + _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch)); + _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw)); control_monitor_update(); } diff --git a/libraries/AC_AttitudeControl/LogStructure.h b/libraries/AC_AttitudeControl/LogStructure.h index 9b5fe8df95..8290ec3d09 100644 --- a/libraries/AC_AttitudeControl/LogStructure.h +++ b/libraries/AC_AttitudeControl/LogStructure.h @@ -58,6 +58,40 @@ struct PACKED log_PSCx { float accel; }; +// @LoggerMessage: RATE +// @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes. +// @Field: TimeUS: Time since system startup +// @Field: RDes: vehicle desired roll rate +// @Field: R: achieved vehicle roll rate +// @Field: ROut: normalized output for Roll +// @Field: PDes: vehicle desired pitch rate +// @Field: P: vehicle pitch rate +// @Field: POut: normalized output for Pitch +// @Field: Y: achieved vehicle yaw rate +// @Field: YOut: normalized output for Yaw +// @Field: YDes: vehicle desired yaw rate +// @Field: ADes: desired vehicle vertical acceleration +// @Field: A: achieved vehicle vertical acceleration +// @Field: AOut: percentage of vertical thrust output current being used +// @Field: AOutSlew: vertical thrust output slew rate +struct PACKED log_Rate { + LOG_PACKET_HEADER; + uint64_t time_us; + float control_roll; + float roll; + float roll_out; + float control_pitch; + float pitch; + float pitch_out; + float control_yaw; + float yaw; + float yaw_out; + float control_accel; + float accel; + float accel_out; + float throttle_slew; +}; + #define PSCx_FMT "Qffffffff" #define PSCx_UNITS "smmnnnooo" #define PSCx_MULTS "F00000000" @@ -68,4 +102,6 @@ struct PACKED log_PSCx { { LOG_PSCE_MSG, sizeof(log_PSCx), \ "PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", 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,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 }