void Navigator::task_main() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() != OK) { mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); } } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[2] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _vehicle_command_sub; fds[1].events = POLLIN; bool global_pos_available_once = false; while (!_task_should_exit) { /* wait for up to 200ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { global_pos_available_once = false; PX4_WARN("navigator: global position timeout"); } /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_WARN("nav: poll error %d, %d", pret, errno); continue; } else { /* success, global pos was available */ global_pos_available_once = true; } perf_begin(_loop_perf); bool updated; /* gps updated */ orb_check(_gps_pos_sub, &updated); if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); if (updated) { params_update(); updateParams(); } /* vehicle control mode updated */ orb_check(_control_mode_sub, &updated); if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { fw_pos_ctrl_status_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; } else { rep->current.yaw = NAN; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.yaw = cmd.param4; if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both rep->current.lat = NAN; rep->current.lon = NAN; } rep->current.alt = cmd.param7; rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { warnx("navigator: got pause/continue command"); } } /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: if (_fw_pos_ctrl_status.abort_landing) { // pos controller aborted landing, requests loiter // above landing waypoint _navigation_mode = &_loiter; _pos_sp_triplet_published_invalid_once = false; } else { _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_rcloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_rcloss_act.get() == 4) { _navigation_mode = &_rcLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_act.get() == 1) { _navigation_mode = &_loiter; } else if (_param_datalinkloss_act.get() == 3) { _navigation_mode = &_land; } else if (_param_datalinkloss_act.get() == 4) { _navigation_mode = &_dataLinkLoss; } else { /* if == 2 or unknown, RTL */ _navigation_mode = &_rtl; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_follow_target; break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; return; }
void Navigator::task_main() { /* inform about start */ warnx("Initializing.."); fflush(stdout); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[navigator] started"); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); } /* * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; fds[2].fd = _home_pos_sub; fds[2].events = POLLIN; fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; fds[4].fd = _offboard_mission_sub; fds[4].events = POLLIN; fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; fds[7].fd = _control_mode_sub; fds[7].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } /* vehicle control mode updated */ if (fds[7].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); /* evaluate state machine from commander and set the navigator mode accordingly */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { bool stick_mode = false; if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } stick_mode = true; } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { request_loiter_or_ready(); stick_mode = true; } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { request_mission_if_available(); stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ request_mission_if_available(); stick_mode = true; } } } if (!stick_mode) { if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { /* commander requested new navigation mode, try to set it */ _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { case NAV_STATE_NONE: /* nothing to do */ break; case NAV_STATE_LOITER: request_loiter_or_ready(); break; case NAV_STATE_MISSION: request_mission_if_available(); break; case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } break; case NAV_STATE_LAND: dispatch(EVENT_LAND_REQUESTED); break; default: warnx("ERROR: Requested navigation state not supported"); break; } } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ if (myState == NAV_STATE_NONE) { request_mission_if_available(); } } } } else { /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } /* parameters updated */ if (fds[0].revents & POLLIN) { parameters_update(); /* note that these new parameters won't be in effect until a mission triplet is published again */ } /* navigation capabilities updated */ if (fds[3].revents & POLLIN) { navigation_capabilities_update(); } /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); /* publish position setpoint triplet on each position update if navigator active */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { _pos_sp_triplet_updated = true; if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { /* got global position when landing, update setpoint */ start_land(); } _global_pos_valid = _global_pos.global_valid; /* check if waypoint has been reached in MISSION, RTL and LAND modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { if (check_mission_item_reached()) { on_mission_item_reached(); } } } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; publish_position_setpoint_triplet(); } /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); prevState = myState; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; _exit(0); }
void Navigator::task_main() { /* inform about start */ warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(); navigation_capabilities_update(); params_update(); /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; fds[5].fd = _param_update_sub; fds[5].events = POLLIN; fds[6].fd = _sensor_combined_sub; fds[6].events = POLLIN; fds[7].fd = _gps_pos_sub; fds[7].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ continue; } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* sensors combined updated */ if (fds[6].revents & POLLIN) { sensor_combined_update(); } /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); updateParams(); } /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ if (fds[3].revents & POLLIN) { vehicle_status_update(); } /* navigation capabilities updated */ if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } /* home position updated */ if (fds[1].revents & POLLIN) { home_position_update(); } /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; _exit(0); }
void Navigator::task_main() { /* inform about start */ warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); params_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; fds[5].fd = _param_update_sub; fds[5].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ continue; } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); updateParams(); } /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ if (fds[3].revents & POLLIN) { vehicle_status_update(); } /* navigation capabilities updated */ if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } /* home position updated */ if (fds[1].revents & POLLIN) { home_position_update(); } /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { if (_navigation_mode == _navigation_mode_array[i]) { _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); } else { _navigation_mode_array[i]->on_inactive(); } } /* if nothing is running, set position setpoint triplet invalid */ if (_navigation_mode == nullptr) { _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _update_triplet = true; } if (_update_triplet) { publish_position_setpoint_triplet(); _update_triplet = false; } perf_end(_loop_perf); } warnx("exiting."); _navigator_task = -1; _exit(0); }