fix: use external precland if available during missions

asimopunov · Aug 28, 2024 · bb44e2c · bb44e2c
1 parent f3d58cd
commit bb44e2c
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 9 deletions.
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
@@ -184,6 +184,7 @@ Mission::do_need_move_to_takeoff()
 	return false;
 }
 
+
 void Mission::setActiveMissionItems()
 {
 	/* Get mission item that comes after current if available */

diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp
@@ -357,13 +357,6 @@ MissionBase::on_active()
 		do_abort_landing();
 	}
 
-	if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) {
-		_navigator->get_precland()->on_active();
-
-	} else if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
-	}
-
 	updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item);
 }
 

diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h
@@ -58,6 +58,9 @@
 #include "mission_block.h"
 #include "navigation.h"
 
+#include <commander/px4_custom_mode.h>
+#include <uORB/topics/vehicle_command.h>
+
 using namespace time_literals;
 
 class Navigator;

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
@@ -55,6 +55,32 @@
 
 using matrix::wrap_pi;
 
+
+static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
+				 const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),
+				 const double param6 = static_cast<double>(NAN), const float param7 = NAN)
+{
+	vehicle_command_s vcmd{};
+	vcmd.command = cmd;
+	vcmd.param1 = param1;
+	vcmd.param2 = param2;
+	vcmd.param3 = param3;
+	vcmd.param4 = param4;
+	vcmd.param5 = param5;
+	vcmd.param6 = param6;
+	vcmd.param7 = param7;
+
+	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
+	vcmd.source_system = vehicle_status_sub.get().system_id;
+	vcmd.target_system = vehicle_status_sub.get().system_id;
+	vcmd.source_component = vehicle_status_sub.get().component_id;
+	vcmd.target_component = vehicle_status_sub.get().component_id;
+
+	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	vcmd.timestamp = hrt_absolute_time();
+	return vcmd_pub.publish(vcmd);
+}
+
 MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) :
 	NavigatorMode(navigator, navigator_state_id)
 {
@@ -1004,12 +1030,13 @@ void MissionBlock::startPrecLand(uint16_t land_precision)
 {
 	if (_mission_item.land_precision == 1) {
 		_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-		_navigator->get_precland()->on_activation();
 
 	} else { //_mission_item.land_precision == 2
 		_navigator->get_precland()->set_mode(PrecLandMode::Required);
-		_navigator->get_precland()->on_activation();
 	}
+
+	send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
+			     PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
 }
 
 void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_item_s mission_item)

diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
@@ -53,6 +53,9 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
+#include <commander/px4_custom_mode.h>
+#include <uORB/topics/vehicle_command.h>
+
 // cosine of maximal course error to exit loiter if exit course is enforced (fixed-wing only)
 static constexpr float kCosineExitCourseThreshold = 0.99619f; // cos(5°)