void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat)
{
MAV_COLLISION_THREAT_LEVEL new_threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
MAV_COLLISION_ACTION action = MAV_COLLISION_ACTION_NONE;
if (threat != nullptr) {
new_threat_level = threat->threat_level;
if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
action = (MAV_COLLISION_ACTION)_fail_action.get();
}
}
uint32_t now = AP_HAL::millis();
if (new_threat_level != _threat_level) {
// transition to higher states immediately, recovery to lower states more slowly
if (((now - _last_state_change_ms) > AP_AVOIDANCE_STATE_RECOVERY_TIME_MS) || (new_threat_level > _threat_level)) {
// handle recovery from high threat level
if (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
handle_recovery(_fail_recovery);
_latest_action = MAV_COLLISION_ACTION_NONE;
}
// update state
_last_state_change_ms = now;
_threat_level = new_threat_level;
}
}
// handle ongoing threat by calling vehicle specific handler
if ((threat != nullptr) && (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) && (action > MAV_COLLISION_ACTION_REPORT)) {
_latest_action = handle_avoidance(threat, action);
}
}
/*
* de-initialize and free up some memory
*/
void AP_Avoidance::deinit(void)
{
if (_obstacles != nullptr) {
delete [] _obstacles;
_obstacles = nullptr;
_obstacles_allocated = 0;
handle_recovery(AP_AVOIDANCE_RECOVERY_RTL);
}
_obstacle_count = 0;
}
/*---------------------------------------------------------------------------*
* main loop
*---------------------------------------------------------------------------*/
static void
mloop(
#ifdef I4B_EXTERNAL_MONITOR
int localmonitor
#ifndef I4B_NOTCPIP_MONITOR
, int remotemonitor
#endif
#endif
)
{
fd_set set;
struct timeval timeout;
int ret;
int high_selfd;
/* go into loop */
dolog(LL_DMN, "i4b isdn daemon started (pid = %d)", getpid());
for(;;)
{
FD_ZERO(&set);
#ifdef USE_CURSES
if(do_fullscreen)
FD_SET(fileno(stdin), &set);
#endif
FD_SET(isdnfd, &set);
high_selfd = isdnfd;
#ifdef I4B_EXTERNAL_MONITOR
if(do_monitor)
{
if (localmonitor != -1) {
/* always watch for new connections */
FD_SET(localmonitor, &set);
if(localmonitor > high_selfd)
high_selfd = localmonitor;
}
#ifndef I4B_NOTCPIP_MONITOR
if (remotemonitor != -1) {
FD_SET(remotemonitor, &set);
if(remotemonitor > high_selfd)
high_selfd = remotemonitor;
}
#endif
/* if there are client connections, let monitor module
* enter them into the fdset */
if(accepted)
{
monitor_prepselect(&set, &high_selfd);
}
}
#endif
timeout.tv_sec = 1;
timeout.tv_usec = 0;
ret = select(high_selfd + 1, &set, NULL, NULL, &timeout);
if(ret > 0)
{
if(FD_ISSET(isdnfd, &set))
isdnrdhdl();
#ifdef USE_CURSES
if(FD_ISSET(fileno(stdin), &set))
kbdrdhdl();
#endif
#ifdef I4B_EXTERNAL_MONITOR
if(do_monitor)
{
if(localmonitor != -1 && FD_ISSET(localmonitor, &set))
monitor_handle_connect(localmonitor, 1);
#ifndef I4B_NOTCPIP_MONITOR
if(remotemonitor != -1 && FD_ISSET(remotemonitor, &set))
monitor_handle_connect(remotemonitor, 0);
#endif
if(accepted)
monitor_handle_input(&set);
}
#endif
}
else if(ret == -1)
{
if(errno != EINTR)
{
dolog(LL_ERR, "mloop: ERROR, select error on isdn device, errno = %d!", errno);
error_exit(1, "mloop: ERROR, select error on isdn device, errno = %d!", errno);
}
}
/* handle timeout and recovery */
handle_recovery();
}
}
