/** * 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); }
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(); }
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 }
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(); }
void ColorPickerButton::pressed() { _update_picker(); popup->set_position(get_global_position() - picker->get_combined_minimum_size()); popup->popup(); picker->set_focus_on_line_edit(); }
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(); }
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(); }
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(); }
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(); }
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(); } }
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(); } }
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); }
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); } }
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; }
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; }
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)); } } }
//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; } }
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(); } } }
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); } }
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"); } } }
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()); } } }
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; }
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(); } }
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; } } }
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 Node2D::global_translate(const Vector2 &p_amount) { set_global_position(get_global_position() + p_amount); }
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; }