precland: save flash space

PX4 · Apr 4, 2022 · 6b67ed5 · 6b67ed5
1 parent 04a4cbb
commit 6b67ed5
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 26 deletions.
diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp
@@ -59,6 +59,8 @@
 
 #define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
 
+static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
+
 PrecLand::PrecLand(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
@@ -89,7 +91,7 @@ PrecLand::on_activation()
 
 	// Check that the current position setpoint is valid, otherwise land at current position
 	if (!pos_sp_triplet->current.valid) {
-		PX4_WARN("Resetting landing position to current position");
+		PX4_WARN("Reset");
 		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
 		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
@@ -191,9 +193,7 @@ PrecLand::run_state_start()
 
 	if (_mode == PrecLandMode::Opportunistic) {
 		// could not see the target immediately, so just fall back to normal landing
-		if (!switch_to_state_fallback()) {
-			PX4_ERR("Can't switch to search or fallback landing");
-		}
+		switch_to_state_fallback();
 	}
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -211,16 +211,12 @@ PrecLand::run_state_start()
 
 			if (hrt_absolute_time() - _point_reached_time > 2000000) {
 				if (!switch_to_state_search()) {
-					if (!switch_to_state_fallback()) {
-						PX4_ERR("Can't switch to search or fallback landing");
-					}
+					switch_to_state_fallback();
 				}
 			}
 
 		} else {
-			if (!switch_to_state_fallback()) {
-				PX4_ERR("Can't switch to search or fallback landing");
-			}
+			switch_to_state_fallback();
 		}
 	}
 }
@@ -232,17 +228,15 @@ PrecLand::run_state_horizontal_approach()
 
 	// check if target visible, if not go to start
 	if (!check_state_conditions(PrecLandState::HorizontalApproach)) {
-		PX4_WARN("Lost landing target while landing (horizontal approach).");
+		PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
 
 		// Stay at current position for searching for the landing target
 		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
 		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
 
 		if (!switch_to_state_start()) {
-			if (!switch_to_state_fallback()) {
-				PX4_ERR("Can't switch to fallback landing");
-			}
+			switch_to_state_fallback();
 		}
 
 		return;
@@ -263,7 +257,6 @@ PrecLand::run_state_horizontal_approach()
 
 	}
 
-
 	float x = _target_pose.x_abs;
 	float y = _target_pose.y_abs;
 
@@ -286,17 +279,15 @@ PrecLand::run_state_descend_above_target()
 	// check if target visible
 	if (!check_state_conditions(PrecLandState::DescendAboveTarget)) {
 		if (!switch_to_state_final_approach()) {
-			PX4_WARN("Lost landing target while landing (descending).");
+			PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
 
 			// Stay at current position for searching for the target
 			pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
 			pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
 			pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
 
 			if (!switch_to_state_start()) {
-				if (!switch_to_state_fallback()) {
-					PX4_ERR("Can't switch to fallback landing");
-				}
+				switch_to_state_fallback();
 			}
 		}
 
@@ -345,9 +336,7 @@ PrecLand::run_state_search()
 	if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) {
 		PX4_WARN("Search timed out");
 
-		if (!switch_to_state_fallback()) {
-			PX4_ERR("Can't switch to fallback landing");
-		}
+		switch_to_state_fallback();
 	}
 }
 
@@ -380,7 +369,7 @@ bool
 PrecLand::switch_to_state_horizontal_approach()
 {
 	if (check_state_conditions(PrecLandState::HorizontalApproach)) {
-		PX4_INFO("Precland: switching to horizontal approach!");
+		print_state_switch_message("horizontal approach");
 		_approach_alt = _navigator->get_global_position()->alt;
 
 		_point_reached_time = 0;
@@ -397,7 +386,7 @@ bool
 PrecLand::switch_to_state_descend_above_target()
 {
 	if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
-		PX4_INFO("Precland: switching to descend!");
+		print_state_switch_message("descend");
 		_state = PrecLandState::DescendAboveTarget;
 		_state_start_time = hrt_absolute_time();
 		return true;
@@ -410,7 +399,7 @@ bool
 PrecLand::switch_to_state_final_approach()
 {
 	if (check_state_conditions(PrecLandState::FinalApproach)) {
-		PX4_INFO("Precland: switching ot final approach");
+		print_state_switch_message("final approach");
 		_state = PrecLandState::FinalApproach;
 		_state_start_time = hrt_absolute_time();
 		return true;
@@ -440,7 +429,7 @@ PrecLand::switch_to_state_search()
 bool
 PrecLand::switch_to_state_fallback()
 {
-	PX4_WARN("Falling back to normal land.");
+	print_state_switch_message("fallback");
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
 	pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
@@ -461,6 +450,11 @@ PrecLand::switch_to_state_done()
 	return true;
 }
 
+void PrecLand::print_state_switch_message(const char *state_name)
+{
+	PX4_INFO("Precland: switching to %s", state_name);
+}
+
 bool PrecLand::check_state_conditions(PrecLandState state)
 {
 	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();

diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h
@@ -101,6 +101,8 @@ class PrecLand : public MissionBlock, public ModuleParams
 	bool switch_to_state_fallback();
 	bool switch_to_state_done();
 
+	void print_state_switch_message(const char *state_name);
+
 	// check if a given state could be changed into. Return true if possible to transition to state, false otherwise
 	bool check_state_conditions(PrecLandState state);
 	void slewrate(float &sp_x, float &sp_y);