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_PosControl_Logging.cpp
Views: 1798
1
#include <AP_Logger/AP_Logger_config.h>

2


3
#if HAL_LOGGING_ENABLED

4


5
#include "AC_PosControl.h"

6


7
#include <AP_Logger/AP_Logger.h>

8
#include "LogStructure.h"

9


10
// a convenience function for writing out the position controller PIDs

11
void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)

12
{

13
    const struct log_PSCx pkt{

14
        LOG_PACKET_HEADER_INIT(id),

15
            time_us         : AP_HAL::micros64(),

16
            pos_desired   : pos_desired * 0.01f,

17
            pos_target    : pos_target * 0.01f,

18
            pos           : pos * 0.01f,

19
            vel_desired   : vel_desired * 0.01f,

20
            vel_target    : vel_target * 0.01f,

21
            vel           : vel * 0.01f,

22
            accel_desired : accel_desired * 0.01f,

23
            accel_target  : accel_target * 0.01f,

24
            accel         : accel * 0.01f

25
    };

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

27
}

28


29
void AC_PosControl::Write_PSCN(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)

30
{

31
    Write_PSCx(LOG_PSCN_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);

32
}

33


34
void AC_PosControl::Write_PSCE(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)

35
{

36
    Write_PSCx(LOG_PSCE_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);

37
}

38


39
void AC_PosControl::Write_PSCD(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)

40
{

41
    Write_PSCx(LOG_PSCD_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);

42
}

43


44
// a convenience function for writing out the position controller offsets

45
void AC_PosControl::Write_PSOx(LogMessages id, float pos_target_offset_cm, float pos_offset_cm,

46
                               float vel_target_offset_cms, float vel_offset_cms,

47
                               float accel_target_offset_cmss, float accel_offset_cmss)

48
{

49
    const struct log_PSOx pkt{

50
        LOG_PACKET_HEADER_INIT(id),

51
            time_us             : AP_HAL::micros64(),

52
            pos_target_offset   : pos_target_offset_cm * 0.01f,

53
            pos_offset          : pos_offset_cm * 0.01f,

54
            vel_target_offset   : vel_target_offset_cms * 0.01f,

55
            vel_offset          : vel_offset_cms * 0.01f,

56
            accel_target_offset : accel_target_offset_cmss * 0.01f,

57
            accel_offset        : accel_offset_cmss * 0.01f,

58
    };

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

60
}

61


62
void AC_PosControl::Write_PSON(float pos_target_offset_cm, float pos_offset_cm,

63
                               float vel_target_offset_cms, float vel_offset_cms,

64
                               float accel_target_offset_cmss, float accel_offset_cmss)

65
{

66
    Write_PSOx(LOG_PSON_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);

67
}

68


69
void AC_PosControl::Write_PSOE(float pos_target_offset_cm, float pos_offset_cm,

70
                               float vel_target_offset_cms, float vel_offset_cms,

71
                               float accel_target_offset_cmss, float accel_offset_cmss)

72
{

73
    Write_PSOx(LOG_PSOE_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);

74
}

75


76
void AC_PosControl::Write_PSOD(float pos_target_offset_cm, float pos_offset_cm,

77
                               float vel_target_offset_cms, float vel_offset_cms,

78
                               float accel_target_offset_cmss, float accel_offset_cmss)

79
{

80
    Write_PSOx(LOG_PSOD_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);

81
}

82


83
void AC_PosControl::Write_PSOT(float pos_target_offset_cm, float pos_offset_cm,

84
                               float vel_target_offset_cms, float vel_offset_cms,

85
                               float accel_target_offset_cmss, float accel_offset_cmss)

86
{

87
    Write_PSOx(LOG_PSOT_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss);

88
}

89


90
#endif  // HAL_LOGGING_ENABLED

91


92