From 47b08fd698e3206ed829ec8d0816d900c1affc83 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Mon, 4 Apr 2022 10:36:52 +0200 Subject: [PATCH] precland: save flash space --- src/modules/navigator/precland.cpp | 46 +++++++++++++----------------- src/modules/navigator/precland.h | 2 ++ 2 files changed, 22 insertions(+), 26 deletions(-) diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 65d8c83a22..080491d37c 100644 --- 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,7 +228,7 @@ 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; @@ -240,9 +236,7 @@ PrecLand::run_state_horizontal_approach() 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,7 +279,7 @@ 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; @@ -294,9 +287,7 @@ PrecLand::run_state_descend_above_target() 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 index 5bfaee5595..df07ff7bd5 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -101,6 +101,8 @@ private: 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);