aboutsummaryrefslogtreecommitdiff
path: root/src/modules/navigator
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/navigator')
-rw-r--r--src/modules/navigator/mission.cpp36
-rw-r--r--src/modules/navigator/mission_feasibility_checker.cpp9
-rw-r--r--src/modules/navigator/module.mk2
-rw-r--r--src/modules/navigator/navigator.h6
-rw-r--r--src/modules/navigator/navigator_main.cpp21
-rw-r--r--src/modules/navigator/navigator_mode.h6
6 files changed, 53 insertions, 27 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index ba766cd10..12f6b9c21 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -314,30 +314,36 @@ Mission::set_mission_items()
/* set previous position setpoint to current */
set_previous_pos_setpoint();
+ /* get home distance state */
+ bool home_dist_ok = check_dist_1wp();
+ /* the home dist check provides user feedback, so we initialize it to this */
+ bool user_feedback_done = !home_dist_ok;
+
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
- mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
- } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
+ } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
- mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
- /* no mission available, switch to loiter */
+ /* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
- mavlink_log_info(_navigator->get_mavlink_fd(),
- "#audio: mission finished");
- } else {
- mavlink_log_info(_navigator->get_mavlink_fd(),
- "#audio: no mission available");
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+ } else if (!user_feedback_done) {
+ /* only tell users that we got no mission if there has not been any
+ * better, more specific feedback yet
+ */
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
}
_mission_type = MISSION_TYPE_NONE;
@@ -397,7 +403,7 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
- mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
@@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
- "#audio: ERROR waypoint could not be read");
+ "ERROR waypoint could not be read");
return false;
}
@@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
- "#audio: ERROR DO JUMP waypoint could not be written");
+ "ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
- mavlink_log_info(_navigator->get_mavlink_fd(),
- "#audio: DO JUMP repetitions completed");
+ mavlink_log_critical(_navigator->get_mavlink_fd(),
+ "DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
- "#audio: ERROR DO JUMP is cycling, giving up");
+ "ERROR DO JUMP is cycling, giving up");
return false;
}
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index dd7f4c801..606521f20 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
}
}
- if (home_alt > missionitem.altitude) {
+ /* calculate the global waypoint altitude */
+ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
+
+ if (home_alt > wp_alt) {
if (throw_error) {
- mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
+ mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
return false;
} else {
- mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
+ mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
return true;
}
}
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 637eaae59..b50198996 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -54,3 +54,5 @@ SRCS = navigator_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS = -Weffc++
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index bf6e2ea0e..8edbb63b3 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -222,5 +222,11 @@ private:
* Publish a new position setpoint triplet for position controllers
*/
void publish_position_setpoint_triplet();
+
+ /* this class has ptr data members, so it should not be copied,
+ * consequently the copy constructors are private.
+ */
+ Navigator(const Navigator&);
+ Navigator operator=(const Navigator&);
};
#endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1a5ba4c1a..331a9a728 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -105,17 +105,18 @@ Navigator::Navigator() :
_control_mode_sub(-1),
_onboard_mission_sub(-1),
_offboard_mission_sub(-1),
+ _param_update_sub(-1),
_pos_sp_triplet_pub(-1),
- _vstatus({}),
- _control_mode({}),
- _global_pos({}),
- _home_pos({}),
- _mission_item({}),
- _nav_caps({}),
- _pos_sp_triplet({}),
+ _vstatus{},
+ _control_mode{},
+ _global_pos{},
+ _home_pos{},
+ _mission_item{},
+ _nav_caps{},
+ _pos_sp_triplet{},
_mission_item_valid(false),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
- _geofence({}),
+ _geofence{},
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
@@ -124,6 +125,8 @@ Navigator::Navigator() :
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_offboard(this, "OFF"),
+ _can_loiter_at_sp(false),
+ _pos_sp_triplet_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD")
{
@@ -455,7 +458,7 @@ void
Navigator::publish_position_setpoint_triplet()
{
/* update navigation state */
- /* TODO: set nav_state */
+ _pos_sp_triplet.nav_state = _vstatus.nav_state;
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index a7ba79bba..de5545dcb 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -88,6 +88,12 @@ protected:
private:
bool _first_run;
+
+ /* this class has ptr data members, so it should not be copied,
+ * consequently the copy constructors are private.
+ */
+ NavigatorMode(const NavigatorMode&);
+ NavigatorMode operator=(const NavigatorMode&);
};
#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 21:40:53 +0000