Ejemplo n.º 1
0
/**
 * Creates a fake traffic measurement with supplied parameters.
 *
 */
void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
			     float altitude_diff, float hor_velocity, float ver_velocity)
{
	double lat, lon;
	waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat,
					   &lon);
	float alt = get_global_position()->alt + altitude_diff;

	// float vel_n = get_global_position()->vel_n;
	// float vel_e = get_global_position()->vel_e;
	// float vel_d = get_global_position()->vel_d;

	transponder_report_s tr = {};
	tr.timestamp = hrt_absolute_time();
	tr.ICAO_address = 1234;
	tr.lat = lat; // Latitude, expressed as degrees
	tr.lon = lon; // Longitude, expressed as degrees
	tr.altitude_type = 0;
	tr.altitude = alt;
	tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians
	tr.hor_velocity	= hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s
	tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up
	strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign));
	tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum
	tr.tslc = 2; // Time since last communication in seconds
	tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
		   transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
		   transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
		   transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields
	tr.squawk = 6667;

	orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH);
	(void)orb_unadvertise(h);
}
Ejemplo n.º 2
0
void GradientEdit::_show_color_picker() {
	if (grabbed == -1)
		return;
	picker->set_pick_color(points[grabbed].color);
	popup->set_position(get_global_position() - popup->get_combined_minimum_size());
	popup->popup();
}
Ejemplo n.º 3
0
void Popup::_fix_size() {

#if 0
	Point2 pos = get_position();
	Size2 size = get_size();
	Point2 window_size = window==this ? get_parent_area_size()  :window->get_size();
#else

	Point2 pos = get_global_position();
	Size2 size = get_size();
	Point2 window_size = get_viewport_rect().size;

#endif
	if (pos.x + size.width > window_size.width)
		pos.x = window_size.width - size.width;
	if (pos.x < 0)
		pos.x = 0;

	if (pos.y + size.height > window_size.height)
		pos.y = window_size.height - size.height;
	if (pos.y < 0)
		pos.y = 0;
#if 0
	if (pos!=get_pos())
		set_position(pos);
#else
	if (pos != get_position())
		set_global_position(pos);

#endif
}
Ejemplo n.º 4
0
void GradientEdit::_show_color_picker() {
	if (grabbed == -1)
		return;
	picker->set_pick_color(points[grabbed].color);
	Size2 minsize = popup->get_combined_minimum_size();
	bool show_above = false;
	if (get_global_position().y + get_size().y + minsize.y > get_viewport_rect().size.y) {
		show_above = true;
	}
	if (show_above) {
		popup->set_position(get_global_position() - Vector2(0, minsize.y));
	} else {
		popup->set_position(get_global_position() + Vector2(0, get_size().y));
	}
	popup->popup();
}
Ejemplo n.º 5
0
void ColorPickerButton::pressed() {

	_update_picker();
	popup->set_position(get_global_position() - picker->get_combined_minimum_size());
	popup->popup();
	picker->set_focus_on_line_edit();
}
Ejemplo n.º 6
0
void OptionButton::pressed() {

	Size2 size = get_size();
	popup->set_global_position(get_global_position() + Size2(0, size.height));
	popup->set_size(Size2(size.width, 0));

	popup->popup();
}
Ejemplo n.º 7
0
void ColorPickerButton::pressed() {

	Size2 ms = Size2(300, picker->get_combined_minimum_size().height + 10);
	popup->set_position(get_global_position() - Size2(0, ms.height));
	popup->set_size(ms);
	popup->popup();
	picker->set_focus_on_line_edit();
}
Ejemplo n.º 8
0
void MenuButton::pressed() {

	emit_signal("about_to_show");
	Size2 size = get_size();

	Point2 gp = get_global_position();
	popup->set_global_position(gp + Size2(0, size.height * get_global_transform().get_scale().y));
	popup->set_size(Size2(size.width, 0));
	popup->set_scale(get_global_transform().get_scale());
	popup->set_parent_rect(Rect2(Point2(gp - popup->get_global_position()), get_size()));
	popup->popup();
}
Ejemplo n.º 9
0
void MenuButton::pressed() {

	emit_signal("about_to_show");
	Size2 size = get_size();

	Point2 gp = get_global_position();
	popup->set_global_position(gp + Size2(0, size.height));
	popup->set_size(Size2(size.width, 0));
	popup->set_parent_rect(Rect2(Point2(gp - popup->get_global_position()), get_size()));
	popup->popup();
	popup->set_invalidate_click_until_motion();
}
Ejemplo n.º 10
0
void EditorAudioBus::_gui_input(const Ref<InputEvent> &p_event) {

	Ref<InputEventKey> k = p_event;
	if (k.is_valid() && k->is_pressed() && k->get_scancode() == KEY_DELETE && !k->is_echo()) {
		accept_event();
		emit_signal("delete_request");
	}

	Ref<InputEventMouseButton> mb = p_event;
	if (mb.is_valid() && mb->get_button_index() == 2 && mb->is_pressed()) {

		Vector2 pos = Vector2(mb->get_position().x, mb->get_position().y);
		bus_popup->set_position(get_global_position() + pos);
		bus_popup->popup();
	}
}
Ejemplo n.º 11
0
void EditorPath::_gui_input(const Ref<InputEvent> &p_event) {

	Ref<InputEventMouseButton> mb = p_event;
	if (mb.is_valid() && mb->get_button_index() == BUTTON_LEFT && mb->is_pressed()) {

		Object *obj = ObjectDB::get_instance(history->get_path_object(history->get_path_size() - 1));
		if (!obj)
			return;

		objects.clear();
		popup->clear();
		_add_children_to_popup(obj);
		popup->set_position(get_global_position() + Vector2(0, get_size().height));
		popup->set_size(Size2(get_size().width, 1));
		popup->popup();
	}
}
Ejemplo n.º 12
0
void PopupMenu::_scroll(float p_factor, const Point2 &p_over) {

	const float global_y = get_global_position().y;

	int vseparation = get_constant("vseparation");
	Ref<Font> font = get_font("font");

	float dy = (vseparation + font->get_height()) * 3 * p_factor;
	if (dy > 0 && global_y < 0)
		dy = MIN(dy, -global_y - 1);
	else if (dy < 0 && global_y + get_size().y > get_viewport_rect().size.y)
		dy = -MIN(-dy, global_y + get_size().y - get_viewport_rect().size.y - 1);
	set_position(get_position() + Vector2(0, dy));

	Ref<InputEventMouseMotion> ie;
	ie.instance();
	ie->set_position(p_over - Vector2(0, dy));
	_gui_input(ie);
}
Ejemplo n.º 13
0
void Parser::bake_meshes_recursive(FbxNode * node, FbxAnimLayer * animation_layer)
{
  FbxPose * pose = NULL;
  FbxTime current_time;
  FbxAMatrix parent_global_position;

  FbxAMatrix global_position = get_global_position(node, current_time, pose, &parent_global_position);

  FbxNodeAttribute* node_attribute = node->GetNodeAttribute();

  if(node_attribute) {
    if(node_attribute->GetAttributeType() == FbxNodeAttribute::eMesh) {
      FbxMesh * mesh = node->GetMesh();
        
      FbxAMatrix geometry_offset = get_geometry(node);
      FbxAMatrix global_offset_position = global_position * geometry_offset;

      FbxVector4* control_points = mesh->GetControlPoints();
      bake_global_positions(control_points, mesh->GetControlPointsCount(), global_offset_position);

      if(mesh && !mesh->GetUserDataPtr()) {
        VBOMesh * mesh_cache = new VBOMesh;

        if(mesh_cache->initialize(mesh)) {
          bake_mesh_deformations(mesh, mesh_cache, current_time, animation_layer, global_offset_position, pose);

          meshes->push_back(mesh_cache);
        }
      }
    }
  }

  const int node_child_count = node->GetChildCount();

  for(int node_child_index = 0; node_child_index < node_child_count; ++node_child_index) {
    bake_meshes_recursive(node->GetChild(node_child_index), animation_layer);
  }
}
Ejemplo n.º 14
0
Ref<InputEvent> InputEventMouseMotion::xformed_by(const Transform2D &p_xform, const Vector2 &p_local_ofs) const {

	Vector2 g = p_xform.xform(get_global_position());
	Vector2 l = p_xform.xform(get_position() + p_local_ofs);
	Vector2 r = p_xform.basis_xform(get_relative());
	Vector2 s = p_xform.basis_xform(get_speed());

	Ref<InputEventMouseMotion> mm;
	mm.instance();

	mm->set_device(get_device());

	mm->set_modifiers_from_event(this);

	mm->set_position(l);
	mm->set_global_position(g);

	mm->set_button_mask(get_button_mask());
	mm->set_relative(r);
	mm->set_speed(s);

	return mm;
}
Ejemplo n.º 15
0
Ref<InputEvent> InputEventMouseButton::xformed_by(const Transform2D &p_xform, const Vector2 &p_local_ofs) const {

	Vector2 g = p_xform.xform(get_global_position());
	Vector2 l = p_xform.xform(get_position() + p_local_ofs);

	Ref<InputEventMouseButton> mb;
	mb.instance();

	mb->set_device(get_device());

	mb->set_modifiers_from_event(this);

	mb->set_position(l);
	mb->set_global_position(g);

	mb->set_button_mask(get_button_mask());
	mb->set_pressed(pressed);
	mb->set_doubleclick(doubleclick);
	mb->set_factor(factor);
	mb->set_button_index(button_index);

	return mb;
}
Ejemplo n.º 16
0
void PopupMenu::_activate_submenu(int over) {

	Node *n = get_node(items[over].submenu);
	ERR_EXPLAIN("item subnode does not exist: " + items[over].submenu);
	ERR_FAIL_COND(!n);
	Popup *pm = n->cast_to<Popup>();
	ERR_EXPLAIN("item subnode is not a Popup: " + items[over].submenu);
	ERR_FAIL_COND(!pm);
	if (pm->is_visible_in_tree())
		return; //already visible!

	Point2 p = get_global_position();
	Rect2 pr(p, get_size());
	Ref<StyleBox> style = get_stylebox("panel");

	Point2 pos = p + Point2(get_size().width, items[over]._ofs_cache - style->get_offset().y);
	Size2 size = pm->get_size();
	// fix pos
	if (pos.x + size.width > get_viewport_rect().size.width)
		pos.x = p.x - size.width;

	pm->set_position(pos);
	pm->popup();

	PopupMenu *pum = pm->cast_to<PopupMenu>();
	if (pum) {

		pr.position -= pum->get_global_position();
		pum->clear_autohide_areas();
		pum->add_autohide_area(Rect2(pr.position.x, pr.position.y, pr.size.x, items[over]._ofs_cache));
		if (over < items.size() - 1) {
			int from = items[over + 1]._ofs_cache;
			pum->add_autohide_area(Rect2(pr.position.x, pr.position.y + from, pr.size.x, pr.size.y - from));
		}
	}
}
Ejemplo n.º 17
0
//Compute the transform matrix that the cluster will transform the vertex.
void compute_cluster_deformation(FbxAMatrix& pGlobalPosition,
                                 FbxMesh* pMesh,
                                 FbxCluster* pCluster,
                                 FbxAMatrix& pVertexTransformMatrix,
                                 FbxTime pTime,
                                 FbxPose* pPose)
{
  FbxCluster::ELinkMode lClusterMode = pCluster->GetLinkMode();

  FbxAMatrix lReferenceGlobalInitPosition;
  FbxAMatrix lReferenceGlobalCurrentPosition;
  FbxAMatrix lAssociateGlobalInitPosition;
  FbxAMatrix lAssociateGlobalCurrentPosition;
  FbxAMatrix lClusterGlobalInitPosition;
  FbxAMatrix lClusterGlobalCurrentPosition;

  FbxAMatrix lReferenceGeometry;
  FbxAMatrix lAssociateGeometry;
  FbxAMatrix lClusterGeometry;

  FbxAMatrix lClusterRelativeInitPosition;
  FbxAMatrix lClusterRelativeCurrentPositionInverse;

  if(lClusterMode == FbxCluster::eAdditive && pCluster->GetAssociateModel()) {
    pCluster->GetTransformAssociateModelMatrix(lAssociateGlobalInitPosition);
    // Geometric transform of the model
    lAssociateGeometry = get_geometry(pCluster->GetAssociateModel());
    lAssociateGlobalInitPosition *= lAssociateGeometry;
    lAssociateGlobalCurrentPosition = get_global_position(pCluster->GetAssociateModel(), pTime, pPose);

    pCluster->GetTransformMatrix(lReferenceGlobalInitPosition);
    // Multiply lReferenceGlobalInitPosition by Geometric Transformation
    lReferenceGeometry = get_geometry(pMesh->GetNode());
    lReferenceGlobalInitPosition *= lReferenceGeometry;
    lReferenceGlobalCurrentPosition = pGlobalPosition;

    // Get the link initial global position and the link current global position.
    pCluster->GetTransformLinkMatrix(lClusterGlobalInitPosition);
    // Multiply lClusterGlobalInitPosition by Geometric Transformation
    lClusterGeometry = get_geometry(pCluster->GetLink());
    lClusterGlobalInitPosition *= lClusterGeometry;
    lClusterGlobalCurrentPosition = get_global_position(pCluster->GetLink(), pTime, pPose);

    // Compute the shift of the link relative to the reference.
    //ModelM-1 * AssoM * AssoGX-1 * LinkGX * LinkM-1*ModelM
    pVertexTransformMatrix = lReferenceGlobalInitPosition.Inverse() * lAssociateGlobalInitPosition * lAssociateGlobalCurrentPosition.Inverse() *
                             lClusterGlobalCurrentPosition * lClusterGlobalInitPosition.Inverse() * lReferenceGlobalInitPosition;
  } else {
    pCluster->GetTransformMatrix(lReferenceGlobalInitPosition);
    lReferenceGlobalCurrentPosition = pGlobalPosition;
    // Multiply lReferenceGlobalInitPosition by Geometric Transformation
    lReferenceGeometry = get_geometry(pMesh->GetNode());
    lReferenceGlobalInitPosition *= lReferenceGeometry;

    // Get the link initial global position and the link current global position.
    pCluster->GetTransformLinkMatrix(lClusterGlobalInitPosition);
    lClusterGlobalCurrentPosition = get_global_position(pCluster->GetLink(), pTime, pPose);

    // Compute the initial position of the link relative to the reference.
    lClusterRelativeInitPosition = lClusterGlobalInitPosition.Inverse() * lReferenceGlobalInitPosition;

    // Compute the current position of the link relative to the reference.
    lClusterRelativeCurrentPositionInverse = lReferenceGlobalCurrentPosition.Inverse() * lClusterGlobalCurrentPosition;

    // Compute the shift of the link relative to the reference.
    pVertexTransformMatrix = lClusterRelativeCurrentPositionInverse * lClusterRelativeInitPosition;
  }
}
Ejemplo n.º 18
0
void PopupMenu::_gui_input(const Ref<InputEvent> &p_event) {

	Ref<InputEventKey> k = p_event;

	if (k.is_valid()) {

		if (!k->is_pressed())
			return;

		switch (k->get_scancode()) {

			case KEY_DOWN: {

				for (int i = mouse_over + 1; i < items.size(); i++) {

					if (i < 0 || i >= items.size())
						continue;

					if (!items[i].separator && !items[i].disabled) {

						mouse_over = i;
						update();
						break;
					}
				}
			} break;
			case KEY_UP: {

				for (int i = mouse_over - 1; i >= 0; i--) {

					if (i < 0 || i >= items.size())
						continue;

					if (!items[i].separator && !items[i].disabled) {

						mouse_over = i;
						update();
						break;
					}
				}
			} break;
			case KEY_ENTER:
			case KEY_KP_ENTER: {

				if (mouse_over >= 0 && mouse_over < items.size() && !items[mouse_over].separator) {

					activate_item(mouse_over);
				}
			} break;
		}
	}

	Ref<InputEventMouseButton> b = p_event;

	if (b.is_valid()) {

		if (b->is_pressed())
			return;

		switch (b->get_button_index()) {

			case BUTTON_WHEEL_DOWN: {

				if (get_global_position().y + get_size().y > get_viewport_rect().size.y) {

					int vseparation = get_constant("vseparation");
					Ref<Font> font = get_font("font");

					Point2 pos = get_position();
					int s = (vseparation + font->get_height()) * 3;
					pos.y -= (s * b->get_factor());
					set_position(pos);

					//update hover
					Ref<InputEventMouseMotion> ie;
					ie.instance();
					ie->set_position(b->get_position() + Vector2(0, s));
					_gui_input(ie);
				}
			} break;
			case BUTTON_WHEEL_UP: {

				if (get_global_position().y < 0) {

					int vseparation = get_constant("vseparation");
					Ref<Font> font = get_font("font");

					Point2 pos = get_position();
					int s = (vseparation + font->get_height()) * 3;
					pos.y += (s * b->get_factor());
					set_position(pos);

					//update hover
					Ref<InputEventMouseMotion> ie;
					ie.instance();
					ie->set_position(b->get_position() - Vector2(0, s));
					_gui_input(ie);
				}
			} break;
			case BUTTON_LEFT: {

				int over = _get_mouse_over(b->get_position());

				if (invalidated_click) {
					invalidated_click = false;
					break;
				}
				if (over < 0) {
					hide();
					break; //non-activable
				}

				if (items[over].separator || items[over].disabled)
					break;

				if (items[over].submenu != "") {

					_activate_submenu(over);
					return;
				}
				activate_item(over);

			} break;
		}

		//update();
	}

	Ref<InputEventMouseMotion> m = p_event;

	if (m.is_valid()) {

		if (invalidated_click) {
			moved += m->get_relative();
			if (moved.length() > 4)
				invalidated_click = false;
		}

		for (List<Rect2>::Element *E = autohide_areas.front(); E; E = E->next()) {

			if (!Rect2(Point2(), get_size()).has_point(m->get_position()) && E->get().has_point(m->get_position())) {
				call_deferred("hide");
				return;
			}
		}

		int over = _get_mouse_over(m->get_position());
		int id = (over < 0 || items[over].separator || items[over].disabled) ? -1 : (items[over].ID >= 0 ? items[over].ID : over);

		if (id < 0) {
			mouse_over = -1;
			update();
			return;
		}

		if (items[over].submenu != "" && submenu_over != over) {
			submenu_over = over;
			submenu_timer->start();
		}

		if (over != mouse_over) {
			mouse_over = over;
			update();
		}
	}
}
Ejemplo n.º 19
0
void Navigator::check_traffic()
{
	double lat = get_global_position()->lat;
	double lon = get_global_position()->lon;
	float alt = get_global_position()->alt;

	// TODO for non-multirotors predicting the future
	// position as accurately as possible will become relevant
	// float vel_n = get_global_position()->vel_n;
	// float vel_e = get_global_position()->vel_e;
	// float vel_d = get_global_position()->vel_d;

	bool changed;
	orb_check(_traffic_sub, &changed);

	float horizontal_separation = 500;
	float vertical_separation = 500;

	while (changed) {

		transponder_report_s tr;
		orb_copy(ORB_ID(transponder_report), _traffic_sub, &tr);

		uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
					  transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
					  transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;

		if ((tr.flags & required_flags) != required_flags) {
			orb_check(_traffic_sub, &changed);
			continue;
		}

		float d_hor, d_vert;
		get_distance_to_point_global_wgs84(lat, lon, alt,
						   tr.lat, tr.lon, tr.altitude, &d_hor, &d_vert);


		// predict final altitude (positive is up) in prediction time frame
		float end_alt = tr.altitude + (d_vert / tr.hor_velocity) * tr.ver_velocity;

		// Predict until the vehicle would have passed this system at its current speed
		float prediction_distance = d_hor + 1000.0f;

		// If the altitude is not getting close to us, do not calculate
		// the horizontal separation.
		// Since commercial flights do most of the time keep flight levels
		// check for the current and for the predicted flight level.
		// we also make the implicit assumption that this system is on the lowest
		// flight level close to ground in the
		// (end_alt - horizontal_separation < alt) condition. If this system should
		// ever be used in normal airspace this implementation would anyway be
		// inappropriate as it should be replaced with a TCAS compliant solution.

		if ((fabsf(alt - tr.altitude) < vertical_separation) || ((end_alt - horizontal_separation) < alt)) {

			double end_lat, end_lon;
			waypoint_from_heading_and_distance(tr.lat, tr.lon, tr.heading, prediction_distance, &end_lat, &end_lon);

			struct crosstrack_error_s cr;

			if (!get_distance_to_line(&cr, lat, lon, tr.lat, tr.lon, end_lat, end_lon)) {

				if (!cr.past_end && (fabsf(cr.distance) < horizontal_separation)) {

					// direction of traffic in human-readable 0..360 degree in earth frame
					int traffic_direction = math::degrees(tr.heading) + 180;

					switch (_param_traffic_avoidance_mode.get()) {

					case 0: {
							/* ignore */
							PX4_WARN("TRAFFIC %s, hdg: %d", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign :
								 "unknown",
								 traffic_direction);
							break;
						}

					case 1: {
							mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately",
									     tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
									     traffic_direction);
							break;
						}

					case 2: {
							mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home",
									     tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
									     traffic_direction);

							// set the return altitude to minimum
							_rtl.set_return_alt_min(true);

							// ask the commander to execute an RTL
							vehicle_command_s vcmd = {};
							vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
							publish_vehicle_cmd(&vcmd);
							break;
						}
					}
				}
			}
		}

		orb_check(_traffic_sub, &changed);
	}
}
Ejemplo n.º 20
0
void AudioStreamPlayer2D::_notification(int p_what) {

	if (p_what == NOTIFICATION_ENTER_TREE) {

		AudioServer::get_singleton()->add_callback(_mix_audios, this);
		if (autoplay && !Engine::get_singleton()->is_editor_hint()) {
			play();
		}
	}

	if (p_what == NOTIFICATION_EXIT_TREE) {

		AudioServer::get_singleton()->remove_callback(_mix_audios, this);
	}

	if (p_what == NOTIFICATION_INTERNAL_FIXED_PROCESS) {

		//update anything related to position first, if possible of course

		if (!output_ready) {
			List<Viewport *> viewports;
			Ref<World2D> world_2d = get_world_2d();
			ERR_FAIL_COND(world_2d.is_null());

			int new_output_count = 0;

			Vector2 global_pos = get_global_position();

			int bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus);

			//check if any area is diverting sound into a bus

			Physics2DDirectSpaceState *space_state = Physics2DServer::get_singleton()->space_get_direct_state(world_2d->get_space());

			Physics2DDirectSpaceState::ShapeResult sr[MAX_INTERSECT_AREAS];

			int areas = space_state->intersect_point(global_pos, sr, MAX_INTERSECT_AREAS, Set<RID>(), area_mask, Physics2DDirectSpaceState::TYPE_MASK_AREA);

			for (int i = 0; i < areas; i++) {

				Area2D *area2d = Object::cast_to<Area2D>(sr[i].collider);
				if (!area2d)
					continue;

				if (!area2d->is_overriding_audio_bus())
					continue;

				StringName bus_name = area2d->get_audio_bus_name();
				bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus_name);
				break;
			}

			world_2d->get_viewport_list(&viewports);
			for (List<Viewport *>::Element *E = viewports.front(); E; E = E->next()) {

				Viewport *vp = E->get();
				if (vp->is_audio_listener_2d()) {

					//compute matrix to convert to screen
					Transform2D to_screen = vp->get_global_canvas_transform() * vp->get_canvas_transform();
					Vector2 screen_size = vp->get_visible_rect().size;

					//screen in global is used for attenuation
					Vector2 screen_in_global = to_screen.affine_inverse().xform(screen_size * 0.5);

					float dist = global_pos.distance_to(screen_in_global); //distance to screen center

					if (dist > max_distance)
						continue; //cant hear this sound in this viewport

					float multiplier = Math::pow(1.0f - dist / max_distance, attenuation);
					multiplier *= Math::db2linear(volume_db); //also apply player volume!

					//point in screen is used for panning
					Vector2 point_in_screen = to_screen.xform(global_pos);

					float pan = CLAMP(point_in_screen.x / screen_size.width, 0.0, 1.0);

					float l = 1.0 - pan;
					float r = pan;

					outputs[new_output_count].vol = AudioFrame(l, r) * multiplier;
					outputs[new_output_count].bus_index = bus_index;
					outputs[new_output_count].viewport = vp; //keep pointer only for reference
					new_output_count++;
					if (new_output_count == MAX_OUTPUTS)
						break;
				}
			}

			output_count = new_output_count;
			output_ready = true;
		}

		//start playing if requested
		if (setplay >= 0.0) {
			setseek = setplay;
			active = true;
			setplay = -1;
			//do not update, this makes it easier to animate (will shut off otherise)
			//_change_notify("playing"); //update property in editor
		}

		//stop playing if no longer active
		if (!active) {
			set_fixed_process_internal(false);
			//do not update, this makes it easier to animate (will shut off otherise)
			//_change_notify("playing"); //update property in editor
			emit_signal("finished");
		}
	}
}
Ejemplo n.º 21
0
void PopupMenu::_gui_input(const Ref<InputEvent> &p_event) {

	if (p_event->is_action("ui_down") && p_event->is_pressed()) {

		int search_from = mouse_over + 1;
		if (search_from >= items.size())
			search_from = 0;

		for (int i = search_from; i < items.size(); i++) {

			if (i < 0 || i >= items.size())
				continue;

			if (!items[i].separator && !items[i].disabled) {

				mouse_over = i;
				emit_signal("id_focused", i);
				update();
				accept_event();
				break;
			}
		}
	} else if (p_event->is_action("ui_up") && p_event->is_pressed()) {

		int search_from = mouse_over - 1;
		if (search_from < 0)
			search_from = items.size() - 1;

		for (int i = search_from; i >= 0; i--) {

			if (i < 0 || i >= items.size())
				continue;

			if (!items[i].separator && !items[i].disabled) {

				mouse_over = i;
				emit_signal("id_focused", i);
				update();
				accept_event();
				break;
			}
		}
	} else if (p_event->is_action("ui_left") && p_event->is_pressed()) {

		Node *n = get_parent();
		if (n && Object::cast_to<PopupMenu>(n)) {
			hide();
			accept_event();
		}
	} else if (p_event->is_action("ui_right") && p_event->is_pressed()) {

		if (mouse_over >= 0 && mouse_over < items.size() && !items[mouse_over].separator && items[mouse_over].submenu != "" && submenu_over != mouse_over) {
			_activate_submenu(mouse_over);
			accept_event();
		}
	} else if (p_event->is_action("ui_accept") && p_event->is_pressed()) {

		if (mouse_over >= 0 && mouse_over < items.size() && !items[mouse_over].separator) {

			if (items[mouse_over].submenu != "" && submenu_over != mouse_over) {
				_activate_submenu(mouse_over);
			} else {
				activate_item(mouse_over);
			}
			accept_event();
		}
	}

	Ref<InputEventMouseButton> b = p_event;

	if (b.is_valid()) {

		if (b->is_pressed())
			return;

		int button_idx = b->get_button_index();
		switch (button_idx) {

			case BUTTON_WHEEL_DOWN: {

				if (get_global_position().y + get_size().y > get_viewport_rect().size.y) {
					_scroll(-b->get_factor(), b->get_position());
				}
			} break;
			case BUTTON_WHEEL_UP: {

				if (get_global_position().y < 0) {
					_scroll(b->get_factor(), b->get_position());
				}
			} break;
			default: {
				// Allow activating item by releasing the LMB or any that was down when the popup appeared
				if (button_idx == BUTTON_LEFT || (initial_button_mask & (1 << (button_idx - 1)))) {

					bool was_during_grabbed_click = during_grabbed_click;
					during_grabbed_click = false;

					int over = _get_mouse_over(b->get_position());

					if (invalidated_click) {
						invalidated_click = false;
						break;
					}
					if (over < 0) {
						if (!was_during_grabbed_click) {
							hide();
						}
						break; //non-activable
					}

					if (items[over].separator || items[over].disabled)
						break;

					if (items[over].submenu != "") {

						_activate_submenu(over);
						return;
					}
					activate_item(over);
				}
			}
		}

		//update();
	}

	Ref<InputEventMouseMotion> m = p_event;

	if (m.is_valid()) {

		if (invalidated_click) {
			moved += m->get_relative();
			if (moved.length() > 4)
				invalidated_click = false;
		}

		for (List<Rect2>::Element *E = autohide_areas.front(); E; E = E->next()) {

			if (!Rect2(Point2(), get_size()).has_point(m->get_position()) && E->get().has_point(m->get_position())) {
				call_deferred("hide");
				return;
			}
		}

		int over = _get_mouse_over(m->get_position());
		int id = (over < 0 || items[over].separator || items[over].disabled) ? -1 : (items[over].ID >= 0 ? items[over].ID : over);

		if (id < 0) {
			mouse_over = -1;
			update();
			return;
		}

		if (items[over].submenu != "" && submenu_over != over) {
			submenu_over = over;
			submenu_timer->start();
		}

		if (over != mouse_over) {
			mouse_over = over;
			update();
		}
	}

	Ref<InputEventPanGesture> pan_gesture = p_event;
	if (pan_gesture.is_valid()) {
		if (get_global_position().y + get_size().y > get_viewport_rect().size.y || get_global_position().y < 0) {
			_scroll(-pan_gesture->get_delta().y, pan_gesture->get_position());
		}
	}
}
Ejemplo n.º 22
0
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) {
		PX4_INFO("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));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_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));
	_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();
	global_position_update();
	local_position_update();
	gps_position_update();
	sensor_combined_update();
	home_position_update(true);
	fw_pos_ctrl_status_update(true);
	params_update();

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[1] = {};

	/* Setup of loop */
	fds[0].fd = _global_pos_sub;
	fds[0].events = POLLIN;

	bool global_pos_available_once = false;

	/* rate-limit global pos subscription to 20 Hz / 50 ms */
	orb_set_interval(_global_pos_sub, 49);

	while (!_task_should_exit) {

		/* wait for up to 1000ms 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("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_ERR("nav: poll error %d, %d", pret, errno);
			usleep(10000);
			continue;

		} else {

			if (fds[0].revents & POLLIN) {
				/* success, global pos is available */
				global_position_update();

				if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
					have_geofence_position_data = true;
				}

				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;
			}
		}

		/* local position updated */
		orb_check(_local_pos_sub, &updated);

		if (updated) {
			local_position_update();
		}

		/* 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();
		}

		/* 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;

				if (home_position_valid()) {
					rep->current.yaw = cmd.param4;
					rep->previous.valid = true;

				} else {
					rep->current.yaw = get_local_position()->yaw;
					rep->previous.valid = false;
				}

				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->current.valid = true;
				rep->next.valid = false;

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {

				/* find NAV_CMD_DO_LAND_START in the mission and
				 * use MAV_CMD_MISSION_START to start the mission there
				 */
				int land_start = _mission.find_offboard_land_start();

				if (land_start != -1) {
					vehicle_command_s vcmd = {};
					vcmd.target_system = get_vstatus()->system_id;
					vcmd.target_component = get_vstatus()->component_id;
					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
					vcmd.param1 = land_start;
					vcmd.param2 = 0;

					publish_vehicle_cmd(vcmd);

				} else {
					PX4_WARN("planned landing not available");
				}

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH) {

				vehicle_command_s vcmd = {};
				vcmd.target_system = get_vstatus()->system_id;
				vcmd.target_component = get_vstatus()->component_id;
				vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;

				orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
				(void)orb_unadvertise(pub);

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {

				if (get_mission_result()->valid &&
				    PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {

					_mission.set_current_offboard_mission_index(cmd.param1);
				}

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
				warnx("navigator: got pause/continue command");

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
				if (cmd.param2 > FLT_EPSILON) {
					// XXX not differentiating ground and airspeed yet
					set_cruising_speed(cmd.param2);

				} else {
					set_cruising_speed();

					/* if no speed target was given try to set throttle */
					if (cmd.param3 > FLT_EPSILON) {
						set_cruising_throttle(cmd.param3 / 100);

					} else {
						set_cruising_throttle();
					}
				}
			}
		}

		/* 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);
			last_geofence_check = hrt_absolute_time();
			have_geofence_position_data = false;

			_geofence_result.timestamp = hrt_absolute_time();
			_geofence_result.geofence_action = _geofence.getGeofenceAction();
			_geofence_result.home_required = _geofence.isHomeRequired();

			if (!inside) {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = true;

				/* 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;

				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}

			publish_geofence_result();
		}

		/* Do stuff according to navigation state set by commander */
		switch (_vstatus.nav_state) {
		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
			_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;
			_navigation_mode = &_rcLoss;
			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:
			_pos_sp_triplet_published_invalid_once = false;
			_navigation_mode = &_dataLinkLoss;
			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;

		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:
		case vehicle_status_s::NAVIGATION_STATE_STAB:
		default:
			_pos_sp_triplet_published_invalid_once = false;
			_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) {
			_pos_sp_triplet.timestamp = hrt_absolute_time();
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

		if (_mission_result_updated) {
			publish_mission_result();
			_mission_result_updated = false;
		}

		perf_end(_loop_perf);
	}

	PX4_INFO("exiting");

	_navigator_task = -1;
}
Ejemplo n.º 23
0
void EditorSpinSlider::_notification(int p_what) {

	if (p_what == MainLoop::NOTIFICATION_WM_FOCUS_OUT || p_what == MainLoop::NOTIFICATION_WM_FOCUS_OUT) {
		if (grabbing_spinner) {
			Input::get_singleton()->set_mouse_mode(Input::MOUSE_MODE_VISIBLE);
			grabbing_spinner = false;
			grabbing_spinner_attempt = false;
		}
	}

	if (p_what == NOTIFICATION_DRAW) {

		updown_offset = -1;

		Ref<StyleBox> sb = get_stylebox("normal", "LineEdit");
		draw_style_box(sb, Rect2(Vector2(), get_size()));
		Ref<Font> font = get_font("font", "LineEdit");

		int avail_width = get_size().width - sb->get_minimum_size().width - sb->get_minimum_size().width;
		avail_width -= font->get_string_size(label).width;
		Ref<Texture> updown = get_icon("updown", "SpinBox");

		if (get_step() == 1) {
			avail_width -= updown->get_width();
		}

		if (has_focus()) {
			Ref<StyleBox> focus = get_stylebox("focus", "LineEdit");
			draw_style_box(focus, Rect2(Vector2(), get_size()));
		}

		String numstr = get_text_value();

		int vofs = (get_size().height - font->get_height()) / 2 + font->get_ascent();

		Color fc = get_color("font_color", "LineEdit");

		int label_ofs = sb->get_offset().x + avail_width;
		draw_string(font, Vector2(label_ofs, vofs), label, fc * Color(1, 1, 1, 0.5));
		draw_string(font, Vector2(sb->get_offset().x, vofs), numstr, fc, avail_width);

		if (get_step() == 1) {
			Ref<Texture> updown = get_icon("updown", "SpinBox");
			int updown_vofs = (get_size().height - updown->get_height()) / 2;
			updown_offset = get_size().width - sb->get_margin(MARGIN_RIGHT) - updown->get_width();
			Color c(1, 1, 1);
			if (hover_updown) {
				c *= Color(1.2, 1.2, 1.2);
			}
			draw_texture(updown, Vector2(updown_offset, updown_vofs), c);
			if (grabber->is_visible()) {
				grabber->hide();
			}
		} else if (!hide_slider) {
			int grabber_w = 4 * EDSCALE;
			int width = get_size().width - sb->get_minimum_size().width - grabber_w;
			int ofs = sb->get_offset().x;
			int svofs = (get_size().height + vofs) / 2 - 1;
			Color c = fc;
			c.a = 0.2;

			draw_rect(Rect2(ofs, svofs + 1, width, 2 * EDSCALE), c);
			int gofs = get_as_ratio() * width;
			c.a = 0.9;
			Rect2 grabber_rect = Rect2(ofs + gofs, svofs + 1, grabber_w, 2 * EDSCALE);
			draw_rect(grabber_rect, c);

			bool display_grabber = (mouse_over_spin || mouse_over_grabber) && !grabbing_spinner;
			if (grabber->is_visible() != display_grabber) {
				if (display_grabber) {
					grabber->show();
				} else {
					grabber->hide();
				}
			}

			if (display_grabber) {
				Ref<Texture> grabber_tex;
				if (mouse_over_grabber) {
					grabber_tex = get_icon("grabber_highlight", "HSlider");
				} else {
					grabber_tex = get_icon("grabber", "HSlider");
				}

				if (grabber->get_texture() != grabber_tex) {
					grabber->set_texture(grabber_tex);
				}

				grabber->set_size(Size2(0, 0));
				grabber->set_position(get_global_position() + grabber_rect.position + grabber_rect.size * 0.5 - grabber->get_size() * 0.5);
				grabber_range = width;
			}
		}
	}

	if (p_what == NOTIFICATION_MOUSE_ENTER) {

		mouse_over_spin = true;
		update();
	}
	if (p_what == NOTIFICATION_MOUSE_EXIT) {

		mouse_over_spin = false;
		update();
	}
}
Ejemplo n.º 24
0
void TabContainer::_gui_input(const Ref<InputEvent> &p_event) {

	Ref<InputEventMouseButton> mb = p_event;

	if (mb.is_valid() && mb->is_pressed() && mb->get_button_index() == BUTTON_LEFT) {

		Point2 pos(mb->get_position().x, mb->get_position().y);
		Size2 size = get_size();

		// Click must be on tabs in the tab header area.
		if (pos.x < tabs_ofs_cache || pos.y > _get_top_margin())
			return;

		// Handle menu button.
		Ref<Texture> menu = get_icon("menu");
		if (popup && pos.x > size.width - menu->get_width()) {
			emit_signal("pre_popup_pressed");

			Vector2 popup_pos = get_global_position();
			popup_pos.x += size.width - popup->get_size().width;
			popup_pos.y += menu->get_height();

			popup->set_global_position(popup_pos);
			popup->popup();
			return;
		}

		Vector<Control *> tabs = _get_tabs();

		// Handle navigation buttons.
		if (buttons_visible_cache) {
			Ref<Texture> increment = get_icon("increment");
			Ref<Texture> decrement = get_icon("decrement");
			if (pos.x > size.width - increment->get_width()) {
				if (last_tab_cache < tabs.size() - 1) {
					first_tab_cache += 1;
					update();
				}
				return;
			} else if (pos.x > size.width - increment->get_width() - decrement->get_width()) {
				if (first_tab_cache > 0) {
					first_tab_cache -= 1;
					update();
				}
				return;
			}
		}

		// Activate the clicked tab.
		pos.x -= tabs_ofs_cache;
		for (int i = first_tab_cache; i <= last_tab_cache; i++) {
			int tab_width = _get_tab_width(i);
			if (pos.x < tab_width) {
				if (!get_tab_disabled(i)) {
					set_current_tab(i);
				}
				break;
			}
			pos.x -= tab_width;
		}
	}
}
Ejemplo n.º 25
0
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;
}
Ejemplo n.º 26
0
void Node2D::global_translate(const Vector2 &p_amount) {

	set_global_position(get_global_position() + p_amount);
}
Ejemplo n.º 27
0
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 */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME);
		_geofence.loadFromFile(GEOFENCE_FILENAME);
	}

	/* do subscriptions */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_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));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
	_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
	_traffic_sub = orb_subscribe(ORB_ID(transponder_report));

	/* copy all topics first time */
	vehicle_status_update();
	vehicle_land_detected_update();
	global_position_update();
	local_position_update();
	gps_position_update();
	sensor_combined_update();
	home_position_update(true);
	fw_pos_ctrl_status_update(true);
	params_update();

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[1] = {};

	/* Setup of loop */
	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;

	/* rate-limit position subscription to 20 Hz / 50 ms */
	orb_set_interval(_local_pos_sub, 50);

	while (!_task_should_exit) {

		/* wait for up to 1000ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);

		if (pret == 0) {
			/* 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_ERR("poll error %d, %d", pret, errno);
			usleep(10000);
			continue;

		} else {
			if (fds[0].revents & POLLIN) {
				/* success, local pos is available */
				local_position_update();
			}
		}

		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;
			}
		}

		/* global position updated */
		orb_check(_global_pos_sub, &updated);

		if (updated) {
			global_position_update();

			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
				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();
		}

		/* 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();
		}

		/* vehicle_command updated */
		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_GO_AROUND) {

				// DO_GO_AROUND is currently handled by the position controller (unacknowledged)
				// TODO: move DO_GO_AROUND handling to navigator
				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {

				position_setpoint_triplet_s *rep = get_reposition_triplet();
				position_setpoint_triplet_s *curr = get_position_setpoint_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;
				rep->current.cruising_speed = get_cruising_speed();
				rep->current.cruising_throttle = get_cruising_throttle();

				// 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)) {

					// Position change with optional altitude change
					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;

					if (PX4_ISFINITE(cmd.param7)) {
						rep->current.alt = cmd.param7;

					} else {
						rep->current.alt = get_global_position()->alt;
					}

				} else if (PX4_ISFINITE(cmd.param7) && curr->current.valid
					   && PX4_ISFINITE(curr->current.lat)
					   && PX4_ISFINITE(curr->current.lon)) {

					// Altitude without position change
					rep->current.lat = curr->current.lat;
					rep->current.lon = curr->current.lon;
					rep->current.alt = cmd.param7;

				} else {
					// All three set to NaN - hold in current position
					rep->current.lat = get_global_position()->lat;
					rep->current.lon = get_global_position()->lon;
					rep->current.alt = get_global_position()->alt;
				}

				rep->previous.valid = true;
				rep->current.valid = true;
				rep->next.valid = false;

				// CMD_DO_REPOSITION is acknowledged by commander

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
				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;

				if (home_position_valid()) {
					rep->current.yaw = cmd.param4;
					rep->previous.valid = true;

				} else {
					rep->current.yaw = get_local_position()->yaw;
					rep->previous.valid = false;
				}

				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->current.valid = true;
				rep->next.valid = false;

				// CMD_NAV_TAKEOFF is acknowledged by commander

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {

				/* find NAV_CMD_DO_LAND_START in the mission and
				 * use MAV_CMD_MISSION_START to start the mission there
				 */
				if (_mission.land_start()) {
					vehicle_command_s vcmd = {};
					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
					vcmd.param1 = _mission.get_land_start_index();
					publish_vehicle_cmd(&vcmd);

				} else {
					PX4_WARN("planned mission landing not available");
				}

				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
				if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
					if (!_mission.set_current_offboard_mission_index(cmd.param1)) {
						PX4_WARN("CMD_MISSION_START failed");
					}
				}

				// CMD_MISSION_START is acknowledged by commander

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
				if (cmd.param2 > FLT_EPSILON) {
					// XXX not differentiating ground and airspeed yet
					set_cruising_speed(cmd.param2);

				} else {
					set_cruising_speed();

					/* if no speed target was given try to set throttle */
					if (cmd.param3 > FLT_EPSILON) {
						set_cruising_throttle(cmd.param3 / 100);

					} else {
						set_cruising_throttle();
					}
				}

				// TODO: handle responses for supported DO_CHANGE_SPEED options?
				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI
				   || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI
				   || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION
				   || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
				   || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE) {
				_vroi = {};

				switch (cmd.command) {
				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
				case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
					_vroi.mode = cmd.param1;
					break;

				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
					_vroi.mode = vehicle_command_s::VEHICLE_ROI_LOCATION;
					break;

				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
					_vroi.mode = vehicle_command_s::VEHICLE_ROI_WPNEXT;
					_vroi.pitchOffset = cmd.param5;
					_vroi.rollOffset = cmd.param6;
					_vroi.yawOffset = cmd.param7;
					break;

				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
					_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
					break;
				}

				switch (_vroi.mode) {
				case vehicle_command_s::VEHICLE_ROI_NONE:
					break;

				case vehicle_command_s::VEHICLE_ROI_WPNEXT:
					// TODO: implement point toward next MISSION
					break;

				case vehicle_command_s::VEHICLE_ROI_WPINDEX:
					_vroi.mission_seq = cmd.param2;
					break;

				case vehicle_command_s::VEHICLE_ROI_LOCATION:
					_vroi.lat = cmd.param5;
					_vroi.lon = cmd.param6;
					_vroi.alt = cmd.param7;
					break;

				case vehicle_command_s::VEHICLE_ROI_TARGET:
					_vroi.target_seq = cmd.param2;
					break;

				default:
					_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
					break;
				}

				_vroi.timestamp = hrt_absolute_time();

				if (_vehicle_roi_pub != nullptr) {
					orb_publish(ORB_ID(vehicle_roi), _vehicle_roi_pub, &_vroi);

				} else {
					_vehicle_roi_pub = orb_advertise(ORB_ID(vehicle_roi), &_vroi);
				}

				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
			}
		}

		/* Check for traffic */
		check_traffic();

		/* 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.check(_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.timestamp = hrt_absolute_time();
			_geofence_result.geofence_action = _geofence.getGeofenceAction();
			_geofence_result.home_required = _geofence.isHomeRequired();

			if (!inside) {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = true;

				/* 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;

				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}

			publish_geofence_result();
		}

		/* Do stuff according to navigation state set by commander */
		NavigatorMode *navigation_mode_new{nullptr};

		switch (_vstatus.nav_state) {
		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_mission;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_loiter;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_rcLoss;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
			_pos_sp_triplet_published_invalid_once = false;

			// if RTL is set to use a mission landing and mission has a planned landing, then use MISSION
			if (mission_landing_required() && on_mission_landing()) {
				navigation_mode_new = &_mission;

			} else {
				navigation_mode_new = &_rtl;
			}

			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_takeoff;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_land;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_precland;
			_precland.set_mode(PrecLandMode::Required);
			break;

		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_land;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_dataLinkLoss;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_engineFailure;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_gpsFailure;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_follow_target;
			break;

		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:
		case vehicle_status_s::NAVIGATION_STATE_STAB:
		default:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = nullptr;
			_can_loiter_at_sp = false;
			break;
		}

		/* we have a new navigation mode: reset triplet */
		if (_navigation_mode != navigation_mode_new) {
			reset_triplets();
		}

		_navigation_mode = navigation_mode_new;

		/* 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 we landed and have not received takeoff setpoint then stay in idle */
		if (_land_detected.landed &&
		    !((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
		      || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) {

			_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
			_pos_sp_triplet.current.valid = true;
			_pos_sp_triplet.previous.valid = false;
			_pos_sp_triplet.next.valid = false;

		}

		/* 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;
			reset_triplets();
		}

		if (_pos_sp_triplet_updated) {
			_pos_sp_triplet.timestamp = hrt_absolute_time();
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

		if (_mission_result_updated) {
			publish_mission_result();
			_mission_result_updated = false;
		}

		perf_end(_loop_perf);
	}

	orb_unsubscribe(_global_pos_sub);
	orb_unsubscribe(_local_pos_sub);
	orb_unsubscribe(_gps_pos_sub);
	orb_unsubscribe(_sensor_combined_sub);
	orb_unsubscribe(_fw_pos_ctrl_status_sub);
	orb_unsubscribe(_vstatus_sub);
	orb_unsubscribe(_land_detected_sub);
	orb_unsubscribe(_home_pos_sub);
	orb_unsubscribe(_offboard_mission_sub);
	orb_unsubscribe(_param_update_sub);
	orb_unsubscribe(_vehicle_command_sub);

	PX4_INFO("exiting");

	_navigator_task = -1;
}