Josh Henderson
4 years ago
committed by
Peter Barker
7 changed files with 178 additions and 13 deletions
@ -0,0 +1,62 @@
@@ -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)); |
||||
} |
||||
@ -0,0 +1,94 @@
@@ -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------"}, |
||||
Loading…
Reference in new issue