Line data Source code
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 : // Internal log writer for PSCx (North, East, Down tracking).
11 : // Reduces duplication between Write_PSCN, PSCE, and PSCD.
12 : // Used for logging desired/target/actual position, velocity, and acceleration per axis.
13 2066106 : void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
14 : {
15 2066106 : const struct log_PSCx pkt{
16 : LOG_PACKET_HEADER_INIT(id),
17 2066106 : time_us : AP_HAL::micros64(),
18 : pos_desired : pos_desired_m,
19 : pos_target : pos_target_m,
20 : pos : pos_m,
21 : vel_desired : vel_desired_ms,
22 : vel_target : vel_target_ms,
23 : vel : vel_ms,
24 : accel_desired : accel_desired_mss,
25 : accel_target : accel_target_mss,
26 : accel : accel_mss
27 2066106 : };
28 2066106 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
29 2066106 : }
30 :
31 : // Logs position controller state along the North axis to PSCN..
32 : // Logs desired, target, and actual position [m], velocity [m/s], and acceleration [m/s²].
33 651419 : void AC_PosControl::Write_PSCN(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
34 : {
35 651419 : Write_PSCx(LOG_PSCN_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss);
36 651419 : }
37 :
38 : // Logs position controller state along the East axis to PSCE.
39 : // Logs desired, target, and actual values for position [m], velocity [m/s], and acceleration [m/s²].
40 651419 : void AC_PosControl::Write_PSCE(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
41 : {
42 651419 : Write_PSCx(LOG_PSCE_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss);
43 651419 : }
44 :
45 : // Logs position controller state along the Down (vertical) axis to PSCD.
46 : // Logs desired, target, and actual values for position [m], velocity [m/s], and acceleration [m/s²].
47 763268 : void AC_PosControl::Write_PSCD(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss)
48 : {
49 763268 : Write_PSCx(LOG_PSCD_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss);
50 763268 : }
51 :
52 : // Internal log writer for PSOx (North, East, Down tracking).
53 : // Reduces duplication between Write_PSON, PSOE, and PSOD.
54 : // Used for logging desired/target/actual position, velocity, and acceleration per axis.
55 18978 : void AC_PosControl::Write_PSOx(LogMessages id, float pos_target_offset_m, float pos_offset_m,
56 : float vel_target_offset_ms, float vel_offset_ms,
57 : float accel_target_offset_mss, float accel_offset_mss)
58 : {
59 18978 : const struct log_PSOx pkt{
60 : LOG_PACKET_HEADER_INIT(id),
61 18978 : time_us : AP_HAL::micros64(),
62 : pos_target_offset : pos_target_offset_m,
63 : pos_offset : pos_offset_m,
64 : vel_target_offset : vel_target_offset_ms,
65 : vel_offset : vel_offset_ms,
66 : accel_target_offset : accel_target_offset_mss,
67 : accel_offset : accel_offset_mss,
68 18978 : };
69 18978 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
70 18978 : }
71 :
72 : // Logs offset tracking along the North axis to PSON.
73 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
74 295 : void AC_PosControl::Write_PSON(float pos_target_offset_m, float pos_offset_m,
75 : float vel_target_offset_ms, float vel_offset_ms,
76 : float accel_target_offset_mss, float accel_offset_mss)
77 : {
78 295 : Write_PSOx(LOG_PSON_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
79 295 : }
80 :
81 : // Logs offset tracking along the East axis to PSOE.
82 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
83 295 : void AC_PosControl::Write_PSOE(float pos_target_offset_m, float pos_offset_m,
84 : float vel_target_offset_ms, float vel_offset_ms,
85 : float accel_target_offset_mss, float accel_offset_mss)
86 : {
87 295 : Write_PSOx(LOG_PSOE_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
88 295 : }
89 :
90 : // Logs offset tracking along the Down axis to PSOD.
91 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
92 713 : void AC_PosControl::Write_PSOD(float pos_target_offset_m, float pos_offset_m,
93 : float vel_target_offset_ms, float vel_offset_ms,
94 : float accel_target_offset_mss, float accel_offset_mss)
95 : {
96 713 : Write_PSOx(LOG_PSOD_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
97 713 : }
98 :
99 : // Logs terrain-following offset tracking along the Down axis to PSOT.
100 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
101 17675 : void AC_PosControl::Write_PSOT(float pos_target_offset_m, float pos_offset_m,
102 : float vel_target_offset_ms, float vel_offset_ms,
103 : float accel_target_offset_mss, float accel_offset_mss)
104 : {
105 17675 : Write_PSOx(LOG_PSOT_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss);
106 17675 : }
107 :
108 : #endif // HAL_LOGGING_ENABLED
|
