Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: move logging of Proximity into Proximity library

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

67
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -27,6 +27,8 @@
#include "AP_Proximity_AirSimSITL.h"
#include "AP_Proximity_Cygbot_D1.h"
#include <AP_Logger/AP_Logger.h>
extern const AP_HAL::HAL &hal;
// table of user settable parameters
@ -522,6 +524,71 @@ void AP_Proximity::set_rangefinder_alt(bool use, bool healthy, float alt_cm)
drivers[primary_instance]->set_rangefinder_alt(use, healthy, alt_cm);
}
#if HAL_LOGGING_ENABLED
// Write proximity sensor distances
void AP_Proximity::log()
{
// exit immediately if not enabled
if (get_status() == AP_Proximity::Status::NotConnected) {
return;
}
Proximity_Distance_Array dist_array{}; // raw distances stored here
Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here
auto &logger { AP::logger() };
for (uint8_t i = 0; i < get_num_layers(); i++) {
const bool active = get_active_layer_distances(i, dist_array, filt_dist_array);
if (!active) {
// nothing on this layer
continue;
}
float dist_up;
if (!get_upward_distance(dist_up)) {
dist_up = 0.0f;
}
float closest_ang = 0.0f;
float closest_dist = 0.0f;
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)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
};
logger.WriteBlock(&pkt_proximity, sizeof(pkt_proximity));
if (_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],
};
logger.WriteBlock(&pkt_proximity_raw, sizeof(pkt_proximity_raw));
}
}
}
#endif
AP_Proximity *AP_Proximity::_singleton;

8
libraries/AP_Proximity/AP_Proximity.h
View File

@ -151,9 +151,6 @@ public:
Type get_type(uint8_t instance) const;
// true if raw distances should be logged
bool get_raw_log_enable() const { return _raw_log_enable; }
// parameter list
static const struct AP_Param::GroupInfo var_info[];
@ -168,6 +165,10 @@ public:
// set alt as read from downward facing rangefinder. Tilt is already adjusted for
void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
// method called by vehicle to have AP_Proximity write onboard log
// messages:
void log();
private:
static AP_Proximity *_singleton;
Proximity_State state[PROXIMITY_MAX_INSTANCES];
@ -195,6 +196,7 @@ private:
AP_Float _min_m; // Proximity minimum range
void detect_instance(uint8_t instance);
};
namespace AP {

77
libraries/AP_Proximity/LogStructure.h Normal file
View File

@ -0,0 +1,77 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_PROXIMITY \
LOG_PROXIMITY_MSG, \
LOG_RAW_PROXIMITY_MSG
// @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
// 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;
};
// @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
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;
};
#define LOG_STRUCTURE_FROM_PROXIMITY \
{ 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 },