diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ea7dae675217..c18c1fa1f835 100644 --- 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 index cee250095be1..0e0614e9c698 100644 --- 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 index f819b5c0e51a..e482fa0ff354 100644 --- 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 +#include + using namespace time_literals; class Navigator; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2659ad95cc57..3df5874eb29d 100644 --- 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(NAN), + const double param6 = static_cast(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_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 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 index c19fcfe11877..50399d097773 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -53,6 +53,9 @@ #include #include +#include +#include + // cosine of maximal course error to exit loiter if exit course is enforced (fixed-wing only) static constexpr float kCosineExitCourseThreshold = 0.99619f; // cos(5°)