Line data    Source code

       1             : #include <AP_Logger/AP_Logger_config.h>
       2             : 
       3             : #if HAL_LOGGING_ENABLED
       4             : 
       5             : #include "AC_AttitudeControl.h"
       6             : #include "AC_PosControl.h"
       7             : #include <AP_Logger/AP_Logger.h>
       8             : #include <AP_Scheduler/AP_Scheduler.h>
       9             : #include "LogStructure.h"
      10             : 
      11             : // Write an ANG packet
      12     3480899 : void AC_AttitudeControl::Write_ANG() const
      13             : {
      14     3480899 :     Vector3f targets = get_att_target_euler_rad() * RAD_TO_DEG;
      15             : 
      16     3480899 :     const struct log_ANG pkt{
      17             :         LOG_PACKET_HEADER_INIT(LOG_ANG_MSG),
      18     3480899 :         time_us         : AP::scheduler().get_loop_start_time_us(),
      19     3480899 :         control_roll    : targets.x,
      20     3480899 :         roll            : degrees(_ahrs.roll),
      21     3480899 :         control_pitch   : targets.y,
      22     3480899 :         pitch           : degrees(_ahrs.pitch),
      23     3480899 :         control_yaw     : wrap_360(targets.z),
      24     3480899 :         yaw             : wrap_360(degrees(_ahrs.yaw)),
      25     3480899 :         sensor_dt       : AP::scheduler().get_last_loop_time_s()
      26     6961798 :     };
      27     3480899 :     AP::logger().WriteBlock(&pkt, sizeof(pkt));
      28     3480899 : }
      29             : 
      30             : // Write a rate packet
      31     4724142 : void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const
      32             : {
      33     4724142 :     const Vector3f rate_targets_degs = rate_bf_targets() * RAD_TO_DEG;
      34     4724142 :     const Vector3f &accel_target_ned_mss = pos_control.get_accel_target_NED_mss();
      35     4724142 :     const Vector3f gyro_rate = _rate_gyro_rads * RAD_TO_DEG;
      36     4724142 :     const struct log_Rate pkt_rate{
      37             :         LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
      38     4724142 :         time_us         : _rate_gyro_time_us,
      39     4724142 :         control_roll    : rate_targets_degs.x,
      40     4724142 :         roll            : gyro_rate.x,
      41     4724142 :         roll_out        : _motors.get_roll()+_motors.get_roll_ff(),
      42     4724142 :         control_pitch   : rate_targets_degs.y,
      43     4724142 :         pitch           : gyro_rate.y,
      44     4724142 :         pitch_out       : _motors.get_pitch()+_motors.get_pitch_ff(),
      45     4724142 :         control_yaw     : rate_targets_degs.z,
      46     4724142 :         yaw             : gyro_rate.z,
      47     4724142 :         yaw_out         : _motors.get_yaw()+_motors.get_yaw_ff(),
      48     4724142 :         control_accel   : (float)(-accel_target_ned_mss.z),
      49     9448284 :         accel           : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS)),
      50     4724142 :         accel_out       : _motors.get_throttle(),
      51     4724142 :         throttle_slew   : _motors.get_throttle_slew_rate()
      52    18896568 :     };
      53     4724142 :     AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
      54             : 
      55             :     /*
      56             :       log P/PD gain scale if not == 1.0
      57             :      */
      58     4724142 :     const Vector3f &scale = get_last_angle_P_scale();
      59     4724142 :     const Vector3f &pd_scale = _pd_scale_used;
      60     4724142 :     const Vector3f &i_scale = _i_scale_used;
      61     4724142 :     if (scale != AC_AttitudeControl::VECTORF_111 
      62     4469731 :         || pd_scale != AC_AttitudeControl::VECTORF_111
      63     9193873 :         || i_scale != AC_AttitudeControl::VECTORF_111) {
      64      254411 :         const struct log_ATSC pkt_ATSC {
      65             :             LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG),
      66      254411 :             time_us  : _rate_gyro_time_us,
      67      254411 :             scaleP_x : scale.x,
      68      254411 :             scaleP_y : scale.y,
      69      254411 :             scaleP_z : scale.z,
      70      254411 :             scalePD_x : pd_scale.x,
      71      254411 :             scalePD_y : pd_scale.y,
      72      254411 :             scalePD_z : pd_scale.z,
      73      254411 :             scaleI_x : i_scale.x,
      74      254411 :             scaleI_y : i_scale.y,
      75      254411 :             scaleI_z : i_scale.z,
      76      254411 :         };
      77      254411 :         AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));
      78             :     }
      79     4724142 : }
      80             : 
      81             : #endif // HAL_LOGGING_ENABLED