Line data Source code
1 : #include "Plane.h"
2 :
3 : #if HAL_LOGGING_ENABLED
4 :
5 : struct PACKED log_ATIS {
6 : LOG_PACKET_HEADER;
7 : uint64_t time_us;
8 : float roll_angle;
9 : float roll_angle_error;
10 : float roll_velocity;
11 : float roll_acceleration;
12 : float pitch_angle;
13 : float pitch_angle_error;
14 : float pitch_velocity;
15 : float pitch_acceleration;
16 : };
17 :
18 : // Write an attitude packet
19 1016541 : void Plane::Log_Write_Attitude(void)
20 : {
21 : Vector3f targets { // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
22 1016541 : nav_roll_cd * 0.01f,
23 1016541 : nav_pitch_cd * 0.01f,
24 : 0 //Plane does not have the concept of navyaw. This is a placeholder.
25 1016541 : };
26 :
27 : #if HAL_QUADPLANE_ENABLED
28 1016541 : if (quadplane.show_vtol_view()) {
29 : // we need the attitude targets from the AC_AttitudeControl controller, as they
30 : // account for the acceleration limits.
31 : // Also, for bodyframe roll input types, _attitude_target_euler_angle is not maintained
32 : // since Euler angles are not used and it is a waste of cpu to compute them at the loop rate.
33 : // Get them from the quaternion instead:
34 90173 : quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z);
35 90173 : quadplane.ahrs_view->Write_AttitudeView(targets * RAD_TO_DEG);
36 : } else
37 : #endif
38 : {
39 926368 : ahrs.Write_Attitude(targets);
40 : }
41 :
42 : #if HAL_QUADPLANE_ENABLED
43 1016541 : if (AP_HAL::millis() - quadplane.last_att_control_ms < 100) {
44 : // log quadplane PIDs separately from fixed wing PIDs
45 91778 : logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
46 91778 : logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
47 91778 : logger.Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
48 91778 : logger.Write_PID(LOG_PIQA_MSG, quadplane.pos_control->D_get_accel_pid().get_pid_info() );
49 :
50 : // Write tailsitter specific log at same rate as PIDs
51 91778 : quadplane.tailsitter.write_log();
52 : }
53 1016541 : if (quadplane.in_vtol_mode() && quadplane.pos_control->NE_is_active()) {
54 72245 : logger.Write_PID(LOG_PIDN_MSG, quadplane.pos_control->NE_get_vel_pid().get_pid_info_x());
55 72245 : logger.Write_PID(LOG_PIDE_MSG, quadplane.pos_control->NE_get_vel_pid().get_pid_info_y());
56 : }
57 : #endif
58 :
59 1016541 : logger.Write_PID(LOG_PIDR_MSG, rollController.get_pid_info());
60 1016541 : logger.Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info());
61 :
62 1016541 : if (yawController.enabled()) {
63 8420 : logger.Write_PID(LOG_PIDY_MSG, yawController.get_pid_info());
64 : }
65 :
66 1016541 : if (steerController.active()) {
67 60722 : logger.Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());
68 : }
69 :
70 : // Log roll and pitch input shaping
71 : float roll_angle, roll_velocity, roll_acceleration;
72 1016541 : rollController.get_input_shaping(roll_angle, roll_velocity, roll_acceleration);
73 :
74 : float pitch_angle, pitch_velocity, pitch_acceleration;
75 1016541 : pitchController.get_input_shaping(pitch_angle, pitch_velocity, pitch_acceleration);
76 :
77 1016541 : struct log_ATIS pkt = {
78 : LOG_PACKET_HEADER_INIT(LOG_ATIS_MSG),
79 1016541 : time_us: AP_HAL::micros64(),
80 : roll_angle: roll_angle,
81 1016541 : roll_angle_error: rollController.get_angle_error_deg(),
82 : roll_velocity: roll_velocity,
83 : roll_acceleration: roll_acceleration,
84 : pitch_angle: pitch_angle,
85 1016541 : pitch_angle_error: pitchController.get_angle_error_deg(),
86 : pitch_velocity: pitch_velocity,
87 : pitch_acceleration: pitch_acceleration
88 1016541 : };
89 1016541 : logger.WriteBlock(&pkt, sizeof(pkt));
90 :
91 1016541 : }
92 :
93 : // do fast logging for plane
94 8376953 : void Plane::Log_Write_FullRate(void)
95 : {
96 : // MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
97 : // highest rate selected wins
98 8376953 : if (should_log(MASK_LOG_ATTITUDE_FULLRATE)) {
99 0 : Log_Write_Attitude();
100 : }
101 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
102 8376953 : if (should_log(MASK_LOG_NOTCH_FULLRATE)) {
103 0 : AP::ins().write_notch_log_messages();
104 : }
105 : #endif
106 8376953 : }
107 :
108 :
109 : struct PACKED log_Control_Tuning {
110 : LOG_PACKET_HEADER;
111 : uint64_t time_us;
112 : int16_t nav_roll_cd;
113 : int16_t roll;
114 : int16_t nav_pitch_cd;
115 : int16_t pitch;
116 : float throttle_out;
117 : float rudder_out;
118 : float throttle_dem;
119 : float airspeed_estimate;
120 : uint8_t airspeed_estimate_status;
121 : float EAS2TAS;
122 : int32_t groundspeed_undershoot;
123 : };
124 :
125 : // Write a control tuning packet. Total length : 22 bytes
126 1310707 : void Plane::Log_Write_Control_Tuning()
127 : {
128 1310707 : float est_airspeed = 0;
129 1310707 : AP_AHRS::AirspeedEstimateType airspeed_estimate_type = AP_AHRS::AirspeedEstimateType::NO_NEW_ESTIMATE;
130 1310707 : ahrs.airspeed_EAS(est_airspeed, airspeed_estimate_type);
131 :
132 1310707 : int16_t pitch = ahrs.pitch_sensor - g.pitch_trim * 100;
133 : #if HAL_QUADPLANE_ENABLED
134 1310707 : if (quadplane.show_vtol_view()) {
135 218320 : pitch = quadplane.ahrs_view->pitch_sensor;
136 : }
137 : #endif
138 1310707 : struct log_Control_Tuning pkt = {
139 : LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
140 1310707 : time_us : AP_HAL::micros64(),
141 1310707 : nav_roll_cd : (int16_t)nav_roll_cd,
142 1310707 : roll : (int16_t)ahrs.roll_sensor,
143 1310707 : nav_pitch_cd : (int16_t)nav_pitch_cd,
144 : pitch : pitch,
145 1310707 : throttle_out : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
146 1310707 : rudder_out : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder),
147 1310707 : throttle_dem : TECS_controller.get_throttle_demand(),
148 : airspeed_estimate : est_airspeed,
149 : airspeed_estimate_status : (uint8_t)airspeed_estimate_type,
150 1310707 : EAS2TAS : ahrs.get_EAS2TAS(),
151 1310707 : groundspeed_undershoot : groundspeed_undershoot,
152 1310707 : };
153 1310707 : logger.WriteBlock(&pkt, sizeof(pkt));
154 1310707 : }
155 :
156 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
157 : struct PACKED log_OFG_Guided {
158 : LOG_PACKET_HEADER;
159 : uint64_t time_us;
160 : float target_airspeed_cm;
161 : float target_airspeed_accel;
162 : float target_alt;
163 : float target_alt_rate;
164 : uint8_t target_mav_frame; // received MavLink frame
165 : float target_heading;
166 : float target_heading_limit;
167 : uint8_t target_alt_frame; // internal AltFrame
168 : };
169 :
170 : // Write a OFG Guided packet.
171 6758 : void Plane::Log_Write_OFG_Guided()
172 : {
173 6758 : struct log_OFG_Guided pkt = {
174 : LOG_PACKET_HEADER_INIT(LOG_OFG_MSG),
175 6758 : time_us : AP_HAL::micros64(),
176 6758 : target_airspeed_cm : (float)guided_state.target_airspeed_cm*(float)0.01,
177 6758 : target_airspeed_accel : guided_state.target_airspeed_accel,
178 6758 : target_alt : guided_state.target_location.alt * 0.01,
179 6758 : target_alt_rate : guided_state.target_alt_rate,
180 6758 : target_mav_frame : guided_state.target_mav_frame,
181 6758 : target_heading : guided_state.target_heading,
182 6758 : target_heading_limit : guided_state.target_heading_accel_limit,
183 6758 : target_alt_frame : static_cast<uint8_t>(guided_state.target_location.get_alt_frame()),
184 6758 : };
185 6758 : logger.WriteBlock(&pkt, sizeof(pkt));
186 6758 : }
187 : #endif
188 :
189 : struct PACKED log_Nav_Tuning {
190 : LOG_PACKET_HEADER;
191 : uint64_t time_us;
192 : float wp_distance;
193 : int16_t target_bearing_cd;
194 : int16_t nav_bearing_cd;
195 : int16_t altitude_error_cm;
196 : float xtrack_error;
197 : float xtrack_error_i;
198 : float airspeed_error;
199 : int32_t target_lat;
200 : int32_t target_lng;
201 : int32_t target_alt_wp;
202 : int32_t target_alt_tecs;
203 : int32_t target_airspeed;
204 : };
205 :
206 : // Write a navigation tuning packet
207 1310707 : void Plane::Log_Write_Nav_Tuning()
208 : {
209 1310707 : struct log_Nav_Tuning pkt = {
210 : LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
211 1310707 : time_us : AP_HAL::micros64(),
212 1310707 : wp_distance : auto_state.wp_distance,
213 1310707 : target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(),
214 1310707 : nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(),
215 1310707 : altitude_error_cm : (int16_t)plane.calc_altitude_error_cm(),
216 1310707 : xtrack_error : nav_controller->crosstrack_error_m(),
217 1310707 : xtrack_error_i : nav_controller->crosstrack_error_integrator(),
218 1310707 : airspeed_error : airspeed_error,
219 1310707 : target_lat : next_WP_loc.lat,
220 1310707 : target_lng : next_WP_loc.lng,
221 1310707 : target_alt_wp : next_WP_loc.alt,
222 1310707 : target_alt_tecs : tecs_target_alt_cm,
223 1310707 : target_airspeed : target_airspeed_cm,
224 1310707 : };
225 1310707 : logger.WriteBlock(&pkt, sizeof(pkt));
226 1310707 : }
227 :
228 : #if AP_RANGEFINDER_ENABLED
229 : struct PACKED log_RFNS {
230 : LOG_PACKET_HEADER;
231 : uint64_t time_us;
232 : bool in_use;
233 : bool in_range;
234 : uint8_t in_range_count;
235 : bool have_initial_reading;
236 : float initial_range;
237 : float last_distance;
238 : float correction;
239 : float initial_correction;
240 : float last_stable_correction;
241 : uint32_t last_correction_time_ms;
242 : float height_estimate;
243 : };
244 :
245 : // Write a Rangefinder State packet
246 5098 : void Plane::Log_Write_RFNS()
247 : {
248 5098 : const struct log_RFNS pkt {
249 : LOG_PACKET_HEADER_INIT(LOG_RFNS_MSG),
250 5098 : time_us : AP_HAL::micros64(),
251 5098 : in_use : rangefinder_state.in_use,
252 5098 : in_range : rangefinder_state.in_range,
253 5098 : in_range_count : rangefinder_state.in_range_count,
254 5098 : have_initial_reading : rangefinder_state.have_initial_reading,
255 5098 : initial_range : rangefinder_state.initial_range,
256 5098 : last_distance : rangefinder_state.last_distance,
257 5098 : correction : rangefinder_state.correction,
258 5098 : initial_correction : rangefinder_state.initial_correction,
259 5098 : last_stable_correction : rangefinder_state.last_stable_correction,
260 5098 : last_correction_time_ms : rangefinder_state.last_correction_time_ms,
261 5098 : height_estimate : rangefinder_state.height_estimate
262 5098 : };
263 5098 : logger.WriteBlock(&pkt, sizeof(pkt));
264 5098 : }
265 : #endif // AP_RANGEFINDER_ENABLED
266 :
267 : struct PACKED log_Status {
268 : LOG_PACKET_HEADER;
269 : uint64_t time_us;
270 : uint8_t is_flying;
271 : float is_flying_probability;
272 : uint8_t armed;
273 : uint8_t safety;
274 : bool is_crashed;
275 : bool is_still;
276 : uint8_t stage;
277 : bool impact;
278 : bool throttle_supressed;
279 : };
280 :
281 281508 : void Plane::Log_Write_Status()
282 : {
283 281508 : struct log_Status pkt = {
284 : LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
285 281508 : ,time_us : AP_HAL::micros64()
286 563016 : ,is_flying : is_flying()
287 281508 : ,is_flying_probability : isFlyingProbability
288 563016 : ,armed : hal.util->get_soft_armed()
289 281508 : ,safety : static_cast<uint8_t>(hal.util->safety_switch_state())
290 281508 : ,is_crashed : crash_state.is_crashed
291 281508 : ,is_still : AP::ins().is_still()
292 281508 : ,stage : static_cast<uint8_t>(flight_stage)
293 281508 : ,impact : crash_state.impact_detected
294 281508 : ,throttle_supressed : throttle_suppressed
295 281508 : };
296 :
297 281508 : logger.WriteBlock(&pkt, sizeof(pkt));
298 281508 : }
299 :
300 : struct PACKED log_AETR {
301 : LOG_PACKET_HEADER;
302 : uint64_t time_us;
303 : float aileron;
304 : float elevator;
305 : float throttle;
306 : float rudder;
307 : float flap;
308 : float steering;
309 : float speed_scaler;
310 : };
311 :
312 1310707 : void Plane::Log_Write_AETR()
313 : {
314 1310707 : struct log_AETR pkt = {
315 : LOG_PACKET_HEADER_INIT(LOG_AETR_MSG)
316 1310707 : ,time_us : AP_HAL::micros64()
317 1310707 : ,aileron : SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)
318 1310707 : ,elevator : SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)
319 1310707 : ,throttle : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)
320 1310707 : ,rudder : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder)
321 1310707 : ,flap : SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto)
322 1310707 : ,steering : SRV_Channels::get_output_scaled(SRV_Channel::k_steering)
323 1310707 : ,speed_scaler : get_speed_scaler(),
324 1310707 : };
325 :
326 1310707 : logger.WriteBlock(&pkt, sizeof(pkt));
327 1310707 : }
328 :
329 1310707 : void Plane::Log_Write_RC(void)
330 : {
331 1310707 : logger.Write_RCIN();
332 1310707 : logger.Write_RCOUT();
333 : #if AP_RSSI_ENABLED
334 1310707 : if (rssi.enabled()) {
335 0 : logger.Write_RSSI();
336 : }
337 : #endif
338 1310707 : Log_Write_AETR();
339 1310707 : }
340 :
341 1310707 : void Plane::Log_Write_Guided(void)
342 : {
343 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
344 1310707 : if (control_mode != &mode_guided) {
345 1209078 : return;
346 : }
347 :
348 101629 : if (guided_state.target_heading_time_ms != 0) {
349 0 : logger.Write_PID(LOG_PIDG_MSG, g2.guidedHeading.get_pid_info());
350 : }
351 :
352 101629 : if (!guided_state.target_location_alt_is_minus_one() || is_positive(guided_state.target_airspeed_cm) ) {
353 6758 : Log_Write_OFG_Guided();
354 : }
355 : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
356 : }
357 :
358 : // incoming-to-vehicle mavlink COMMAND_INT can be logged
359 : struct PACKED log_CMDI {
360 : LOG_PACKET_HEADER;
361 : uint64_t TimeUS;
362 : uint16_t CId;
363 : uint8_t TSys;
364 : uint8_t TCmp;
365 : uint8_t cur;
366 : uint8_t cont;
367 : float Prm1;
368 : float Prm2;
369 : float Prm3;
370 : float Prm4;
371 : int32_t Lat;
372 : int32_t Lng;
373 : float Alt;
374 : uint8_t F;
375 : };
376 :
377 : // type and unit information can be found in
378 : // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
379 : // units and "Format characters" for field type information
380 : const struct LogStructure Plane::log_structure[] = {
381 : LOG_COMMON_STRUCTURES,
382 :
383 : // @LoggerMessage: CTUN
384 : // @Description: Control Tuning information
385 : // @Field: TimeUS: Time since system startup
386 : // @Field: NavRoll: desired roll
387 : // @Field: Roll: achieved roll
388 : // @Field: NavPitch: desired pitch assuming pitch trims are already applied
389 : // @Field: Pitch: achieved pitch assuming pitch trims are already applied,ie "0deg" is level flight trimmed pitch attitude as shown on artificial horizon level line.
390 : // @Field: ThO: scaled output throttle
391 : // @Field: RdO: scaled output rudder
392 : // @Field: ThD: demanded speed-height-controller throttle
393 : // @Field: As: airspeed estimate (or measurement if airspeed sensor healthy and ARSPD_USE>0)
394 : // @Field: AsT: airspeed type ( old estimate or source of new estimate)
395 : // @FieldValueEnum: AsT: AP_AHRS::AirspeedEstimateType
396 : // @Field: E2T: equivalent to true airspeed ratio
397 : // @Field: GU: groundspeed undershoot when flying with minimum groundspeed
398 :
399 : { LOG_CTUN_MSG, sizeof(log_Control_Tuning),
400 : "CTUN", "QccccffffBfi", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdO,ThD,As,AsT,E2T,GU", "sdddd---n--n", "FBBBB---00-B" , true },
401 :
402 : // @LoggerMessage: NTUN
403 : // @Description: Navigation Tuning information - e.g. vehicle destination
404 : // @Field: TimeUS: Time since system startup
405 : // @Field: Dist: distance to the current navigation waypoint
406 : // @Field: TBrg: bearing to the current navigation waypoint
407 : // @Field: NavBrg: the vehicle's desired heading
408 : // @Field: AltE: difference between current vehicle height and target height
409 : // @Field: XT: the vehicle's current distance from the current travel segment
410 : // @Field: XTi: integration of the vehicle's crosstrack error
411 : // @Field: AsE: difference between vehicle's airspeed and desired airspeed
412 : // @Field: TLat: target latitude
413 : // @Field: TLng: target longitude
414 : // @Field: TAW: target altitude WP
415 : // @Field: TAT: target altitude TECS
416 : // @Field: TAsp: target airspeed
417 : { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
418 : "NTUN", "QfcccfffLLeee", "TimeUS,Dist,TBrg,NavBrg,AltE,XT,XTi,AsE,TLat,TLng,TAW,TAT,TAsp", "smddmmmnDUmmn", "F0BBB0B0GG000" , true },
419 :
420 : // @LoggerMessage: ATRP
421 : // @Description: Plane AutoTune
422 : // @Vehicles: Plane
423 : // @Field: TimeUS: Time since system startup
424 : // @Field: Axis: tuning axis
425 : // @Field: State: tuning state
426 : // @Field: Sur: control surface deflection
427 : // @Field: PSlew: P slew rate
428 : // @Field: DSlew: D slew rate
429 : // @Field: FF0: FF value single sample
430 : // @Field: FF: FF value
431 : // @Field: P: P value
432 : // @Field: I: I value
433 : // @Field: D: D value
434 : // @Field: Action: action taken
435 : // @Field: RMAX: Rate maximum
436 : // @Field: TAU: time constant
437 : { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP),
438 : "ATRP", "QBBffffffffBff", "TimeUS,Axis,State,Sur,PSlew,DSlew,FF0,FF,P,I,D,Action,RMAX,TAU", "s#-dkk------ks", "F--00000000-00" , true },
439 :
440 : // @LoggerMessage: STAT
441 : // @Description: Current status of the aircraft
442 : // @Field: TimeUS: Time since system startup
443 : // @Field: isFlying: True if aircraft is probably flying
444 : // @Field: isFlyProb: Probability that the aircraft is flying
445 : // @Field: Armed: Arm status of the aircraft
446 : // @Field: Safety: State of the safety switch
447 : // @Field: Crash: True if crash is detected
448 : // @Field: Still: True when vehicle is not moving in any axis
449 : // @Field: Stage: Current stage of the flight
450 : // @Field: Hit: True if impact is detected
451 : // @Field: Sup: True if throttle is suppressed
452 : { LOG_STATUS_MSG, sizeof(log_Status),
453 : "STAT", "QBfBBBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit,Sup", "s---------", "F---------" , true },
454 :
455 : // @LoggerMessage: QTUN
456 : // @Description: QuadPlane vertical tuning message
457 : // @Field: TimeUS: Time since system startup
458 : // @Field: ThI: throttle input
459 : // @Field: ABst: angle boost
460 : // @Field: ThO: throttle output
461 : // @Field: ThH: calculated hover throttle
462 : // @Field: DAlt: desired altitude
463 : // @Field: Alt: achieved altitude
464 : // @Field: BAlt: barometric altitude
465 : // @Field: DCRt: desired climb rate
466 : // @Field: CRt: climb rate
467 : // @Field: TMix: transition throttle mix value
468 : // @Field: Trn: Transition state: 0-AirspeedWait,1-Timer,2-Done / TailSitter: 0-FW Wait,1-VTOL Wait,2-Done
469 : // @Field: Ast: bitmask of assistance flags
470 : // @FieldBitmaskEnum: Ast: log_assistance_flags
471 : #if HAL_QUADPLANE_ENABLED
472 : { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
473 : "QTUN", "QffffffeccfBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Trn,Ast", "s----mmmnn---", "F----00000---" , true },
474 : #endif
475 :
476 : // @LoggerMessage: PIQR
477 : // @Description: QuadPlane Proportional/Integral/Derivative gain values for Roll rate
478 : // @LoggerMessage: PIQP
479 : // @Description: QuadPlane Proportional/Integral/Derivative gain values for Pitch rate
480 : // @LoggerMessage: PIQY
481 : // @Description: QuadPlane Proportional/Integral/Derivative gain values for Yaw rate
482 : // @LoggerMessage: PIQA
483 : // @Description: QuadPlane Proportional/Integral/Derivative gain values for vertical acceleration
484 : // @Field: TimeUS: Time since system startup
485 : // @Field: Tar: desired value
486 : // @Field: Act: achieved value
487 : // @Field: Err: error between target and achieved
488 : // @Field: P: proportional part of PID
489 : // @Field: I: integral part of PID
490 : // @Field: D: derivative part of PID
491 : // @Field: FF: controller feed-forward portion of response
492 : // @Field: DFF: controller derivative feed-forward portion of response
493 : // @Field: Dmod: scaler applied to D gain to reduce limit cycling
494 : // @Field: SRate: slew rate
495 : // @Field: Flags: bitmask of PID state flags
496 : // @FieldBitmaskEnum: Flags: log_PID_Flags
497 : #if HAL_QUADPLANE_ENABLED
498 : { LOG_PIQR_MSG, sizeof(log_PID),
499 : "PIQR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true },
500 : { LOG_PIQP_MSG, sizeof(log_PID),
501 : "PIQP", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true },
502 : { LOG_PIQY_MSG, sizeof(log_PID),
503 : "PIQY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true },
504 : { LOG_PIQA_MSG, sizeof(log_PID),
505 : "PIQA", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true },
506 : #endif
507 :
508 : // @LoggerMessage: TSIT
509 : // @Description: tailsitter speed scaling values
510 : // @Field: TimeUS: Time since system startup
511 : // @Field: Ts: throttle scaling used for tilt motors
512 : // @Field: Ss: speed scaling used for control surfaces method from Q_TAILSIT_GSCMSK
513 : // @Field: Tmin: minimum output throttle calculated from disk thoery gain scale with Q_TAILSIT_MIN_VO
514 : #if HAL_QUADPLANE_ENABLED
515 : { LOG_TSIT_MSG, sizeof(Tailsitter::log_tailsitter),
516 : "TSIT", "Qfff", "TimeUS,Ts,Ss,Tmin", "s---", "F---" , true },
517 : #endif
518 :
519 : // @LoggerMessage: TILT
520 : // @Description: Tiltrotor tilt values
521 : // @Field: TimeUS: Time since system startup
522 : // @Field: Tilt: Current tilt angle, 0 deg vertical, 90 deg horizontal
523 : // @Field: FL: Front left tilt angle, 0 deg vertical, 90 deg horizontal
524 : // @Field: FR: Front right tilt angle, 0 deg vertical, 90 deg horizontal
525 : #if HAL_QUADPLANE_ENABLED
526 : { LOG_TILT_MSG, sizeof(Tiltrotor::log_tiltrotor),
527 : "TILT", "Qfff", "TimeUS,Tilt,FL,FR", "sddd", "F---" , true },
528 : #endif
529 :
530 : // @LoggerMessage: PIDG
531 : // @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control.
532 : // @Field: TimeUS: Time since system startup
533 : // @Field: Tar: desired value
534 : // @Field: Act: achieved value
535 : // @Field: Err: error between target and achieved
536 : // @Field: P: proportional part of PID
537 : // @Field: I: integral part of PID
538 : // @Field: D: derivative part of PID
539 : // @Field: FF: controller feed-forward portion of response
540 : // @Field: DFF: controller derivative feed-forward portion of response
541 : // @Field: Dmod: scaler applied to D gain to reduce limit cycling
542 : // @Field: SRate: slew rate
543 : // @Field: Flags: bitmask of PID state flags
544 : // @FieldBitmaskEnum: Flags: log_PID_Flags
545 : { LOG_PIDG_MSG, sizeof(log_PID),
546 : "PIDG", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true },
547 :
548 : // @LoggerMessage: AETR
549 : // @Description: Normalised pre-mixer control surface outputs
550 : // @Field: TimeUS: Time since system startup
551 : // @Field: Ail: Pre-mixer value for aileron output (between -4500 and 4500)
552 : // @Field: Elev: Pre-mixer value for elevator output (between -4500 and 4500)
553 : // @Field: Thr: Pre-mixer value for throttle output (between -100 and 100)
554 : // @Field: Rudd: Pre-mixer value for rudder output (between -4500 and 4500)
555 : // @Field: Flap: Pre-mixer value for flaps output (between 0 and 100)
556 : // @Field: Steer: Pre-mixer value for steering output (between -4500 and 4500)
557 : // @Field: SS: Surface movement / airspeed scaling value
558 : { LOG_AETR_MSG, sizeof(log_AETR),
559 : "AETR", "Qfffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,Steer,SS", "s-------", "F-------" , true },
560 :
561 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
562 : // @LoggerMessage: OFG
563 : // @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s.
564 : // @Field: TimeUS: Time since system startup
565 : // @Field: Arsp: target airspeed cm
566 : // @Field: ArspA: target airspeed accel
567 : // @Field: Alt: target alt
568 : // @Field: AltA: target alt velocity (rate of change)
569 : // @Field: AltF: target alt frame (MAVLink)
570 : // @Field: Hdg: target heading
571 : // @Field: HdgA: target heading lim
572 : // @Field: AltL: target alt frame (Location)
573 : { LOG_OFG_MSG, sizeof(log_OFG_Guided),
574 : "OFG", "QffffBffB", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA,AltL", "snnmo-d--", "F--------" , true },
575 : #endif
576 :
577 : #if AP_RANGEFINDER_ENABLED
578 : // @LoggerMessage: RFNS
579 : // @Description: Rangefinder state
580 : // @Field: TimeUS: Time since system startup
581 : // @Field: InUse: Whether the rangefinder is in use.
582 : // @Field: InRng: Whether the rangefinder is in range.
583 : // @Field: IRCnt: Count of in-range measurements
584 : // @Field: Ms0OK: Whether there has been an initial measurement.
585 : // @Field: Ms0: The initial measured range
586 : // @Field: Dst: The last recorded distance
587 : // @Field: Cor: The rangefinder correction
588 : // @Field: Cor0: The initial rangefinder correction
589 : // @Field: CorL: The last stable correction
590 : // @Field: TimeCL: The last correction time
591 : // @Field: HE: Height estimate
592 : { LOG_RFNS_MSG, sizeof(log_RFNS),
593 : "RFNS", "QBBBBfffffIf", "TimeUS,InUse,InRng,IRCnt,Ms0OK,Ms0,Dst,Cor,Cor0,CorL,TimeCL,HE", "s----mmmmmsm", "F----00000C0", true },
594 : #endif // AP_RANGEFINDER_ENABLED
595 :
596 : // @LoggerMessage: ATIS
597 : // @Description: Fixedwing attitude control input shaping
598 : // @Field: TimeUS: Time since system startup
599 : // @Field: rAng: Shaped roll angle target
600 : // @Field: rErr: Roll angle error between shaped and measured
601 : // @Field: rVel: Shaped roll velocity target
602 : // @Field: rAcc: Shaped roll acceleration target
603 : // @Field: pAng: Shaped pitch angle target
604 : // @Field: pErr: Pitch angle error between shaped and measured
605 : // @Field: pVel: Shaped pitch velocity target
606 : // @Field: pAcc: Shaped pitch acceleration target
607 : { LOG_ATIS_MSG, sizeof(log_ATIS),
608 : "ATIS", "Qffffffff", "TimeUS,rAng,rErr,rVel,rAcc,pAng,pErr,pVel,pAcc", "sddkeddke", "F00000000", true },
609 : };
610 :
611 692 : uint8_t Plane::get_num_log_structures() const
612 : {
613 692 : return ARRAY_SIZE(log_structure);
614 : }
615 :
616 1825 : void Plane::Log_Write_Vehicle_Startup_Messages()
617 : {
618 : // only 200(?) bytes are guaranteed by AP_Logger
619 : #if HAL_QUADPLANE_ENABLED
620 1825 : if (quadplane.initialised) {
621 : char frame_and_type_string[30];
622 538 : quadplane.motors->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
623 538 : logger.Write_MessageF("QuadPlane %s", frame_and_type_string);
624 : }
625 : #endif
626 1825 : logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
627 1825 : ahrs.Log_Write_Home_And_Origin();
628 1825 : gps.Write_AP_Logger_Log_Startup_messages();
629 1825 : }
630 :
631 : #if AP_PLANE_BLACKBOX_LOGGING
632 : /*
633 : logging for blackbox recording. Split across two messages but with
634 : the same timestamp
635 : */
636 : void Plane::Log_Write_Blackbox(void)
637 : {
638 : const uint64_t now_us = AP_HAL::micros64();
639 : // @LoggerMessage: BBX1
640 : // @Description: BlackBox data1
641 : // @Field: TimeUS: Time since system startup
642 : // @Field: Lat: Latitude
643 : // @Field: Lon: Longitude
644 : // @Field: Alt: altitude above sea level
645 : // @Field: Roll: euler roll
646 : // @Field: Pitch: euler pitch
647 : // @Field: Yaw: euler yaw
648 : // @Field: VN: velocity north
649 : // @Field: VE: velocity east
650 : // @Field: VD: velocity down
651 : Location loc;
652 : Vector3f vel;
653 : float alt;
654 : if (!ahrs.get_location(loc) ||
655 : !ahrs.get_velocity_NED(vel) ||
656 : !loc.get_alt_m(Location::AltFrame::ABSOLUTE, alt)) {
657 : return;
658 : }
659 :
660 : AP::logger().WriteStreaming("BBX1", "TimeUS,Lat,Lon,Alt,Roll,Pitch,Yaw,VN,VE,VD",
661 : "sDUmdddnnn",
662 : "FGG-------",
663 : "QLLfffffff",
664 : now_us,
665 : loc.lat, loc.lng, alt,
666 : degrees(ahrs.get_roll_rad()),
667 : degrees(ahrs.get_pitch_rad()),
668 : degrees(ahrs.get_yaw_rad()),
669 : vel.x, vel.y, vel.z);
670 :
671 : // @LoggerMessage: BBX2
672 : // @Description: BlackBox data2
673 : // @Field: TimeUS: Time since system startup
674 : // @Field: GyrX: X axis gyro
675 : // @Field: GyrY: Y axis gyro
676 : // @Field: GyrZ: Z axis gyro
677 : // @Field: AccX: accel X axis (front)
678 : // @Field: AccY: accel Y axis (right)
679 : // @Field: AccZ: accel Z axis (down)
680 : const auto &gyro = ahrs.get_gyro();
681 : const auto &accel = ahrs.get_accel();
682 : AP::logger().WriteStreaming("BBX2", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ",
683 : "skkkooo",
684 : "F------",
685 : "Qffffff",
686 : now_us,
687 : degrees(gyro.x), degrees(gyro.y), degrees(gyro.z),
688 : accel.x, accel.y, accel.z);
689 : }
690 : #endif // AP_PLANE_BLACKBOX_LOGGING
691 :
692 : #endif // HAL_LOGGING_ENABLED
|
