diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 335b4bcc9d466..cb07bd3d9a2f1 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -76,8 +76,8 @@ void Copter::Log_Write_Control_Tuning() void Copter::Log_Write_Attitude() { ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG); - ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); - } + attitude_control->Write_Rate(*pos_control); +} // Write PIDS packets void Copter::Log_Write_PIDS() diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0a41b5551b6e6..3c0d2e18fd259 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1764,7 +1764,7 @@ void QuadPlane::update(void) const bool motors_active = in_vtol_mode() || assisted_flight; if (motors_active && (motors->get_spool_state() != AP_Motors::SpoolState::SHUT_DOWN)) { // log RATE at main loop rate - ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); + attitude_control->Write_Rate(*pos_control); // log CTRL and MOTB at 10 Hz if (now - last_ctrl_log_ms > 100) { diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index cf1c6f9abcbe3..b66dc2b644a85 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -183,7 +183,7 @@ void Sub::ten_hz_logging_loop() // log attitude data if we're not already logging at the higher rate if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); - ahrs_view.Write_Rate(motors, attitude_control, pos_control); + attitude_control.Write_Rate(pos_control); 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_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); @@ -222,7 +222,7 @@ void Sub::twentyfive_hz_logging() { if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); - ahrs_view.Write_Rate(motors, attitude_control, pos_control); + attitude_control.Write_Rate(pos_control); 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_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 14d3a1fc7f32d..d95651fbd03e8 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 b3968282580d5..104720f38a56f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -428,8 +428,8 @@ class AC_AttitudeControl { // 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 @@ class AC_AttitudeControl { 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 39c9ad277e982..4c67892fe0f17 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 947975a0a1686..91378c8bfa067 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 7f2791c241a36..e83abb39aeb87 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 9b5fe8df95bb3..8290ec3d09f20 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 } diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 64dafeab8e671..213ae21551507 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -443,7 +443,7 @@ void AC_AutoTune::control_attitude() #if HAL_LOGGING_ENABLED // log this iterations lean angle and rotation rate Log_AutoTuneDetails(); - ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); + attitude_control->Write_Rate(*pos_control); log_pids(); #endif diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index afeef1fa32d36..1530a19b3d367 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -139,50 +139,4 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -// Write a rate packet -void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control, - const AC_PosControl &pos_control) const -{ - const Vector3f &rate_targets = attitude_control.rate_bf_targets(); - const Vector3f &accel_target = pos_control.get_accel_target_cmss(); - const auto timeus = AP_HAL::micros64(); - const struct log_Rate pkt_rate{ - LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), - time_us : timeus, - control_roll : degrees(rate_targets.x), - roll : degrees(get_gyro().x), - roll_out : motors.get_roll()+motors.get_roll_ff(), - control_pitch : degrees(rate_targets.y), - pitch : degrees(get_gyro().y), - pitch_out : motors.get_pitch()+motors.get_pitch_ff(), - control_yaw : degrees(rate_targets.z), - yaw : degrees(get_gyro().z), - yaw_out : motors.get_yaw()+motors.get_yaw_ff(), - control_accel : (float)accel_target.z, - accel : (float)(-(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 = attitude_control.get_last_angle_P_scale(); - const Vector3f &pd_scale = attitude_control.get_PD_scale_logging(); - 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 : timeus, - 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/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index ff0f2293c1651..1090e112f89be 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -174,9 +174,6 @@ class AP_AHRS_View // Logging Functions void Write_AttitudeView(const Vector3f &targets) const; - void Write_Rate(const class AP_Motors &motors, - const class AC_AttitudeControl &attitude_control, - const AC_PosControl &pos_control) const; float roll; float pitch; diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 08da53e1c2ea7..af0c4c36d1fa4 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -105,40 +105,6 @@ struct PACKED log_POS { float rel_origin_alt; }; -// @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; -}; - // @LoggerMessage: VSTB // @Description: Log message for video stabilisation software such as Gyroflow // @Field: TimeUS: Time since system startup @@ -200,8 +166,6 @@ struct PACKED log_ATSC { "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \ { LOG_POS_MSG, sizeof(log_POS), \ "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" , true }, \ - { 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 }, \ { LOG_ATSC_MSG, sizeof(log_ATSC), \ "ATSC", "Qffffff", "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \ { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \