fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 23:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: switch to events

Browse Source
This commit is contained in:
Beat Küng
2021-07-08 17:03:38 +02:00
committed by Daniel Agar
parent 5ac43e7236
commit 1f73294ad7
8 changed files with 224 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/enginefailure.cpp
+3 -1
View File
@@ -45,6 +45,7 @@
#include <systemlib/err.h>
#include <lib/geo/geo.h>
#include <navigator/navigation.h>
#include <px4_platform_common/events.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@@ -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;
src/modules/navigator/geofence.cpp
+13 -5
View File
@@ -47,6 +47,7 @@
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <systemlib/mavlink_log.h>
#include <px4_platform_common/events.h>
#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<float>(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<float>(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();
src/modules/navigator/gpsfailure.cpp
+6 -2
View File
@@ -47,6 +47,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <px4_platform_common/events.h>
#include <mathlib/mathlib.h>
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:
src/modules/navigator/mission_block.cpp
+1 -1
View File
@@ -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 {
src/modules/navigator/mission_feasibility_checker.cpp
+113 -35
View File
@@ -52,6 +52,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/position_controller_landing_status.h>
#include <px4_platform_common/events.h>
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<int16_t>(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<int16_t>(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<int16_t>(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<uint16_t, uint16_t>(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<uint32_t>(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<uint32_t, uint32_t>(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<float>(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<int32_t, int32_t>(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<uint32_t, uint32_t>(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<uint32_t, uint32_t>(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<float, float>(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;
src/modules/navigator/navigator.h
+1 -1
View File
@@ -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; }
src/modules/navigator/navigator_main.cpp
+72 -14
View File
@@ -54,6 +54,7 @@
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <systemlib/mavlink_log.h>
@@ -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<uint64_t, int32_t, int16_t>(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<uint64_t, int32_t, int16_t>(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<uint64_t, int32_t, int16_t>(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<uint64_t, int32_t, int16_t>(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");
}
}
src/modules/navigator/takeoff.cpp
+15 -3
View File
@@ -40,6 +40,7 @@
#include "takeoff.h"
#include "navigator.h"
#include <px4_platform_common/events.h>
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<float>(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