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

AP_Proximity: move logging of Proximity into Proximity library

apm_2208
Peter Barker 3 years ago committed by Peter Barker
parent
795427e574
commit
a62e08ee40
3 changed files with 149 additions and 3 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. 67
      libraries/AP_Proximity/AP_Proximity.cpp
  2. 8
      libraries/AP_Proximity/AP_Proximity.h
  3. 77
      libraries/AP_Proximity/LogStructure.h

67
libraries/AP_Proximity/AP_Proximity.cpp
Unescape View File

@ -27,6 +27,8 @@ @@ -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) @@ -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
Unescape View File

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

@ -0,0 +1,77 @@ @@ -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 },
Write Preview
Loading…
Cancel
Save