bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
// Geofence function checks against home altitude amsl
missionitem.altitude = missionitem.altitude_is_relative
? missionitem.altitude + home_alt
: missionitem.altitude;
if (MissionBlock::item_contains_position(&missionitem) &&
!geofence.inside(missionitem)) {
mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1);
return false;
}
}
}
return true;
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
return false;
}
}
}
return true;
}
void
Navigator::parameters_update()
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
param_get(_parameter_handles.land_alt, &(_parameters.land_alt));
param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay));
_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
_geofence.updateParams();
}
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::load_fence_from_file(const char *filename)
{
_geofence.loadFromFile(filename);
}
void Navigator::add_fence_point(int argc, char *argv[])
{
_geofence.addPoint(argc, argv);
}