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

AP_Logger: Privatize AC_Avoidance Logging

zr-v5.1
Josh Henderson 4 years ago committed by Peter Barker
parent
aac9771daf
commit
c8b8ff3380
3 changed files with 3 additions and 151 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. 3
      libraries/AP_Logger/AP_Logger.h
  2. 58
      libraries/AP_Logger/LogFile.cpp
  3. 93
      libraries/AP_Logger/LogStructure.h

3
libraries/AP_Logger/AP_Logger.h
Unescape View File

@ -318,9 +318,6 @@ public: @@ -318,9 +318,6 @@ public:
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_OABendyRuler(uint8_t type, bool active, float target_yaw, float target_pitch, bool ignore_chg, float margin, const Location &final_dest, const Location &oa_dest);
void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
void Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up);
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_PSC(const Vector3f &pos_target, const Vector3f &position, const Vector3f &vel_target, const Vector3f &velocity, const Vector3f &accel_target, const float &accel_x, const float &accel_y);
void Write_PSCZ(float pos_target_z, float pos_z, float vel_desired_z, float vel_target_z, float vel_z, float accel_desired_z, float accel_target_z, float accel_z, float throttle_out);

58
libraries/AP_Logger/LogFile.cpp
Unescape View File

@ -566,64 +566,6 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points @@ -566,64 +566,6 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points
WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
}
void AP_Logger::Write_OABendyRuler(uint8_t type, bool active, float target_yaw, float target_pitch, bool resist_chg, float margin, const Location &final_dest, const Location &oa_dest)
{
int32_t oa_dest_alt, final_alt;
bool got_oa_dest = oa_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, oa_dest_alt);
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
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest)
{
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
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up)
{
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,
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::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)
{
struct log_Winch pkt{

93
libraries/AP_Logger/LogStructure.h
Unescape View File

@ -129,6 +129,7 @@ const struct MultiplierStructure log_Multipliers[] = { @@ -129,6 +129,7 @@ const struct MultiplierStructure log_Multipliers[] = {
#include <AP_Baro/LogStructure.h>
#include <AP_VisualOdom/LogStructure.h>
#include <AC_PrecLand/LogStructure.h>
#include <AC_Avoidance/LogStructure.h>
// structure used to define logging format
struct LogStructure {
@ -657,48 +658,6 @@ struct PACKED log_SRTL { @@ -657,48 +658,6 @@ struct PACKED log_SRTL {
float D;
};
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;
};
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;
};
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;
};
struct PACKED log_DSTL {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1032,35 +991,6 @@ struct PACKED log_PSCZ { @@ -1032,35 +991,6 @@ struct PACKED log_PSCZ {
// @Field: Id: character referenced by FMTU
// @Field: Mult: numeric multiplier
// @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
// @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
// @LoggerMessage: OF
// @Description: Optical flow sensor data
// @Field: TimeUS: Time since system startup
@ -1245,16 +1175,6 @@ struct PACKED log_PSCZ { @@ -1245,16 +1175,6 @@ struct PACKED log_PSCZ {
// @Field: E: point associated with most recent action (East component)
// @Field: D: point associated with most recent action (Down component)
// @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
// @LoggerMessage: TERR
// @Description: Terrain database infomration
// @Field: TimeUS: Time since system startup
@ -1385,12 +1305,7 @@ LOG_STRUCTURE_FROM_CAMERA \ @@ -1385,12 +1305,7 @@ LOG_STRUCTURE_FROM_CAMERA \
"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), \
"SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
{ 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------"}, \
LOG_STRUCTURE_FROM_AVOIDANCE \
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
"SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
@ -1558,9 +1473,7 @@ enum LogMessages : uint8_t { @@ -1558,9 +1473,7 @@ enum LogMessages : uint8_t {
LOG_ERROR_MSG,
LOG_ADSB_MSG,
LOG_ARM_DISARM_MSG,
LOG_OA_BENDYRULER_MSG,
LOG_OA_DIJKSTRA_MSG,
LOG_SIMPLE_AVOID_MSG,
LOG_IDS_FROM_AVOIDANCE,
LOG_WINCH_MSG,
LOG_PSC_MSG,
LOG_PSCZ_MSG,

Write Preview
Loading…
Cancel
Save