diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index b3d4a7e9a1..f76586ac4a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -210,6 +210,12 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) _backend->update(); run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid); } + + const uint32_t now = AP_HAL::millis(); + if (now - last_log_ms > 40) { // 25Hz + last_log_ms = now; + Write_Precland(); + } } bool AC_PrecLand::target_acquired() @@ -454,3 +460,38 @@ void AC_PrecLand::run_output_prediction() _target_pos_rel_out_NE.x += land_ofs_ned_m.x; _target_pos_rel_out_NE.y += land_ofs_ned_m.y; } + +// Write a precision landing entry +void AC_PrecLand::Write_Precland() +{ + // exit immediately if not enabled + if (!enabled()) { + return; + } + + Vector3f target_pos_meas; + Vector2f target_pos_rel; + Vector2f target_vel_rel; + get_target_position_relative_cm(target_pos_rel); + get_target_velocity_relative_cms(target_vel_rel); + get_target_position_measurement_cm(target_pos_meas); + + const struct log_Precland pkt { + LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG), + time_us : AP_HAL::micros64(), + healthy : healthy(), + target_acquired : target_acquired(), + pos_x : target_pos_rel.x, + pos_y : target_pos_rel.y, + vel_x : target_vel_rel.x, + vel_y : target_vel_rel.y, + meas_x : target_pos_meas.x, + meas_y : target_pos_meas.y, + meas_z : target_pos_meas.z, + last_meas : last_backend_los_meas_ms(), + ekf_outcount : ekf_outlier_count(), + estimator : estimator_type() + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 1253530966..f8eb4fe686 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -156,4 +156,8 @@ private: bool healthy; } _backend_state; AC_PrecLand_Backend *_backend; // pointers to backend precision landing driver + + // write out PREC message to log: + void Write_Precland(); + uint32_t last_log_ms; // last time we logged }; diff --git a/libraries/AC_PrecLand/LogStructure.h b/libraries/AC_PrecLand/LogStructure.h new file mode 100644 index 0000000000..216330cd5b --- /dev/null +++ b/libraries/AC_PrecLand/LogStructure.h @@ -0,0 +1,44 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_PRECLAND \ + LOG_PRECLAND_MSG + +// @LoggerMessage: PL +// @Description: Precision Landing messages +// @Field: TimeUS: Time since system startup +// @Field: Heal: True if Precision Landing is healthy +// @Field: TAcq: True if landing target is detected +// @Field: pX: Target position relative to vehicle, X-Axis (0 if target not found) +// @Field: pY: Target position relative to vehicle, Y-Axis (0 if target not found) +// @Field: vX: Target velocity relative to vehicle, X-Axis (0 if target not found) +// @Field: vY: Target velocity relative to vehicle, Y-Axis (0 if target not found) +// @Field: mX: Target's relative to origin position as 3-D Vector, X-Axis +// @Field: mY: Target's relative to origin position as 3-D Vector, Y-Axis +// @Field: mZ: Target's relative to origin position as 3-D Vector, Z-Axis +// @Field: LastMeasMS: Time when target was last detected +// @Field: EKFOutl: EKF's outlier count +// @Field: Est: Type of estimator used + +// precision landing logging +struct PACKED log_Precland { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t healthy; + uint8_t target_acquired; + float pos_x; + float pos_y; + float vel_x; + float vel_y; + float meas_x; + float meas_y; + float meas_z; + uint32_t last_meas; + uint32_t ekf_outcount; + uint8_t estimator; +}; + +#define LOG_STRUCTURE_FROM_PRECLAND \ + { LOG_PRECLAND_MSG, sizeof(log_Precland), \ + "PL", "QBBfffffffIIB", "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasMS,EKFOutl,Est", "s--mmnnmmms--","F--BBBBBBBC--" },