mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-22 00:30:35 +08:00
make navigator mode a child of navigator
This commit is contained in:
@@ -58,14 +58,14 @@
|
|||||||
DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
|
DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
|
||||||
MissionBlock(navigator, name),
|
MissionBlock(navigator, name),
|
||||||
_vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100),
|
_vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100),
|
||||||
_param_commsholdwaittime(this, "NAV_DLL_CH_T", false),
|
_param_commsholdwaittime(this, "CH_T"),
|
||||||
_param_commsholdlat(this, "NAV_DLL_CH_LAT", false),
|
_param_commsholdlat(this, "CH_LAT"),
|
||||||
_param_commsholdlon(this, "NAV_DLL_CH_LON", false),
|
_param_commsholdlon(this, "CH_LON"),
|
||||||
_param_commsholdalt(this, "NAV_DLL_CH_ALT", false),
|
_param_commsholdalt(this, "CH_ALT"),
|
||||||
_param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false),
|
_param_airfieldhomelat(this, "AH_LAT"),
|
||||||
_param_airfieldhomelon(this, "NAV_DLL_AH_LON", false),
|
_param_airfieldhomelon(this, "AH_LON"),
|
||||||
_param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false),
|
_param_airfieldhomealt(this, "AH_ALT"),
|
||||||
_param_numberdatalinklosses(this, "NAV_DLL_N", false),
|
_param_numberdatalinklosses(this, "N"),
|
||||||
_dll_state(DLL_STATE_NONE)
|
_dll_state(DLL_STATE_NONE)
|
||||||
{
|
{
|
||||||
/* load initial params */
|
/* load initial params */
|
||||||
@@ -182,14 +182,17 @@ DataLinkLoss::advance_dll()
|
|||||||
updateSubscriptions();
|
updateSubscriptions();
|
||||||
if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) {
|
if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) {
|
||||||
warnx("too many data link losses, fly to airfield home");
|
warnx("too many data link losses, fly to airfield home");
|
||||||
|
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home");
|
||||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||||
} else {
|
} else {
|
||||||
warnx("fly to comms hold");
|
warnx("fly to comms hold");
|
||||||
|
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
|
||||||
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
|
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case DLL_STATE_FLYTOCOMMSHOLDWP:
|
case DLL_STATE_FLYTOCOMMSHOLDWP:
|
||||||
warnx("fly to airfield home");
|
warnx("fly to airfield home");
|
||||||
|
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to home");
|
||||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||||
_navigator->publish_mission_result();
|
_navigator->publish_mission_result();
|
||||||
|
|||||||
@@ -59,9 +59,9 @@
|
|||||||
|
|
||||||
Mission::Mission(Navigator *navigator, const char *name) :
|
Mission::Mission(Navigator *navigator, const char *name) :
|
||||||
MissionBlock(navigator, name),
|
MissionBlock(navigator, name),
|
||||||
_param_onboard_enabled(this, "ONBOARD_EN"),
|
_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
|
||||||
_param_takeoff_alt(this, "TAKEOFF_ALT"),
|
_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
|
||||||
_param_dist_1wp(this, "DIST_1WP"),
|
_param_dist_1wp(this, "MIS_DIST_1WP", false),
|
||||||
_onboard_mission({0}),
|
_onboard_mission({0}),
|
||||||
_offboard_mission({0}),
|
_offboard_mission({0}),
|
||||||
_current_onboard_mission_index(-1),
|
_current_onboard_mission_index(-1),
|
||||||
|
|||||||
@@ -43,7 +43,7 @@
|
|||||||
#include "navigator.h"
|
#include "navigator.h"
|
||||||
|
|
||||||
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
|
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
|
||||||
SuperBlock(NULL, name),
|
SuperBlock(navigator, name),
|
||||||
_navigator(navigator),
|
_navigator(navigator),
|
||||||
_first_run(true)
|
_first_run(true)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -58,9 +58,9 @@
|
|||||||
RTL::RTL(Navigator *navigator, const char *name) :
|
RTL::RTL(Navigator *navigator, const char *name) :
|
||||||
MissionBlock(navigator, name),
|
MissionBlock(navigator, name),
|
||||||
_rtl_state(RTL_STATE_NONE),
|
_rtl_state(RTL_STATE_NONE),
|
||||||
_param_return_alt(this, "RETURN_ALT"),
|
_param_return_alt(this, "RTL_RETURN_ALT", false),
|
||||||
_param_descend_alt(this, "DESCEND_ALT"),
|
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
|
||||||
_param_land_delay(this, "LAND_DELAY")
|
_param_land_delay(this, "RTL_LAND_DELAY", false)
|
||||||
{
|
{
|
||||||
/* load initial params */
|
/* load initial params */
|
||||||
updateParams();
|
updateParams();
|
||||||
|
|||||||
Reference in New Issue
Block a user