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

AC_Avoid: reenable avoidance logging

This commit is contained in:
Rishabh 2021-05-09 23:17:47 +05:30 committed by Randy Mackay
parent e1a5d73504
commit 7e79999b86
4 changed files with 26 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -241,6 +241,23 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
if (desired_vel_cms_original != desired_vel_cms) {
_last_limit_time = AP_HAL::millis();
}
if (limits_active()) {
// log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!)
uint32_t now = AP_HAL::millis();
if ((now - _last_log_ms) > 100) {
_last_log_ms = now;
Write_SimpleAvoidance(true, desired_vel_cms_original, desired_vel_cms, backing_up);
}
} else {
// avoidance isn't active anymore
// log once so that it registers in logs
if (_last_log_ms) {
Write_SimpleAvoidance(false, desired_vel_cms_original, desired_vel_cms, backing_up);
// this makes sure logging won't run again till it is active
_last_log_ms = 0;
}
}
}
/*

2
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -205,7 +205,7 @@ private:
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;
void Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const;
// parameters
AP_Int8 _enabled;

4
libraries/AC_Avoidance/AC_Avoidance_Logging.cpp
View File

@ -46,7 +46,7 @@ void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id
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
void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const
{
const struct log_SimpleAvoid pkt{
LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG),
@ -54,8 +54,10 @@ void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector2f& desire
state : state,
desired_vel_x : desired_vel.x * 0.01f,
desired_vel_y : desired_vel.y * 0.01f,
desired_vel_z : desired_vel.z * 0.01f,
modified_vel_x : modified_vel.x * 0.01f,
modified_vel_y : modified_vel.y * 0.01f,
modified_vel_z : modified_vel.z * 0.01f,
backing_up : back_up,
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));

6
libraries/AC_Avoidance/LogStructure.h
View File

@ -71,8 +71,10 @@ struct PACKED log_OADijkstra {
// @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: DVelZ: Desired velocity, Z-Axis (Velocity before Avoidance)
// @Field: MVelX: Modified velocity, X-Axis (Velocity after Avoidance)
// @Field: MVelY: Modified velocity, Y-Axis (Velocity after Avoidance)
// @Field: MVelZ: Modified velocity, Z-Axis (Velocity after Avoidance)
// @Field: Back: True if vehicle is backing away
struct PACKED log_SimpleAvoid {
LOG_PACKET_HEADER;
@ -80,8 +82,10 @@ struct PACKED log_SimpleAvoid {
uint8_t state;
float desired_vel_x;
float desired_vel_y;
float desired_vel_z;
float modified_vel_x;
float modified_vel_y;
float modified_vel_z;
uint8_t backing_up;
};
@ -91,4 +95,4 @@ struct PACKED log_SimpleAvoid {
{ 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------"},
"SA", "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "sbnnnnnnb", "F--------"},