From 795427e574c60bbc59536a25f3234214e857d871 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:12:18 +1100 Subject: [PATCH] AP_Logger: move logging of Proximity into Proximity library --- libraries/AP_Logger/AP_Logger.h | 4 -- libraries/AP_Logger/LogFile.cpp | 65 --------------------------- libraries/AP_Logger/LogStructure.h | 71 ++---------------------------- 3 files changed, 3 insertions(+), 137 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 41f31b8ece..4ed38cee1d 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -59,7 +59,6 @@ #include #include #include -#include #include #include @@ -308,9 +307,6 @@ public: uint8_t sequence, const RallyLocation &rally_point); void Write_Beacon(AP_Beacon &beacon); -#if HAL_PROXIMITY_ENABLED - void Write_Proximity(AP_Proximity &proximity); -#endif void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); void Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp); void Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index dc0700e21d..80d3290c08 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -453,71 +453,6 @@ void AP_Logger::Write_Beacon(AP_Beacon &beacon) WriteBlock(&pkt_beacon, sizeof(pkt_beacon)); } -#if HAL_PROXIMITY_ENABLED -// Write proximity sensor distances -void AP_Logger::Write_Proximity(AP_Proximity &proximity) -{ - // exit immediately if not enabled - if (proximity.get_status() == AP_Proximity::Status::NotConnected) { - return; - } - - 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 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)); - } - } -} -#endif - void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb) { const struct log_SRTL pkt_srtl{ diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 0ea2caa454..b6604f26ab 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -132,6 +132,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include #include #include #include @@ -527,38 +528,6 @@ struct PACKED log_Beacon { float posz; }; -// proximity sensor logging -struct PACKED log_Proximity { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t instance; - uint8_t health; - float dist0; - float dist45; - float dist90; - float dist135; - float dist180; - float dist225; - float dist270; - float dist315; - float distup; - 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; uint64_t time_us; @@ -1005,36 +974,6 @@ struct PACKED log_VER { // @Field: MVmin: MCU Voltage min // @Field: MVmax: MCU Voltage max -// @LoggerMessage: PRX -// @Description: Proximity Filtered 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: 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 -// @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 -// @Field: DUp: Nearest object in upwards direction -// @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 @@ -1293,10 +1232,7 @@ LOG_STRUCTURE_FROM_CAMERA \ "DMS", "QIIIIBBBBBBBBB", "TimeUS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "F-------------" }, \ { LOG_BEACON_MSG, sizeof(log_Beacon), \ "BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--0000000", true }, \ - { LOG_PROXIMITY_MSG, sizeof(log_Proximity), \ - "PRX", "QBBfffffffffff", "TimeUS,Layer,He,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis", "s#-mmmmmmmmmhm", "F--00000000000", true }, \ - { LOG_RAW_PROXIMITY_MSG, sizeof(log_Proximity_raw), \ - "PRXR", "QBffffffff", "TimeUS,Layer,D0,D45,D90,D135,D180,D225,D270,D315", "s#mmmmmmmm", "F-00000000", true }, \ + LOG_STRUCTURE_FROM_PROXIMITY \ { 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), \ @@ -1430,7 +1366,7 @@ enum LogMessages : uint8_t { LOG_IDS_FROM_VISUALODOM, LOG_IDS_FROM_AVOIDANCE, LOG_BEACON_MSG, - LOG_PROXIMITY_MSG, + LOG_IDS_FROM_PROXIMITY, LOG_DF_FILE_STATS, LOG_SRTL_MSG, LOG_PERFORMANCE_MSG, @@ -1445,7 +1381,6 @@ enum LogMessages : uint8_t { LOG_PSCN_MSG, LOG_PSCE_MSG, LOG_PSCD_MSG, - LOG_RAW_PROXIMITY_MSG, LOG_IDS_FROM_PRECLAND, LOG_IDS_FROM_AIS, LOG_STAK_MSG,