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