Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: move logging of Proximity into Proximity library

Browse Source
This commit is contained in:
Peter Barker 2022-03-04 12:12:18 +11:00 committed by Peter Barker
parent 3d854c7e92
commit 795427e574
3 changed files with 3 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Logger/AP_Logger.h
View File

@ -59,7 +59,6 @@
#include <AP_Motors/AP_Motors.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_Beacon/AP_Beacon.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_Vehicle/ModeReason.h>
#include <stdint.h>
@ -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);

65
libraries/AP_Logger/LogFile.cpp
View File

@ -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{

71
libraries/AP_Logger/LogStructure.h
View File

@ -132,6 +132,7 @@ const struct MultiplierStructure log_Multipliers[] = {
#include <AP_Baro/LogStructure.h>
#include <AP_VisualOdom/LogStructure.h>
#include <AC_PrecLand/LogStructure.h>
#include <AP_Proximity/LogStructure.h>
#include <AC_Avoidance/LogStructure.h>
#include <AP_ESC_Telem/LogStructure.h>
#include <AP_AIS/LogStructure.h>
@ -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,