CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp
Views: 1798
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
void AC_AttitudeControl::Write_ANG() const

13
{

14
    Vector3f targets = get_att_target_euler_rad() * RAD_TO_DEG;

15


16
    const struct log_ANG pkt{

17
        LOG_PACKET_HEADER_INIT(LOG_ANG_MSG),

18
        time_us         : AP::scheduler().get_loop_start_time_us(),

19
        control_roll    : targets.x,

20
        roll            : degrees(_ahrs.roll),

21
        control_pitch   : targets.y,

22
        pitch           : degrees(_ahrs.pitch),

23
        control_yaw     : wrap_360(targets.z),

24
        yaw             : wrap_360(degrees(_ahrs.yaw)),

25
        sensor_dt       : AP::scheduler().get_last_loop_time_s()

26
    };

27
    AP::logger().WriteBlock(&pkt, sizeof(pkt));

28
}

29


30
// Write a rate packet

31
void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const

32
{

33
    const Vector3f rate_targets = rate_bf_targets() * RAD_TO_DEG;

34
    const Vector3f &accel_target = pos_control.get_accel_target_cmss();

35
    const Vector3f gyro_rate = _rate_gyro * RAD_TO_DEG;

36
    const struct log_Rate pkt_rate{

37
        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),

38
        time_us         : _rate_gyro_time_us,

39
        control_roll    : rate_targets.x,

40
        roll            : gyro_rate.x,

41
        roll_out        : _motors.get_roll()+_motors.get_roll_ff(),

42
        control_pitch   : rate_targets.y,

43
        pitch           : gyro_rate.y,

44
        pitch_out       : _motors.get_pitch()+_motors.get_pitch_ff(),

45
        control_yaw     : rate_targets.z,

46
        yaw             : gyro_rate.z,

47
        yaw_out         : _motors.get_yaw()+_motors.get_yaw_ff(),

48
        control_accel   : (float)accel_target.z,

49
        accel           : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f),

50
        accel_out       : _motors.get_throttle(),

51
        throttle_slew   : _motors.get_throttle_slew_rate()

52
    };

53
    AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));

54


55
    /*

56
      log P/PD gain scale if not == 1.0

57
     */

58
    const Vector3f &scale = get_last_angle_P_scale();

59
    const Vector3f &pd_scale = _pd_scale_used;

60
    if (scale != AC_AttitudeControl::VECTORF_111 || pd_scale != AC_AttitudeControl::VECTORF_111) {

61
        const struct log_ATSC pkt_ATSC {

62
            LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG),

63
            time_us  : _rate_gyro_time_us,

64
            scaleP_x : scale.x,

65
            scaleP_y : scale.y,

66
            scaleP_z : scale.z,

67
            scalePD_x : pd_scale.x,

68
            scalePD_y : pd_scale.y,

69
            scalePD_z : pd_scale.z,

70
        };

71
        AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));

72
    }

73
}

74


75
#endif // HAL_LOGGING_ENABLED

76


77