From e09e9b1e6a04394a2c49fef75ac4d220e27c5f8e Mon Sep 17 00:00:00 2001 From: Rishabh Date: Fri, 12 Feb 2021 14:01:30 +0530 Subject: [PATCH] AP_Logger: Log raw and filtered distances from proximity lib --- libraries/AP_Logger/LogFile.cpp | 77 ++++++++++++++++++++---------- libraries/AP_Logger/LogStructure.h | 37 ++++++++++++-- 2 files changed, 85 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 32b1301575..003b9d9817 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -554,34 +554,59 @@ void AP_Logger::Write_Proximity(AP_Proximity &proximity) return; } - AP_Proximity::Proximity_Distance_Array dist_array {}; - proximity.get_horizontal_distances(dist_array); + AP_Proximity::Proximity_Distance_Array dist_array{}; // raw distances stored here + AP_Proximity::Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here + for (uint8_t i = 0; i < proximity.get_num_layers(); i++) { + const bool active = proximity.get_active_layer_distances(i, dist_array, filt_dist_array); + if (!active) { + // nothing on this layer + continue; + } + float dist_up; + if (!proximity.get_upward_distance(dist_up)) { + dist_up = 0.0f; + } - float dist_up; - if (!proximity.get_upward_distance(dist_up)) { - dist_up = 0.0f; + float closest_ang = 0.0f; + float closest_dist = 0.0f; + proximity.get_closest_object(closest_ang, closest_dist); + + const struct log_Proximity pkt_proximity{ + LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG), + time_us : AP_HAL::micros64(), + instance : i, + health : (uint8_t)proximity.get_status(), + dist0 : filt_dist_array.distance[0], + dist45 : filt_dist_array.distance[1], + dist90 : filt_dist_array.distance[2], + dist135 : filt_dist_array.distance[3], + dist180 : filt_dist_array.distance[4], + dist225 : filt_dist_array.distance[5], + dist270 : filt_dist_array.distance[6], + dist315 : filt_dist_array.distance[7], + distup : dist_up, + closest_angle : closest_ang, + closest_dist : closest_dist + }; + WriteBlock(&pkt_proximity, sizeof(pkt_proximity)); + + if (proximity.get_raw_log_enable()) { + const struct log_Proximity_raw pkt_proximity_raw{ + LOG_PACKET_HEADER_INIT(LOG_RAW_PROXIMITY_MSG), + time_us : AP_HAL::micros64(), + instance : i, + raw_dist0 : dist_array.distance[0], + raw_dist45 : dist_array.distance[1], + raw_dist90 : dist_array.distance[2], + raw_dist135 : dist_array.distance[3], + raw_dist180 : dist_array.distance[4], + raw_dist225 : dist_array.distance[5], + raw_dist270 : dist_array.distance[6], + raw_dist315 : dist_array.distance[7], + }; + WriteBlock(&pkt_proximity_raw, sizeof(pkt_proximity_raw)); + } } - - float close_ang = 0.0f, close_dist = 0.0f; - proximity.get_closest_object(close_ang, close_dist); - - const struct log_Proximity pkt_proximity{ - LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG), - time_us : AP_HAL::micros64(), - health : (uint8_t)proximity.get_status(), - dist0 : dist_array.distance[0], - dist45 : dist_array.distance[1], - dist90 : dist_array.distance[2], - dist135 : dist_array.distance[3], - dist180 : dist_array.distance[4], - dist225 : dist_array.distance[5], - dist270 : dist_array.distance[6], - dist315 : dist_array.distance[7], - distup : dist_up, - closest_angle : close_ang, - closest_dist : close_dist - }; - WriteBlock(&pkt_proximity, sizeof(pkt_proximity)); } void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 3c47c00ae1..932808724b 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -657,6 +657,7 @@ struct PACKED log_Beacon { struct PACKED log_Proximity { LOG_PACKET_HEADER; uint64_t time_us; + uint8_t instance; uint8_t health; float dist0; float dist45; @@ -670,6 +671,19 @@ struct PACKED log_Proximity { float closest_angle; float closest_dist; }; +struct PACKED log_Proximity_raw { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + float raw_dist0; + float raw_dist45; + float raw_dist90; + float raw_dist135; + float raw_dist180; + float raw_dist225; + float raw_dist270; + float raw_dist315; +}; struct PACKED log_Performance { LOG_PACKET_HEADER; @@ -1205,9 +1219,10 @@ struct PACKED log_PSCZ { // @Field: Safety: Hardware Safety Switch status // @LoggerMessage: PRX -// @Description: Proximity sensor data +// @Description: Proximity Filtered sensor data // @Field: TimeUS: Time since system startup -// @Field: Health: True if proximity sensor is healthy +// @Field: Layer: Pitch(instance) at which the obstacle is at. 0th layer {-75,-45} degrees. 1st layer {-45,-15} degrees. 2nd layer {-15, 15} degrees. 3rd layer {15, 45} degrees. 4th layer {45,75} degrees. Minimum distance in each layer will be logged. +// @Field: He: True if proximity sensor is healthy // @Field: D0: Nearest object in sector surrounding 0-degrees // @Field: D45: Nearest object in sector surrounding 45-degrees // @Field: D90: Nearest object in sector surrounding 90-degrees @@ -1220,6 +1235,19 @@ struct PACKED log_PSCZ { // @Field: CAn: Angle to closest object // @Field: CDis: Distance to closest object +// @LoggerMessage: PRXR +// @Description: Proximity Raw sensor data +// @Field: TimeUS: Time since system startup +// @Field: Layer: Pitch(instance) at which the obstacle is at. 0th layer {-75,-45} degrees. 1st layer {-45,-15} degrees. 2nd layer {-15, 15} degrees. 3rd layer {15, 45} degrees. 4th layer {45,75} degrees. Minimum distance in each layer will be logged. +// @Field: D0: Nearest object in sector surrounding 0-degrees +// @Field: D45: Nearest object in sector surrounding 45-degrees +// @Field: D90: Nearest object in sector surrounding 90-degrees +// @Field: D135: Nearest object in sector surrounding 135-degrees +// @Field: D180: Nearest object in sector surrounding 180-degrees +// @Field: D225: Nearest object in sector surrounding 225-degrees +// @Field: D270: Nearest object in sector surrounding 270-degrees +// @Field: D315: Nearest object in sector surrounding 315-degrees + // @LoggerMessage: RAD // @Description: Telemetry radio statistics // @Field: TimeUS: Time since system startup @@ -1463,7 +1491,9 @@ LOG_STRUCTURE_FROM_CAMERA \ { LOG_BEACON_MSG, sizeof(log_Beacon), \ "BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--0000000" }, \ { LOG_PROXIMITY_MSG, sizeof(log_Proximity), \ - "PRX", "QBfffffffffff", "TimeUS,Health,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis", "s-mmmmmmmmmhm", "F-00000000000" }, \ + "PRX", "QBBfffffffffff", "TimeUS,Layer,He,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis", "s#-mmmmmmmmmhm", "F--00000000000" }, \ + { LOG_RAW_PROXIMITY_MSG, sizeof(log_Proximity_raw), \ + "PRXR", "QBffffffff", "TimeUS,Layer,D0,D45,D90,D135,D180,D225,D270,D315", "s#mmmmmmmm", "F-00000000" }, \ { LOG_PERFORMANCE_MSG, sizeof(log_Performance), \ "PM", "QHHIIHHIIIIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "s---b%------s", "F---0A------F" }, \ { LOG_SRTL_MSG, sizeof(log_SRTL), \ @@ -1655,6 +1685,7 @@ enum LogMessages : uint8_t { LOG_WINCH_MSG, LOG_PSC_MSG, LOG_PSCZ_MSG, + LOG_RAW_PROXIMITY_MSG, _LOG_LAST_MSG_ };