mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-06 12:48:54 +08:00
datalinkloss: use vstatus from navigator
For some reason the own subscription did not work (the task launch pattern used for the navigator may be the reason again)
This commit is contained in:
@@ -57,7 +57,6 @@
|
||||
|
||||
DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100),
|
||||
_param_commsholdwaittime(this, "CH_T"),
|
||||
_param_commsholdlat(this, "CH_LAT"),
|
||||
_param_commsholdlon(this, "CH_LON"),
|
||||
@@ -91,6 +90,7 @@ void
|
||||
DataLinkLoss::on_activation()
|
||||
{
|
||||
_dll_state = DLL_STATE_NONE;
|
||||
updateParams();
|
||||
advance_dll();
|
||||
set_dll_item();
|
||||
}
|
||||
@@ -99,6 +99,7 @@ void
|
||||
DataLinkLoss::on_active()
|
||||
{
|
||||
if (is_mission_item_reached()) {
|
||||
updateParams();
|
||||
advance_dll();
|
||||
set_dll_item();
|
||||
}
|
||||
@@ -109,9 +110,6 @@ DataLinkLoss::set_dll_item()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
/* make sure we have the latest params */
|
||||
updateParams();
|
||||
|
||||
set_previous_pos_setpoint();
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
@@ -167,17 +165,15 @@ DataLinkLoss::set_dll_item()
|
||||
void
|
||||
DataLinkLoss::advance_dll()
|
||||
{
|
||||
warnx("dll_state %u", _dll_state);
|
||||
switch (_dll_state) {
|
||||
case DLL_STATE_NONE:
|
||||
/* Check the number of data link losses. If above home fly home directly */
|
||||
updateSubscriptions();
|
||||
if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) {
|
||||
warnx("too many data link losses, fly to airfield home");
|
||||
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
|
||||
warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home");
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
} else {
|
||||
warnx("fly to comms hold");
|
||||
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
|
||||
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
|
||||
}
|
||||
|
||||
@@ -64,9 +64,6 @@ public:
|
||||
virtual void on_active();
|
||||
|
||||
private:
|
||||
/* Subscriptions */
|
||||
uORB::Subscription<vehicle_status_s> _vehicleStatus;
|
||||
|
||||
/* Params */
|
||||
control::BlockParamFloat _param_commsholdwaittime;
|
||||
control::BlockParamInt _param_commsholdlat; // * 1e7
|
||||
|
||||
Reference in New Issue
Block a user