Line data Source code
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 : float desired_rangefinder_alt;
19 : float 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 52342 : void Sub::Log_Write_Control_Tuning()
27 : {
28 : // get terrain altitude
29 52342 : float terr_alt = 0.0f;
30 : #if AP_TERRAIN_AVAILABLE
31 52342 : if (terrain.enabled()) {
32 0 : terrain.height_above_terrain(terr_alt, true);
33 : } else {
34 52342 : 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 52342 : struct log_Control_Tuning pkt = {
41 : LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
42 52342 : time_us : AP_HAL::micros64(),
43 52342 : throttle_in : attitude_control.get_throttle_in(),
44 52342 : angle_boost : attitude_control.angle_boost(),
45 52342 : throttle_out : motors.get_throttle(),
46 52342 : throttle_hover : motors.get_throttle_hover(),
47 104684 : desired_alt : pos_control.get_pos_target_U_cm() * 0.01f,
48 52342 : inav_alt : pos_control.get_pos_estimate_U_m(),
49 52342 : baro_alt : barometer.get_altitude(),
50 104684 : desired_rangefinder_alt : mode_surftrak.get_rangefinder_target_cm() * 0.01,
51 52342 : rangefinder_alt : rangefinder_state.alt,
52 : terr_alt : terr_alt,
53 104684 : target_climb_rate : (int16_t)pos_control.get_vel_target_U_cms(),
54 104684 : climb_rate : int16_t(pos_control.get_vel_estimate_U_ms() * 100.0f)
55 52342 : };
56 52342 : logger.WriteBlock(&pkt, sizeof(pkt));
57 52342 : }
58 :
59 : // Write an attitude packet
60 52321 : void Sub::Log_Write_Attitude()
61 : {
62 52321 : ahrs.Write_Attitude(attitude_control.get_att_target_euler_rad() * RAD_TO_DEG);
63 :
64 52321 : AP::ahrs().Log_Write();
65 52321 : }
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 0 : void Sub::Log_Write_Data(LogDataID id, int16_t value)
77 : {
78 0 : if (should_log(MASK_LOG_ANY)) {
79 0 : struct log_Data_Int16t pkt = {
80 : LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG),
81 0 : time_us : AP_HAL::micros64(),
82 : id : (uint8_t)id,
83 : data_value : value
84 0 : };
85 0 : logger.WriteCriticalBlock(&pkt, sizeof(pkt));
86 : }
87 0 : }
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 0 : void Sub::Log_Write_Data(LogDataID id, uint16_t value)
99 : {
100 0 : if (should_log(MASK_LOG_ANY)) {
101 0 : struct log_Data_UInt16t pkt = {
102 : LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG),
103 0 : time_us : AP_HAL::micros64(),
104 : id : (uint8_t)id,
105 : data_value : value
106 0 : };
107 0 : logger.WriteCriticalBlock(&pkt, sizeof(pkt));
108 : }
109 0 : }
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 0 : void Sub::Log_Write_Data(LogDataID id, int32_t value)
120 : {
121 0 : if (should_log(MASK_LOG_ANY)) {
122 0 : struct log_Data_Int32t pkt = {
123 : LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG),
124 0 : time_us : AP_HAL::micros64(),
125 : id : (uint8_t)id,
126 : data_value : value
127 0 : };
128 0 : logger.WriteCriticalBlock(&pkt, sizeof(pkt));
129 : }
130 0 : }
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 5198 : void Sub::Log_Write_Data(LogDataID id, uint32_t value)
141 : {
142 5198 : if (should_log(MASK_LOG_ANY)) {
143 5198 : struct log_Data_UInt32t pkt = {
144 : LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG),
145 5198 : time_us : AP_HAL::micros64(),
146 : id : (uint8_t)id,
147 : data_value : value
148 5198 : };
149 5198 : logger.WriteCriticalBlock(&pkt, sizeof(pkt));
150 : }
151 5198 : }
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 0 : void Sub::Log_Write_Data(LogDataID id, float value)
163 : {
164 0 : if (should_log(MASK_LOG_ANY)) {
165 0 : struct log_Data_Float pkt = {
166 : LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG),
167 0 : time_us : AP_HAL::micros64(),
168 : id : (uint8_t)id,
169 : data_value : value
170 0 : };
171 0 : logger.WriteCriticalBlock(&pkt, sizeof(pkt));
172 : }
173 0 : }
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 : float acc_target_x;
186 : float acc_target_y;
187 : float acc_target_z;
188 : };
189 :
190 : // Write a Guided mode target
191 1233 : void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target_neu_cm, const Vector3f& vel_target_neu_cms, const Vector3f& acc_target_neu_cmss)
192 : {
193 1233 : struct log_GuidedTarget pkt = {
194 : LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
195 1233 : time_us : AP_HAL::micros64(),
196 : type : target_type,
197 1233 : pos_target_x : pos_target_neu_cm.x * 0.01f,
198 1233 : pos_target_y : pos_target_neu_cm.y * 0.01f,
199 1233 : pos_target_z : pos_target_neu_cm.z * 0.01f,
200 1233 : vel_target_x : vel_target_neu_cms.x * 0.01f,
201 1233 : vel_target_y : vel_target_neu_cms.y * 0.01f,
202 1233 : vel_target_z : vel_target_neu_cms.z * 0.01f,
203 1233 : acc_target_x : acc_target_neu_cmss.x * 0.01f,
204 1233 : acc_target_y : acc_target_neu_cmss.y * 0.01f,
205 1233 : acc_target_z : acc_target_neu_cmss.z * 0.01f
206 1233 : };
207 1233 : logger.WriteBlock(&pkt, sizeof(pkt));
208 1233 : }
209 :
210 : // @LoggerMessage: CTUN
211 : // @Description: Control Tuning information
212 : // @Field: TimeUS: Time since system startup
213 : // @Field: ThI: throttle input
214 : // @Field: ABst: angle boost
215 : // @Field: ThO: throttle output
216 : // @Field: ThH: calculated hover throttle
217 : // @Field: DAlt: desired altitude
218 : // @Field: Alt: achieved altitude
219 : // @Field: BAlt: barometric altitude
220 : // @Field: DSAlt: desired rangefinder altitude
221 : // @Field: SAlt: achieved rangefinder altitude
222 : // @Field: TAlt: terrain altitude
223 : // @Field: DCRt: desired climb rate
224 : // @Field: CRt: climb rate
225 :
226 : // @LoggerMessage: D16
227 : // @Description: Generic 16-bit-signed-integer storage
228 : // @Field: TimeUS: Time since system startup
229 : // @Field: Id: Data type identifier
230 : // @Field: Value: Value
231 :
232 : // @LoggerMessage: D32
233 : // @Description: Generic 32-bit-signed-integer storage
234 : // @Field: TimeUS: Time since system startup
235 : // @Field: Id: Data type identifier
236 : // @Field: Value: Value
237 :
238 : // @LoggerMessage: DFLT
239 : // @Description: Generic float storage
240 : // @Field: TimeUS: Time since system startup
241 : // @Field: Id: Data type identifier
242 : // @Field: Value: Value
243 :
244 : // @LoggerMessage: DU16
245 : // @Description: Generic 16-bit-unsigned-integer storage
246 : // @Field: TimeUS: Time since system startup
247 : // @Field: Id: Data type identifier
248 : // @Field: Value: Value
249 :
250 : // @LoggerMessage: DU32
251 : // @Description: Generic 32-bit-unsigned-integer storage
252 : // @Field: TimeUS: Time since system startup
253 : // @Field: Id: Data type identifier
254 : // @Field: Value: Value
255 :
256 : // @LoggerMessage: GUIP
257 : // @Description: Guided mode target information
258 : // @Field: TimeUS: Time since system startup
259 : // @Field: Type: Type of guided mode
260 : // @Field: pX: Target position, X-Axis
261 : // @Field: pY: Target position, Y-Axis
262 : // @Field: pZ: Target position, Z-Axis
263 : // @Field: vX: Target velocity, X-Axis
264 : // @Field: vY: Target velocity, Y-Axis
265 : // @Field: vZ: Target velocity, Z-Axis
266 : // @Field: aX: Target acceleration, X-Axis
267 : // @Field: aY: Target acceleration, Y-Axis
268 : // @Field: aZ: Target acceleration, Z-Axis
269 :
270 : // type and unit information can be found in
271 : // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
272 : // units and "Format characters" for field type information
273 : const struct LogStructure Sub::log_structure[] = {
274 : LOG_COMMON_STRUCTURES,
275 : { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
276 : "CTUN", "Qffffffffffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" },
277 : { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),
278 : "D16", "QBh", "TimeUS,Id,Value", "s--", "F--" },
279 : { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),
280 : "DU16", "QBH", "TimeUS,Id,Value", "s--", "F--" },
281 : { LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),
282 : "D32", "QBi", "TimeUS,Id,Value", "s--", "F--" },
283 : { LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),
284 : "DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" },
285 : { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
286 : "DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" },
287 : { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
288 : "GUIP", "QBfffffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ,aX,aY,aZ", "s-mmmnnnooo", "F-000000000" },
289 : };
290 :
291 83 : uint8_t Sub::get_num_log_structures() const
292 : {
293 83 : return ARRAY_SIZE(log_structure);
294 : }
295 :
296 225 : void Sub::Log_Write_Vehicle_Startup_Messages()
297 : {
298 : // only 200(?) bytes are guaranteed by AP_Logger
299 225 : logger.Write_Mode((uint8_t)control_mode, control_mode_reason);
300 225 : ahrs.Log_Write_Home_And_Origin();
301 225 : gps.Write_AP_Logger_Log_Startup_messages();
302 225 : }
303 :
304 :
305 : #endif // HAL_LOGGING_ENABLED
|
