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/ArduSub/Log.cpp
Views: 1798
1
#include "Sub.h"
2
3
#if HAL_LOGGING_ENABLED
4
5
// Code to Write and Read packets from AP_Logger log memory
6
// Code to interact with the user to dump or erase logs
7
8
struct PACKED log_Control_Tuning {
9
LOG_PACKET_HEADER;
10
uint64_t time_us;
11
float throttle_in;
12
float angle_boost;
13
float throttle_out;
14
float throttle_hover;
15
float desired_alt;
16
float inav_alt;
17
float baro_alt;
18
int16_t desired_rangefinder_alt;
19
int16_t rangefinder_alt;
20
float terr_alt;
21
int16_t target_climb_rate;
22
int16_t climb_rate;
23
};
24
25
// Write a control tuning packet
26
void Sub::Log_Write_Control_Tuning()
27
{
28
// get terrain altitude
29
float terr_alt = 0.0f;
30
#if AP_TERRAIN_AVAILABLE
31
if (terrain.enabled()) {
32
terrain.height_above_terrain(terr_alt, true);
33
} else {
34
terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f;
35
}
36
#else
37
terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f;
38
#endif
39
40
struct log_Control_Tuning pkt = {
41
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
42
time_us : AP_HAL::micros64(),
43
throttle_in : attitude_control.get_throttle_in(),
44
angle_boost : attitude_control.angle_boost(),
45
throttle_out : motors.get_throttle(),
46
throttle_hover : motors.get_throttle_hover(),
47
desired_alt : pos_control.get_pos_target_z_cm() * 0.01f,
48
inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f,
49
baro_alt : barometer.get_altitude(),
50
desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(),
51
rangefinder_alt : rangefinder_state.alt_cm,
52
terr_alt : terr_alt,
53
target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(),
54
climb_rate : climb_rate
55
};
56
logger.WriteBlock(&pkt, sizeof(pkt));
57
}
58
59
// Write an attitude packet
60
void Sub::Log_Write_Attitude()
61
{
62
ahrs.Write_Attitude(attitude_control.get_att_target_euler_rad() * RAD_TO_DEG);
63
64
AP::ahrs().Log_Write();
65
}
66
67
struct PACKED log_Data_Int16t {
68
LOG_PACKET_HEADER;
69
uint64_t time_us;
70
uint8_t id;
71
int16_t data_value;
72
};
73
74
// Write an int16_t data packet
75
UNUSED_FUNCTION
76
void Sub::Log_Write_Data(LogDataID id, int16_t value)
77
{
78
if (should_log(MASK_LOG_ANY)) {
79
struct log_Data_Int16t pkt = {
80
LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG),
81
time_us : AP_HAL::micros64(),
82
id : (uint8_t)id,
83
data_value : value
84
};
85
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
86
}
87
}
88
89
struct PACKED log_Data_UInt16t {
90
LOG_PACKET_HEADER;
91
uint64_t time_us;
92
uint8_t id;
93
uint16_t data_value;
94
};
95
96
// Write an uint16_t data packet
97
UNUSED_FUNCTION
98
void Sub::Log_Write_Data(LogDataID id, uint16_t value)
99
{
100
if (should_log(MASK_LOG_ANY)) {
101
struct log_Data_UInt16t pkt = {
102
LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG),
103
time_us : AP_HAL::micros64(),
104
id : (uint8_t)id,
105
data_value : value
106
};
107
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
108
}
109
}
110
111
struct PACKED log_Data_Int32t {
112
LOG_PACKET_HEADER;
113
uint64_t time_us;
114
uint8_t id;
115
int32_t data_value;
116
};
117
118
// Write an int32_t data packet
119
void Sub::Log_Write_Data(LogDataID id, int32_t value)
120
{
121
if (should_log(MASK_LOG_ANY)) {
122
struct log_Data_Int32t pkt = {
123
LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG),
124
time_us : AP_HAL::micros64(),
125
id : (uint8_t)id,
126
data_value : value
127
};
128
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
129
}
130
}
131
132
struct PACKED log_Data_UInt32t {
133
LOG_PACKET_HEADER;
134
uint64_t time_us;
135
uint8_t id;
136
uint32_t data_value;
137
};
138
139
// Write a uint32_t data packet
140
void Sub::Log_Write_Data(LogDataID id, uint32_t value)
141
{
142
if (should_log(MASK_LOG_ANY)) {
143
struct log_Data_UInt32t pkt = {
144
LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG),
145
time_us : AP_HAL::micros64(),
146
id : (uint8_t)id,
147
data_value : value
148
};
149
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
150
}
151
}
152
153
struct PACKED log_Data_Float {
154
LOG_PACKET_HEADER;
155
uint64_t time_us;
156
uint8_t id;
157
float data_value;
158
};
159
160
// Write a float data packet
161
UNUSED_FUNCTION
162
void Sub::Log_Write_Data(LogDataID id, float value)
163
{
164
if (should_log(MASK_LOG_ANY)) {
165
struct log_Data_Float pkt = {
166
LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG),
167
time_us : AP_HAL::micros64(),
168
id : (uint8_t)id,
169
data_value : value
170
};
171
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
172
}
173
}
174
175
struct PACKED log_GuidedTarget {
176
LOG_PACKET_HEADER;
177
uint64_t time_us;
178
uint8_t type;
179
float pos_target_x;
180
float pos_target_y;
181
float pos_target_z;
182
float vel_target_x;
183
float vel_target_y;
184
float vel_target_z;
185
};
186
187
// Write a Guided mode target
188
void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
189
{
190
struct log_GuidedTarget pkt = {
191
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
192
time_us : AP_HAL::micros64(),
193
type : target_type,
194
pos_target_x : pos_target.x,
195
pos_target_y : pos_target.y,
196
pos_target_z : pos_target.z,
197
vel_target_x : vel_target.x,
198
vel_target_y : vel_target.y,
199
vel_target_z : vel_target.z
200
};
201
logger.WriteBlock(&pkt, sizeof(pkt));
202
}
203
204
// @LoggerMessage: CTUN
205
// @Description: Control Tuning information
206
// @Field: TimeUS: Time since system startup
207
// @Field: ThI: throttle input
208
// @Field: ABst: angle boost
209
// @Field: ThO: throttle output
210
// @Field: ThH: calculated hover throttle
211
// @Field: DAlt: desired altitude
212
// @Field: Alt: achieved altitude
213
// @Field: BAlt: barometric altitude
214
// @Field: DSAlt: desired rangefinder altitude
215
// @Field: SAlt: achieved rangefinder altitude
216
// @Field: TAlt: terrain altitude
217
// @Field: DCRt: desired climb rate
218
// @Field: CRt: climb rate
219
220
// @LoggerMessage: D16
221
// @Description: Generic 16-bit-signed-integer storage
222
// @Field: TimeUS: Time since system startup
223
// @Field: Id: Data type identifier
224
// @Field: Value: Value
225
226
// @LoggerMessage: D32
227
// @Description: Generic 32-bit-signed-integer storage
228
// @Field: TimeUS: Time since system startup
229
// @Field: Id: Data type identifier
230
// @Field: Value: Value
231
232
// @LoggerMessage: DFLT
233
// @Description: Generic float storage
234
// @Field: TimeUS: Time since system startup
235
// @Field: Id: Data type identifier
236
// @Field: Value: Value
237
238
// @LoggerMessage: DU16
239
// @Description: Generic 16-bit-unsigned-integer storage
240
// @Field: TimeUS: Time since system startup
241
// @Field: Id: Data type identifier
242
// @Field: Value: Value
243
244
// @LoggerMessage: DU32
245
// @Description: Generic 32-bit-unsigned-integer storage
246
// @Field: TimeUS: Time since system startup
247
// @Field: Id: Data type identifier
248
// @Field: Value: Value
249
250
// @LoggerMessage: GUIP
251
// @Description: Guided mode target information
252
// @Field: TimeUS: Time since system startup
253
// @Field: Type: Type of guided mode
254
// @Field: pX: Target position, X-Axis
255
// @Field: pY: Target position, Y-Axis
256
// @Field: pZ: Target position, Z-Axis
257
// @Field: vX: Target velocity, X-Axis
258
// @Field: vY: Target velocity, Y-Axis
259
// @Field: vZ: Target velocity, Z-Axis
260
261
// type and unit information can be found in
262
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
263
// units and "Format characters" for field type information
264
const struct LogStructure Sub::log_structure[] = {
265
LOG_COMMON_STRUCTURES,
266
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
267
"CTUN", "Qfffffffccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" },
268
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),
269
"D16", "QBh", "TimeUS,Id,Value", "s--", "F--" },
270
{ LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),
271
"DU16", "QBH", "TimeUS,Id,Value", "s--", "F--" },
272
{ LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),
273
"D32", "QBi", "TimeUS,Id,Value", "s--", "F--" },
274
{ LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),
275
"DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" },
276
{ LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
277
"DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" },
278
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
279
"GUIP", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
280
};
281
282
uint8_t Sub::get_num_log_structures() const
283
{
284
return ARRAY_SIZE(log_structure);
285
}
286
287
void Sub::Log_Write_Vehicle_Startup_Messages()
288
{
289
// only 200(?) bytes are guaranteed by AP_Logger
290
logger.Write_Mode((uint8_t)control_mode, control_mode_reason);
291
ahrs.Log_Write_Home_And_Origin();
292
gps.Write_AP_Logger_Log_Startup_messages();
293
}
294
295
296
#endif // HAL_LOGGING_ENABLED
297
298