zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Logger: move logging of Proximity into Proximity library

apm_2208
Peter Barker 3 years ago committed by Peter Barker
parent
3d854c7e92
commit
795427e574
3 changed files with 3 additions and 137 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_Logger/AP_Logger.h
  2. 65
      libraries/AP_Logger/LogFile.cpp
  3. 71
      libraries/AP_Logger/LogStructure.h

4
libraries/AP_Logger/AP_Logger.h
Unescape View File

@ -59,7 +59,6 @@ @@ -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: @@ -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
Unescape View File

@ -453,71 +453,6 @@ void AP_Logger::Write_Beacon(AP_Beacon &beacon) @@ -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
Unescape View File

@ -132,6 +132,7 @@ const struct MultiplierStructure log_Multipliers[] = { @@ -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 { @@ -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 { @@ -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 \ @@ -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 { @@ -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 { @@ -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,

Write Preview
Loading…
Cancel
Save