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

AP_Avoidance: use an enumeration for the AP_Avoidance recovery action

This commit is contained in:
Peter Barker 2020-08-16 12:12:52 +10:00 committed by Peter Barker
parent ebc28d817f
commit 36e6ce6e5e
2 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Avoidance/AP_Avoidance.cpp
View File

@ -15,7 +15,7 @@ extern const AP_HAL::HAL& hal;
#define AP_AVOIDANCE_WARN_DISTANCE_Z_DEFAULT 300 #define AP_AVOIDANCE_WARN_DISTANCE_Z_DEFAULT 300
#define AP_AVOIDANCE_FAIL_DISTANCE_XY_DEFAULT 300 #define AP_AVOIDANCE_FAIL_DISTANCE_XY_DEFAULT 300
#define AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT 100 #define AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT 100
#define AP_AVOIDANCE_RECOVERY_DEFAULT AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER #define AP_AVOIDANCE_RECOVERY_DEFAULT RecoveryAction::RESUME_IF_AUTO_ELSE_LOITER
#define AP_AVOIDANCE_FAIL_ACTION_DEFAULT MAV_COLLISION_ACTION_REPORT #define AP_AVOIDANCE_FAIL_ACTION_DEFAULT MAV_COLLISION_ACTION_REPORT
#else // APM_BUILD_TYPE(APM_BUILD_ArduCopter), Rover, Boat #else // APM_BUILD_TYPE(APM_BUILD_ArduCopter), Rover, Boat
#define AP_AVOIDANCE_WARN_TIME_DEFAULT 30 #define AP_AVOIDANCE_WARN_TIME_DEFAULT 30
@ -24,7 +24,7 @@ extern const AP_HAL::HAL& hal;
#define AP_AVOIDANCE_WARN_DISTANCE_Z_DEFAULT 300 #define AP_AVOIDANCE_WARN_DISTANCE_Z_DEFAULT 300
#define AP_AVOIDANCE_FAIL_DISTANCE_XY_DEFAULT 100 #define AP_AVOIDANCE_FAIL_DISTANCE_XY_DEFAULT 100
#define AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT 100 #define AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT 100
#define AP_AVOIDANCE_RECOVERY_DEFAULT AP_AVOIDANCE_RECOVERY_RTL #define AP_AVOIDANCE_RECOVERY_DEFAULT RecoveryAction::RTL
#define AP_AVOIDANCE_FAIL_ACTION_DEFAULT MAV_COLLISION_ACTION_REPORT #define AP_AVOIDANCE_FAIL_ACTION_DEFAULT MAV_COLLISION_ACTION_REPORT
#endif #endif
@ -64,7 +64,7 @@ const AP_Param::GroupInfo AP_Avoidance::var_info[] = {
// @Description: Determines what the aircraft will do after a fail event is resolved // @Description: Determines what the aircraft will do after a fail event is resolved
// @Values: 0:Remain in AVOID_ADSB,1:Resume previous flight mode,2:RTL,3:Resume if AUTO else Loiter // @Values: 0:Remain in AVOID_ADSB,1:Resume previous flight mode,2:RTL,3:Resume if AUTO else Loiter
// @User: Advanced // @User: Advanced
AP_GROUPINFO("F_RCVRY", 4, AP_Avoidance, _fail_recovery, AP_AVOIDANCE_RECOVERY_DEFAULT), AP_GROUPINFO("F_RCVRY", 4, AP_Avoidance, _fail_recovery, uint8_t(AP_AVOIDANCE_RECOVERY_DEFAULT)),
// @Param: OBS_MAX // @Param: OBS_MAX
// @DisplayName: Maximum number of obstacles to track // @DisplayName: Maximum number of obstacles to track
@ -168,7 +168,7 @@ void AP_Avoidance::deinit(void)
delete [] _obstacles; delete [] _obstacles;
_obstacles = nullptr; _obstacles = nullptr;
_obstacles_allocated = 0; _obstacles_allocated = 0;
handle_recovery(AP_AVOIDANCE_RECOVERY_RTL); handle_recovery(RecoveryAction::RTL);
} }
_obstacle_count = 0; _obstacle_count = 0;
} }
@ -554,7 +554,7 @@ void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat)
if (((now - _last_state_change_ms) > AP_AVOIDANCE_STATE_RECOVERY_TIME_MS) || (new_threat_level > _threat_level)) { if (((now - _last_state_change_ms) > AP_AVOIDANCE_STATE_RECOVERY_TIME_MS) || (new_threat_level > _threat_level)) {
// handle recovery from high threat level // handle recovery from high threat level
if (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) { if (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
handle_recovery(_fail_recovery); handle_recovery(RecoveryAction(_fail_recovery.get()));
_latest_action = MAV_COLLISION_ACTION_NONE; _latest_action = MAV_COLLISION_ACTION_NONE;
} }

16
libraries/AP_Avoidance/AP_Avoidance.h
View File

@ -27,12 +27,6 @@
#include <AP_ADSB/AP_ADSB.h> #include <AP_ADSB/AP_ADSB.h>
// F_RCVRY possible parameter values
#define AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB 0
#define AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE 1
#define AP_AVOIDANCE_RECOVERY_RTL 2
#define AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER 3
#define AP_AVOIDANCE_STATE_RECOVERY_TIME_MS 2000 // we will not downgrade state any faster than this (2 seconds) #define AP_AVOIDANCE_STATE_RECOVERY_TIME_MS 2000 // we will not downgrade state any faster than this (2 seconds)
#define AP_AVOIDANCE_ESCAPE_TIME_SEC 2 // vehicle runs from thread for 2 seconds #define AP_AVOIDANCE_ESCAPE_TIME_SEC 2 // vehicle runs from thread for 2 seconds
@ -52,6 +46,14 @@ public:
return _singleton; return _singleton;
} }
// F_RCVRY possible parameter values:
enum class RecoveryAction {
REMAIN_IN_AVOID_ADSB = 0,
RESUME_PREVIOUS_FLIGHTMODE = 1,
RTL = 2,
RESUME_IF_AUTO_ELSE_LOITER = 3,
};
// obstacle class to hold latest information for a known obstacles // obstacle class to hold latest information for a known obstacles
class Obstacle { class Obstacle {
public: public:
@ -114,7 +116,7 @@ protected:
// recover after all threats have cleared. child classes must override this method // recover after all threats have cleared. child classes must override this method
// recovery_action is from F_RCVRY parameter // recovery_action is from F_RCVRY parameter
virtual void handle_recovery(uint8_t recovery_action) = 0; virtual void handle_recovery(RecoveryAction recovery_action) = 0;
uint32_t _last_state_change_ms = 0; uint32_t _last_state_change_ms = 0;
MAV_COLLISION_THREAT_LEVEL _threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; MAV_COLLISION_THREAT_LEVEL _threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;