diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index 5ad8d966ee..9977799542 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -128,7 +129,8 @@ EngineFailure::advance_ef() { switch (_ef_state) { case EF_STATE_NONE: - mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down"); + mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down\t"); + events::send(events::ID("enginefailure_loitering"), events::Log::Emergency, "Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 163107c3c5..aa14669c05 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include "navigator.h" @@ -239,8 +240,11 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) { if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)\t", (double)max_horizontal_distance); + events::send(events::ID("navigator_geofence_max_dist_from_home"), {events::Log::Critical, events::LogInternal::Warning}, + "Geofence: maximum distance from home reached ({1:.0m})", + max_horizontal_distance); _last_horizontal_range_warning = hrt_absolute_time(); } @@ -264,8 +268,11 @@ bool Geofence::isBelowMaxAltitude(float altitude) if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) { if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)\t", (double)max_vertical_distance); + events::send(events::ID("navigator_geofence_max_alt_from_home"), {events::Log::Critical, events::LogInternal::Warning}, + "Geofence: maximum altitude above home reached ({1:.0m_v})", + max_vertical_distance); _last_vertical_range_warning = hrt_absolute_time(); } @@ -553,7 +560,8 @@ Geofence::loadFromFile(const char *filename) /* Check if import was successful */ if (gotVertical && pointCounter > 2) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t"); + events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported"); rc = PX4_OK; /* do a second pass, now that we know the number of vertices */ @@ -573,8 +581,8 @@ Geofence::loadFromFile(const char *filename) rc = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); } else { - PX4_ERR("Geofence: import error"); - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); + events::send(events::ID("navigator_geofence_import_failed"), events::Log::Error, "Geofence: import error"); } updateFence(); diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index da237ad5b3..5a6b5b4c4f 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include using matrix::Eulerf; @@ -153,12 +154,15 @@ GpsFailure::advance_gpsf() switch (_gpsf_state) { case GPSF_STATE_NONE: _gpsf_state = GPSF_STATE_LOITER; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: loitering"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: loitering\t"); + events::send(events::ID("navigator_gpsfailure_loitering"), events::Log::Error, "Global position failure: loitering"); break; case GPSF_STATE_LOITER: _gpsf_state = GPSF_STATE_TERMINATE; - mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight"); + mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight\t"); + events::send(events::ID("navigator_gpsfailure_terminate"), events::Log::Emergency, + "No GPS recovery, terminating flight"); break; case GPSF_STATE_TERMINATE: diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index ec69ea8893..e616f29cc1 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -342,7 +342,7 @@ MissionBlock::is_mission_item_reached() (_navigator->get_yaw_timeout() >= FLT_EPSILON) && (now - _time_wp_reached >= (hrt_abstime)_navigator->get_yaw_timeout() * 1e6f)) { - _navigator->set_mission_failure("unable to reach heading within timeout"); + _navigator->set_mission_failure_heading_timeout(); } } else { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index c3b6149747..2df3f922d8 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -52,6 +52,7 @@ #include #include #include +#include bool MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, @@ -74,7 +75,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, if (!home_alt_valid) { failed = true; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock."); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.\t"); + events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock"); } else { failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint); @@ -137,7 +139,9 @@ bool MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid) { if (_navigator->get_geofence().isHomeRequired() && !home_valid) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t"); + events::send(events::ID("navigator_mis_geofence_no_home"), {events::Log::Error, events::LogInternal::Info}, + "Geofence requires a valid home position"); return false; } @@ -153,7 +157,9 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al } if (missionitem.altitude_is_relative && !home_valid) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t"); + events::send(events::ID("navigator_mis_geofence_no_home2"), {events::Log::Error, events::LogInternal::Info}, + "Geofence requires a valid home position"); return false; } @@ -162,7 +168,10 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().check(missionitem)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1); + events::send(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info}, + "Geofence violation for waypoint {1}", + i + 1); return false; } } @@ -190,7 +199,10 @@ MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, f _navigator->get_mission_result()->warning = true; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt\t", i + 1); + events::send(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: No home position, waypoint {1} uses relative altitude", + i + 1); return false; } @@ -202,7 +214,9 @@ MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, f _navigator->get_mission_result()->warning = true; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home\t", i + 1); + events::send(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info}, + "Waypoint {1} below home", i + 1); } } @@ -219,7 +233,9 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { // not supposed to happen unless the datamanager can't access the SD card, etc. - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card\t"); + events::send(events::ID("navigator_mis_sd_failure"), events::Log::Error, + "Mission rejected: Cannot access mission storage"); return false; } @@ -264,8 +280,11 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1), + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d\t", + (int)(i + 1), (int)missionitem.nav_cmd); + events::send(events::ID("navigator_mis_unsup_cmd"), {events::Log::Error, events::LogInternal::Warning}, + "Mission rejected: item {1}: unsupported command: {2}", i + 1, missionitem.nav_cmd); return false; } @@ -274,15 +293,19 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) /* check actuator number */ if (missionitem.params[0] < 0 || missionitem.params[0] > 5) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Actuator number %d is out of bounds 0..5", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Actuator number %d is out of bounds 0..5\t", (int)missionitem.params[0]); + events::send(events::ID("navigator_mis_act_index"), {events::Log::Error, events::LogInternal::Warning}, + "Actuator number {1} is out of bounds 0..5", (int)missionitem.params[0]); return false; } /* check actuator value */ if (missionitem.params[1] < -PWM_DEFAULT_MAX || missionitem.params[1] > PWM_DEFAULT_MAX) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX", (int)missionitem.params[1]); + "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX\t", (int)missionitem.params[1]); + events::send(events::ID("navigator_mis_act_range"), {events::Log::Error, events::LogInternal::Warning}, + "Actuator value {1} is out of bounds -{2}..{2}", (int)missionitem.params[1], PWM_DEFAULT_MAX); return false; } } @@ -290,7 +313,9 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) // check if the mission starts with a land command while the vehicle is landed if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->get_land_detected()->landed) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing\t"); + events::send(events::ID("navigator_mis_starts_w_landing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: starts with landing"); return false; } } @@ -331,7 +356,12 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt } if (takeoff_alt - 1.0f < acceptance_radius) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!\t"); + /* EVENT + * @description The minimum takeoff altitude is the acceptance radius plus 1m. + */ + events::send(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", acceptance_radius + 1.f); return false; } @@ -401,14 +431,18 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt // MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission // while the vehicle is flying and it does not require a takeoff waypoint if (!has_takeoff) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff waypoint required."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff waypoint required.\t"); + events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: takeoff waypoint required"); return false; } else if (!takeoff_first) { // check if the takeoff waypoint is the first waypoint item on the mission // i.e, an item with position/attitude change modification // if it is not, the mission should be rejected - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t"); + events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: takeoff is not the first waypoint item"); return false; } } @@ -441,7 +475,9 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool // if DO_LAND_START found then require valid landing AFTER if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { if (land_start_found) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t"); + events::send(events::ID("navigator_mis_multiple_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: more than one land start commands"); return false; } else { @@ -483,27 +519,39 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool if (missionitem_previous.altitude > slope_alt_req + 1.0f) { /* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach."); - const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, horizontal_slope_displacement, slope_angle_rad); - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.\t"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.\t", (int)ceilf(slope_alt_req - missionitem_previous.altitude), (int)ceilf(wp_distance_req - wp_distance)); + /* EVENT + * @description + * The landing waypoint must be above the altitude of slope at the given waypoint distance. + * Move it down {1m_v} or move it further away by {2m}. + */ + events::send(events::ID("navigator_mis_land_approach"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: adjust landing approach", + (int)ceilf(slope_alt_req - missionitem_previous.altitude), + (int)ceilf(wp_distance_req - wp_distance)); return false; } } else { /* Landing waypoint is above last waypoint */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.\t"); + events::send(events::ID("navigator_mis_land_too_high"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: landing waypoint is above the last waypoint"); return false; } } else { /* Last wp is in flare region */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.\t"); + events::send(events::ID("navigator_mis_land_within_flare"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: waypoint is within landing flare"); return false; } @@ -511,31 +559,41 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool } else { // mission item before land doesn't have a position - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach.\t"); + events::send(events::ID("navigator_mis_req_landing_approach"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: landing approach is required"); return false; } } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.\t"); + events::send(events::ID("navigator_mis_starts_w_landing2"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: starts with landing"); return false; } } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { if (land_start_found && do_land_start_index < i) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: land start item before RTL item not possible."); + "Mission rejected: land start item before RTL item not possible.\t"); + events::send(events::ID("navigator_mis_land_before_rtl"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: land start item before RTL item is not possible"); return false; } } } if (land_start_req && !land_start_found) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t"); + events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: landing pattern required"); return false; } if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t"); + events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: invalid land start"); return false; } @@ -565,7 +623,9 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_ // if DO_LAND_START found then require valid landing AFTER if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { if (land_start_found) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t"); + events::send(events::ID("navigator_mis_multi_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: more than one land start commands"); return false; } else { @@ -587,26 +647,34 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_ } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.\t"); + events::send(events::ID("navigator_mis_starts_w_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: starts with land waypoint"); return false; } } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { if (land_start_found && do_land_start_index < i) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: land start item before RTL item not possible."); + "Mission rejected: land start item before RTL item not possible.\t"); + events::send(events::ID("navigator_mis_land_before_rtl2"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: land start item before RTL item is not possible"); return false; } } } if (land_start_req && !land_start_found) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t"); + events::send(events::ID("navigator_mis_land_missing2"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: landing pattern required"); return false; } if (land_start_found && (do_land_start_index > landing_approach_index)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start."); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t"); + events::send(events::ID("navigator_mis_invalid_land2"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: invalid land start"); return false; } @@ -629,7 +697,9 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission."); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.\t"); + events::send(events::ID("navigator_mis_storage_failure"), events::Log::Error, + "Error reading mission storage"); return false; } @@ -650,8 +720,10 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission } else { /* item is too far from home */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "First waypoint too far away: %dm, %d max", + "First waypoint too far away: %dm, %d max\t", (int)dist_to_1wp, (int)max_distance); + events::send(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info}, + "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)max_distance); _navigator->get_mission_result()->warning = true; return false; @@ -681,7 +753,9 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission."); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.\t"); + events::send(events::ID("navigator_mis_storage_failure2"), events::Log::Error, + "Error reading mission storage"); return false; } @@ -702,8 +776,10 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi if (dist_between_waypoints > max_distance) { /* distance between waypoints is too high */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Distance between waypoints too far: %d meters, %d max.", + "Distance between waypoints too far: %d meters, %d max.\t", (int)dist_between_waypoints, (int)max_distance); + events::send(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info}, + "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints, (uint32_t)max_distance); _navigator->get_mission_result()->warning = true; return false; @@ -719,8 +795,10 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi * invalid mission and makes calculating the direction from one waypoint * to another impossible. */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Distance between waypoint and gate too close: %d meters", + "Distance between waypoint and gate too close: %d meters\t", (int)dist_between_waypoints); + events::send(events::ID("navigator_mis_wp_gate_too_close"), {events::Log::Error, events::LogInternal::Info}, + "Distance between waypoint and gate too close: {1:.3m} (minimum: {2:.3m})", dist_between_waypoints, 0.05f); _navigator->get_mission_result()->warning = true; return false; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index da33c8058e..e1549e8b92 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -267,7 +267,7 @@ public: void increment_mission_instance_count() { _mission_result.instance_count++; } int mission_instance_count() const { return _mission_result.instance_count; } - void set_mission_failure(const char *reason); + void set_mission_failure_heading_timeout(); void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 169efe4ee5..ae922ed802 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -362,7 +363,9 @@ Navigator::run() rep->next.valid = false; } else { - mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence"); + mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence\t"); + events::send(events::ID("navigator_reposition_outside_geofence"), {events::Log::Error, events::LogInternal::Info}, + "Reposition is outside geofence"); } // CMD_DO_REPOSITION is acknowledged by commander @@ -533,10 +536,13 @@ Navigator::run() case RTL::RTL_CLOSEST: if (rtl_activated) { if (rtl_type() == RTL::RTL_LAND) { - mavlink_log_info(get_mavlink_log_pub(), "RTL LAND activated"); + mavlink_log_info(get_mavlink_log_pub(), "RTL LAND activated\t"); + events::send(events::ID("navigator_rtl_landing_activated"), events::Log::Info, "RTL activated"); } else { - mavlink_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated"); + mavlink_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated\t"); + events::send(events::ID("navigator_rtl_closest_point_activated"), events::Log::Info, + "RTL to closest landing point activated"); } } @@ -577,7 +583,9 @@ Navigator::run() } if (rtl_activated) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission"); + mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t"); + events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info, + "RTL Mission activated, continue mission"); } navigation_mode_new = &_mission; @@ -599,14 +607,18 @@ Navigator::run() } if (rtl_activated) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse"); + mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t"); + events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info, + "RTL Mission activated, fly mission in reverse"); } navigation_mode_new = &_mission; } else { if (rtl_activated) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home"); + mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t"); + events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info, + "RTL Mission activated, fly to home"); } navigation_mode_new = &_rtl; @@ -617,7 +629,8 @@ Navigator::run() default: if (rtl_activated) { - mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated"); + mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t"); + events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated"); } navigation_mode_new = &_rtl; @@ -794,7 +807,9 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) /* Issue a warning about the geofence violation once and only if we are armed */ if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence"); + mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence\t"); + events::send(events::ID("navigator_approach_geofence"), {events::Log::Warning, events::LogInternal::Info}, + "Approaching on Geofence"); // we have predicted a geofence violation and if the action is to loiter then // demand a reposition to a location which is inside the geofence @@ -1154,6 +1169,12 @@ void Navigator::check_traffic() snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", tr.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]); } + uint64_t uas_id_int = 0; + + for (int i = 0; i < 8; i++) { + uas_id_int |= (uint64_t)(tr.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8); + } + //Manned/Unmanned Vehicle Seperation Distance if (tr.emitter_type == transponder_report_s::ADSB_EMITTER_TYPE_UAV) { horizontal_separation = NAVTrafficAvoidUnmanned; @@ -1212,19 +1233,36 @@ void Navigator::check_traffic() case 1: { /* Warn only */ - mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d", + mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, traffic_seperation, traffic_direction); + /* EVENT + * @description + * - ID: {1} + * - Distance: {2m} + * - Direction: {3} degrees + */ + events::send(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert", + uas_id_int, traffic_seperation, traffic_direction); break; } case 2: { /* RTL Mode */ - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d", + mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, traffic_seperation, traffic_direction); + /* EVENT + * @description + * - ID: {1} + * - Distance: {2m} + * - Direction: {3} degrees + */ + events::send(events::ID("navigator_traffic_rtl"), events::Log::Critical, + "Traffic alert, returning home", + uas_id_int, traffic_seperation, traffic_direction); // set the return altitude to minimum _rtl.set_return_alt_min(true); @@ -1238,10 +1276,19 @@ void Navigator::check_traffic() case 3: { /* Land Mode */ - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d", + mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, traffic_seperation, traffic_direction); + /* EVENT + * @description + * - ID: {1} + * - Distance: {2m} + * - Direction: {3} degrees + */ + events::send(events::ID("navigator_traffic_land"), events::Log::Critical, + "Traffic alert, landing", + uas_id_int, traffic_seperation, traffic_direction); // ask the commander to land vehicle_command_s vcmd = {}; @@ -1253,10 +1300,19 @@ void Navigator::check_traffic() case 4: { /* Position hold */ - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d", + mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, traffic_seperation, traffic_direction); + /* EVENT + * @description + * - ID: {1} + * - Distance: {2m} + * - Direction: {3} degrees + */ + events::send(events::ID("navigator_traffic_hold"), events::Log::Critical, + "Traffic alert, holding position", + uas_id_int, traffic_seperation, traffic_direction); // ask the commander to Loiter vehicle_command_s vcmd = {}; @@ -1375,12 +1431,14 @@ Navigator::publish_mission_result() } void -Navigator::set_mission_failure(const char *reason) +Navigator::set_mission_failure_heading_timeout() { if (!_mission_result.failure) { _mission_result.failure = true; set_mission_result_updated(); - mavlink_log_critical(&_mavlink_log_pub, "%s", reason); + mavlink_log_critical(&_mavlink_log_pub, "unable to reach heading within timeout\t"); + events::send(events::ID("navigator_mission_failure_heading"), events::Log::Critical, + "Mission failure: unable to reach heading within timeout"); } } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 2bde007358..06dc1e2b59 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -40,6 +40,7 @@ #include "takeoff.h" #include "navigator.h" +#include Takeoff::Takeoff(Navigator *navigator) : MissionBlock(navigator) @@ -91,6 +92,8 @@ Takeoff::set_takeoff_position() } // Use altitude if it has been set. If home position is invalid use min_abs_altitude + events::LogLevel log_level = events::LogLevel::Disabled; + if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) { abs_altitude = rep->current.alt; @@ -98,7 +101,8 @@ Takeoff::set_takeoff_position() if (abs_altitude < min_abs_altitude) { if (abs_altitude < min_abs_altitude - 0.1f) { // don't complain if difference is smaller than 10cm mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt()); + "Using minimum takeoff altitude: %.2f m\t", (double)_navigator->get_takeoff_min_alt()); + log_level = events::LogLevel::Warning; } abs_altitude = min_abs_altitude; @@ -108,14 +112,22 @@ Takeoff::set_takeoff_position() // Use home + minimum clearance but only notify. abs_altitude = min_abs_altitude; mavlink_log_info(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt()); + "Using minimum takeoff altitude: %.2f m\t", (double)_navigator->get_takeoff_min_alt()); + log_level = events::LogLevel::Info; } + if (log_level != events::LogLevel::Disabled) { + events::send(events::ID("navigator_takeoff_min_alt"), {log_level, events::LogInternal::Info}, + "Using minimum takeoff altitude: {1:.2m}", + _navigator->get_takeoff_min_alt()); + } if (abs_altitude < _navigator->get_global_position()->alt) { // If the suggestion is lower than our current alt, let's not go down. abs_altitude = _navigator->get_global_position()->alt; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude\t"); + events::send(events::ID("navigator_takeoff_already_higher"), {events::Log::Error, events::LogInternal::Info}, + "Already higher than takeoff altitude (not descending)"); } // set current mission item to takeoff