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

AC_Avoidance: Privatize Logging

Browse Source
This commit is contained in:
Josh Henderson 2021-01-22 02:23:53 -05:00 committed by Peter Barker
parent 8fdfdebe55
commit aac9771daf
7 changed files with 178 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -204,6 +204,9 @@ private:
// returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor
void get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative);
// Logging function
void Write_SimpleAvoidance(const uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, const bool back_up) const;
// parameters
AP_Int8 _enabled;
AP_Int16 _angle_max; // maximum lean angle to avoid obstacles (only used in non-GPS flight modes)

62
libraries/AC_Avoidance/AC_Avoidance_Logging.cpp Normal file
View File

@ -0,0 +1,62 @@
#include "AC_Avoid.h"
#include "AP_OADijkstra.h"
#include "AP_OABendyRuler.h"
#include <AP_Logger/AP_Logger.h>
void AP_OABendyRuler::Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const
{
int32_t oa_dest_alt, final_alt;
const bool got_oa_dest = oa_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, oa_dest_alt);
const bool got_final_dest = final_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, final_alt);
const struct log_OABendyRuler pkt{
LOG_PACKET_HEADER_INIT(LOG_OA_BENDYRULER_MSG),
time_us : AP_HAL::micros64(),
type : type,
active : active,
target_yaw : (uint16_t)wrap_360(target_yaw),
yaw : (uint16_t)wrap_360(AP::ahrs().yaw_sensor * 0.01f),
target_pitch: (uint16_t)target_pitch,
resist_chg : resist_chg,
margin : margin,
final_lat : final_dest.lat,
final_lng : final_dest.lng,
final_alt : got_final_dest ? final_alt : final_dest.alt,
oa_lat : oa_dest.lat,
oa_lng : oa_dest.lng,
oa_alt : got_oa_dest ? oa_dest_alt : oa_dest.alt
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const
{
const struct log_OADijkstra pkt{
LOG_PACKET_HEADER_INIT(LOG_OA_DIJKSTRA_MSG),
time_us : AP_HAL::micros64(),
state : state,
error_id : error_id,
curr_point : curr_point,
tot_points : tot_points,
final_lat : final_dest.lat,
final_lng : final_dest.lng,
oa_lat : oa_dest.lat,
oa_lng : oa_dest.lng
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, const bool back_up) const
{
const struct log_SimpleAvoid pkt{
LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG),
time_us : AP_HAL::micros64(),
state : state,
desired_vel_x : desired_vel.x * 0.01f,
desired_vel_y : desired_vel.y * 0.01f,
modified_vel_x : modified_vel.x * 0.01f,
modified_vel_y : modified_vel.y * 0.01f,
backing_up : back_up,
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

8
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -198,7 +198,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
destination_new = current_loc;
destination_new.offset_bearing(final_bearing, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, active, bearing_to_dest, 0.0f, ignore_bearing_change, final_margin, destination, destination_new);
Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, active, bearing_to_dest, 0.0f, ignore_bearing_change, final_margin, destination, destination_new);
return active;
}
}
@ -224,7 +224,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
destination_new.offset_bearing(chosen_bearing, distance_to_dest);
// log results
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new);
Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new);
return true;
}
@ -306,7 +306,7 @@ bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Lo
destination_new.offset_bearing_and_pitch(bearing_to_dest,pitch_delta, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new);
Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new);
return active;
}
}
@ -332,7 +332,7 @@ bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Lo
destination_new.offset_bearing_and_pitch(bearing_to_dest, chosen_pitch, distance_to_dest);
// log results
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, true, bearing_to_dest, chosen_pitch,false, best_margin, destination, destination_new);
Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, true, bearing_to_dest, chosen_pitch,false, best_margin, destination, destination_new);
return true;
}

3
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -68,6 +68,9 @@ private:
// on success returns true and updates margin
bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const;
// Logging function
void Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const;
// OA common parameters
float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle

18
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -41,13 +41,13 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
// avoidance is not required if no fences
if (!some_fences_enabled()) {
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}
// no avoidance required if destination is same as current location
if (current_loc.same_latlon_as(destination)) {
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}
@ -78,7 +78,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id);
if (!_inclusion_polygon_with_margin_ok) {
report_error(error_id);
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
}
@ -88,7 +88,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id);
if (!_exclusion_polygon_with_margin_ok) {
report_error(error_id);
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
}
@ -98,7 +98,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, error_id);
if (!_exclusion_circle_with_margin_ok) {
report_error(error_id);
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
}
@ -109,7 +109,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
if (!_polyfence_visgraph_ok) {
_shortest_path_ok = false;
report_error(error_id);
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
}
@ -125,7 +125,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_shortest_path_ok = calc_shortest_path(current_loc, destination, error_id);
if (!_shortest_path_ok) {
report_error(error_id);
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
// start from 2nd point on path (first is the original origin)
@ -160,12 +160,12 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_path_idx_returned++;
}
// log success
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, _path_numpoints, destination, destination_new);
Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, _path_numpoints, destination, destination_new);
return DIJKSTRA_STATE_SUCCESS;
}
// we have reached the destination so avoidance is no longer required
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}

3
libraries/AC_Avoidance/AP_OADijkstra.h
View File

@ -189,4 +189,7 @@ private:
AP_OADijkstra_Error _error_last_id; // last error id sent to GCS
uint32_t _error_last_report_ms; // last time an error message was sent to GCS
// Logging function
void Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const;
};

94
libraries/AC_Avoidance/LogStructure.h Normal file
View File

@ -0,0 +1,94 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_AVOIDANCE \
LOG_OA_BENDYRULER_MSG, \
LOG_OA_DIJKSTRA_MSG, \
LOG_SIMPLE_AVOID_MSG
// @LoggerMessage: OABR
// @Description: Object avoidance (Bendy Ruler) diagnostics
// @Field: TimeUS: Time since system startup
// @Field: Type: Type of BendyRuler currently active
// @Field: Act: True if Bendy Ruler avoidance is being used
// @Field: DYaw: Best yaw chosen to avoid obstacle
// @Field: Yaw: Current vehicle yaw
// @Field: DP: Desired pitch chosen to avoid obstacle
// @Field: RChg: True if BendyRuler resisted changing bearing and continued in last calculated bearing
// @Field: Mar: Margin from path to obstacle on best yaw chosen
// @Field: DLt: Destination latitude
// @Field: DLg: Destination longitude
// @Field: DAlt: Desired alt above EKF Origin
// @Field: OLt: Intermediate location chosen for avoidance
// @Field: OLg: Intermediate location chosen for avoidance
// @Field: OAlt: Intermediate alt chosen for avoidance above EKF origin
struct PACKED log_OABendyRuler {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t type;
uint8_t active;
uint16_t target_yaw;
uint16_t yaw;
uint16_t target_pitch;
bool resist_chg;
float margin;
int32_t final_lat;
int32_t final_lng;
int32_t final_alt;
int32_t oa_lat;
int32_t oa_lng;
int32_t oa_alt;
};
// @LoggerMessage: OADJ
// @Description: Object avoidance (Dijkstra) diagnostics
// @Field: TimeUS: Time since system startup
// @Field: State: Dijkstra avoidance library state
// @Field: Err: Dijkstra library error condition
// @Field: CurrPoint: Destination point in calculated path to destination
// @Field: TotPoints: Number of points in path to destination
// @Field: DLat: Destination latitude
// @Field: DLng: Destination longitude
// @Field: OALat: Object Avoidance chosen destination point latitude
// @Field: OALng: Object Avoidance chosen destination point longitude
struct PACKED log_OADijkstra {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t state;
uint8_t error_id;
uint8_t curr_point;
uint8_t tot_points;
int32_t final_lat;
int32_t final_lng;
int32_t oa_lat;
int32_t oa_lng;
};
// @LoggerMessage: SA
// @Description: Simple Avoidance messages
// @Field: TimeUS: Time since system startup
// @Field: State: True if Simple Avoidance is active
// @Field: DVelX: Desired velocity, X-Axis (Velocity before Avoidance)
// @Field: DVelY: Desired velocity, Y-Axis (Velocity before Avoidance)
// @Field: MVelX: Modified velocity, X-Axis (Velocity after Avoidance)
// @Field: MVelY: Modified velocity, Y-Axis (Velocity after Avoidance)
// @Field: Back: True if vehicle is backing away
struct PACKED log_SimpleAvoid {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t state;
float desired_vel_x;
float desired_vel_y;
float modified_vel_x;
float modified_vel_y;
uint8_t backing_up;
};
#define LOG_STRUCTURE_FROM_AVOIDANCE \
{ LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \
"OABR","QBBHHHBfLLiLLi","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s-bddd-mDUmDUm", "F-------GGBGGB" }, \
{ LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
"OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \
{ LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \
"SA", "QBffffB","TimeUS,State,DVelX,DVelY,MVelX,MVelY,Back", "sbnnnnb", "F------"},