mirror of https://github.com/ArduPilot/ardupilot
AP_Logger: Log raw and filtered distances from proximity lib
This commit is contained in:
Rishabh
Randy Mackay
parent
044e1850d4
commit
e09e9b1e6a
|
|
@ -554,34 +554,59 @@ void AP_Logger::Write_Proximity(AP_Proximity &proximity)
|
|||
return;
|
||||
}
|
||||
|
||||
AP_Proximity::Proximity_Distance_Array dist_array {};
|
||||
proximity.get_horizontal_distances(dist_array);
|
||||
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 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));
|
||||
}
|
||||
}
|
||||
|
||||
float close_ang = 0.0f, close_dist = 0.0f;
|
||||
proximity.get_closest_object(close_ang, close_dist);
|
||||
|
||||
const struct log_Proximity pkt_proximity{
|
||||
LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
health : (uint8_t)proximity.get_status(),
|
||||
dist0 : dist_array.distance[0],
|
||||
dist45 : dist_array.distance[1],
|
||||
dist90 : dist_array.distance[2],
|
||||
dist135 : dist_array.distance[3],
|
||||
dist180 : dist_array.distance[4],
|
||||
dist225 : dist_array.distance[5],
|
||||
dist270 : dist_array.distance[6],
|
||||
dist315 : dist_array.distance[7],
|
||||
distup : dist_up,
|
||||
closest_angle : close_ang,
|
||||
closest_dist : close_dist
|
||||
};
|
||||
WriteBlock(&pkt_proximity, sizeof(pkt_proximity));
|
||||
}
|
||||
|
||||
void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb)
|
||||
|
|
|
|||
|
|
@ -657,6 +657,7 @@ struct PACKED log_Beacon {
|
|||
struct PACKED log_Proximity {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
uint8_t instance;
|
||||
uint8_t health;
|
||||
float dist0;
|
||||
float dist45;
|
||||
|
|
@ -670,6 +671,19 @@ struct PACKED log_Proximity {
|
|||
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;
|
||||
|
|
@ -1205,9 +1219,10 @@ struct PACKED log_PSCZ {
|
|||
// @Field: Safety: Hardware Safety Switch status
|
||||
|
||||
// @LoggerMessage: PRX
|
||||
// @Description: Proximity sensor data
|
||||
// @Description: Proximity Filtered sensor data
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: Health: True if proximity sensor is healthy
|
||||
// @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
|
||||
|
|
@ -1220,6 +1235,19 @@ struct PACKED log_PSCZ {
|
|||
// @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
|
||||
|
|
@ -1463,7 +1491,9 @@ LOG_STRUCTURE_FROM_CAMERA \
|
|||
{ LOG_BEACON_MSG, sizeof(log_Beacon), \
|
||||
"BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--0000000" }, \
|
||||
{ LOG_PROXIMITY_MSG, sizeof(log_Proximity), \
|
||||
"PRX", "QBfffffffffff", "TimeUS,Health,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis", "s-mmmmmmmmmhm", "F-00000000000" }, \
|
||||
"PRX", "QBBfffffffffff", "TimeUS,Layer,He,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis", "s#-mmmmmmmmmhm", "F--00000000000" }, \
|
||||
{ LOG_RAW_PROXIMITY_MSG, sizeof(log_Proximity_raw), \
|
||||
"PRXR", "QBffffffff", "TimeUS,Layer,D0,D45,D90,D135,D180,D225,D270,D315", "s#mmmmmmmm", "F-00000000" }, \
|
||||
{ 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), \
|
||||
|
|
@ -1655,6 +1685,7 @@ enum LogMessages : uint8_t {
|
|||
LOG_WINCH_MSG,
|
||||
LOG_PSC_MSG,
|
||||
LOG_PSCZ_MSG,
|
||||
LOG_RAW_PROXIMITY_MSG,
|
||||
|
||||
_LOG_LAST_MSG_
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue