From b63a704300c3e8d56ef7877b3299cae576547492 Mon Sep 17 00:00:00 2001 From: Peter Burke Date: Mon, 25 Mar 2024 22:32:00 -0700 Subject: [PATCH] change to xyz distance but has bug crashes code --- ArduCopter/dumpcore.sh_arducopter.183560.out | 6 +++ ArduCopter/dumpcore.sh_arducopter.186040.out | 6 +++ ArduCopter/dumpstack.sh_arducopter.183560.out | 6 +++ ArduCopter/dumpstack.sh_arducopter.186040.out | 6 +++ libraries/AP_Avoidance/AP_Avoidance.cpp | 52 +++++++++++++++++-- libraries/AP_Avoidance/AP_Avoidance.h | 7 +++ 6 files changed, 80 insertions(+), 3 deletions(-) create mode 100644 ArduCopter/dumpcore.sh_arducopter.183560.out create mode 100644 ArduCopter/dumpcore.sh_arducopter.186040.out create mode 100644 ArduCopter/dumpstack.sh_arducopter.183560.out create mode 100644 ArduCopter/dumpstack.sh_arducopter.186040.out diff --git a/ArduCopter/dumpcore.sh_arducopter.183560.out b/ArduCopter/dumpcore.sh_arducopter.183560.out new file mode 100644 index 0000000000000..e9d147ddd1aff --- /dev/null +++ b/ArduCopter/dumpcore.sh_arducopter.183560.out @@ -0,0 +1,6 @@ +Could not attach to process. If your uid matches the uid of the target +process, check the setting of /proc/sys/kernel/yama/ptrace_scope, or try +again as the root user. For more details, see /etc/sysctl.d/10-ptrace.conf +ptrace: Inappropriate ioctl for device. +/tmp/gdb.183997:2: Error in sourced command file: +You can't do that without a process to debug. diff --git a/ArduCopter/dumpcore.sh_arducopter.186040.out b/ArduCopter/dumpcore.sh_arducopter.186040.out new file mode 100644 index 0000000000000..cead010b311a9 --- /dev/null +++ b/ArduCopter/dumpcore.sh_arducopter.186040.out @@ -0,0 +1,6 @@ +Could not attach to process. If your uid matches the uid of the target +process, check the setting of /proc/sys/kernel/yama/ptrace_scope, or try +again as the root user. For more details, see /etc/sysctl.d/10-ptrace.conf +ptrace: Inappropriate ioctl for device. +/tmp/gdb.187452:2: Error in sourced command file: +You can't do that without a process to debug. diff --git a/ArduCopter/dumpstack.sh_arducopter.183560.out b/ArduCopter/dumpstack.sh_arducopter.183560.out new file mode 100644 index 0000000000000..2e2096d13f415 --- /dev/null +++ b/ArduCopter/dumpstack.sh_arducopter.183560.out @@ -0,0 +1,6 @@ +Could not attach to process. If your uid matches the uid of the target +process, check the setting of /proc/sys/kernel/yama/ptrace_scope, or try +again as the root user. For more details, see /etc/sysctl.d/10-ptrace.conf +ptrace: Inappropriate ioctl for device. +/tmp/gdb.183975:2: Error in sourced command file: +No stack. diff --git a/ArduCopter/dumpstack.sh_arducopter.186040.out b/ArduCopter/dumpstack.sh_arducopter.186040.out new file mode 100644 index 0000000000000..6982590743cec --- /dev/null +++ b/ArduCopter/dumpstack.sh_arducopter.186040.out @@ -0,0 +1,6 @@ +Could not attach to process. If your uid matches the uid of the target +process, check the setting of /proc/sys/kernel/yama/ptrace_scope, or try +again as the root user. For more details, see /etc/sysctl.d/10-ptrace.conf +ptrace: Inappropriate ioctl for device. +/tmp/gdb.187429:2: Error in sourced command file: +No stack. diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp index a9da26ea106ef..3c5e7a6ad1a9f 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.cpp +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -468,6 +468,39 @@ void AP_Avoidance::update_threat_level(const Location &my_loc, } } +void AP_Avoidance::update_threat_level_ODID(const Location &my_loc, + const Vector3f &my_vel, + AP_Avoidance::Obstacle &obstacle) +{ + + Location &obstacle_loc = obstacle._location; + // Vector3f &obstacle_vel = obstacle._velocity; + + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + + const uint32_t obstacle_age = AP_HAL::millis() - obstacle.timestamp_ms; + + // float closest_xy = closest_approach_xy(my_loc, my_vel, obstacle_loc, obstacle_vel, _fail_time_horizon + obstacle_age/1000); + + double instantaneous_xy = my_loc.get_distance(obstacle_loc); + double instantaneous_z = my_loc.get_alt_distance(obstacle_loc,instantaneous_xy); + + + + if ((instantaneous_xy < _fail_distance_xy) || (instantaneous_z < _fail_distance_z)) { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_HIGH; + } else { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_LOW; + } + + + // If we haven't heard from a vehicle then assume it is no threat + if (obstacle_age > MAX_OBSTACLE_AGE_MS) { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + } + +} + MAV_COLLISION_THREAT_LEVEL AP_Avoidance::current_threat_level() const { if (_obstacles == nullptr) { return MAV_COLLISION_THREAT_LEVEL_NONE; @@ -563,7 +596,8 @@ void AP_Avoidance::check_for_threats() const uint32_t obstacle_age = AP_HAL::millis() - obstacle.timestamp_ms; debug("i=%d src_id=%d timestamp=%u age=%d", i, obstacle.src_id, obstacle.timestamp_ms, obstacle_age); - update_threat_level(my_loc, my_vel, obstacle); + //update_threat_level(my_loc, my_vel, obstacle); + update_threat_level_ODID(my_loc, my_vel, obstacle); debug(" threat-level=%d", obstacle.threat_level); // ignore any really old data: @@ -626,13 +660,25 @@ void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat) if (threat != nullptr) { - // xxx need a timer here, update to OSD is too often at 10 Hz... + // need a timer here, update to OSD is too often at 10 Hz... // pseudo code: // now=millis() // if now-_time_of_last_GCS_nearest_drone_update > 1000 ms then post new message... uint32_t now = AP_HAL::millis(); if ( (now - time_of_last_GCS_nearest_drone_update ) > 1000){ - GCS_SEND_TEXT(MAV_SEVERITY_INFO,"xy: %f,z: %f", threat->closest_approach_xy, threat->closest_approach_z); + // xxx want to send instantaneous xy and z distance, from the threat object.... + const AP_AHRS &_ahrs = AP::ahrs(); + Location my_loc; + if (!_ahrs.get_location(my_loc)) { + // if we don't know our own location we can't determine any threat level + return; + } + + double instantaneous_xy = my_loc.get_distance(threat->_location); + double instantaneous_z = my_loc.get_alt_distance(threat->_location,instantaneous_xy); + + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"xy: %f,z: %f", instantaneous_xy, instantaneous_z); + //GCS_SEND_TEXT(MAV_SEVERITY_INFO,"xy: %f,z: %f", threat->closest_approach_xy, threat->closest_approach_z); time_of_last_GCS_nearest_drone_update=now; } // double closest_approach_xy = threat->closest_approach_xy; diff --git a/libraries/AP_Avoidance/AP_Avoidance.h b/libraries/AP_Avoidance/AP_Avoidance.h index 76645d9bad1ba..ea7feb2df3669 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.h +++ b/libraries/AP_Avoidance/AP_Avoidance.h @@ -186,6 +186,13 @@ class AP_Avoidance { const Vector3f &my_vel, AP_Avoidance::Obstacle &obstacle); + // collision avoidance using ODID RID, only use proximity (e.g. drone within a certain distance), not projected flight path + void update_threat_level_ODID(const Location &my_loc, + const Vector3f &my_vel, + AP_Avoidance::Obstacle &obstacle); + + + // calls into the AP_ADSB library to retrieve vehicle data void get_adsb_samples(); void get_odid_samples();