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

navigator: switch to events

Browse Source
This commit is contained in:
Beat Küng
2021-09-11 14:08:41 -04: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/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;