Browse Source

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)
sbg
Thomas Gubler 11 years ago
parent
commit
94765f1fe0
  1. 14
      src/modules/navigator/datalinkloss.cpp
  2. 3
      src/modules/navigator/datalinkloss.h

14
src/modules/navigator/datalinkloss.cpp

@ -57,7 +57,6 @@
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),
_param_commsholdwaittime(this, "CH_T"), _param_commsholdwaittime(this, "CH_T"),
_param_commsholdlat(this, "CH_LAT"), _param_commsholdlat(this, "CH_LAT"),
_param_commsholdlon(this, "CH_LON"), _param_commsholdlon(this, "CH_LON"),
@ -91,6 +90,7 @@ void
DataLinkLoss::on_activation() DataLinkLoss::on_activation()
{ {
_dll_state = DLL_STATE_NONE; _dll_state = DLL_STATE_NONE;
updateParams();
advance_dll(); advance_dll();
set_dll_item(); set_dll_item();
} }
@ -99,6 +99,7 @@ void
DataLinkLoss::on_active() DataLinkLoss::on_active()
{ {
if (is_mission_item_reached()) { if (is_mission_item_reached()) {
updateParams();
advance_dll(); advance_dll();
set_dll_item(); set_dll_item();
} }
@ -109,9 +110,6 @@ DataLinkLoss::set_dll_item()
{ {
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); 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(); set_previous_pos_setpoint();
_navigator->set_can_loiter_at_sp(false); _navigator->set_can_loiter_at_sp(false);
@ -167,17 +165,15 @@ DataLinkLoss::set_dll_item()
void void
DataLinkLoss::advance_dll() DataLinkLoss::advance_dll()
{ {
warnx("dll_state %u", _dll_state);
switch (_dll_state) { switch (_dll_state) {
case DLL_STATE_NONE: case DLL_STATE_NONE:
/* Check the number of data link losses. If above home fly home directly */ /* Check the number of data link losses. If above home fly home directly */
updateSubscriptions(); if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
if (_vehicleStatus.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());
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"); 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, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: 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;
} }

3
src/modules/navigator/datalinkloss.h

@ -64,9 +64,6 @@ public:
virtual void on_active(); virtual void on_active();
private: private:
/* Subscriptions */
uORB::Subscription<vehicle_status_s> _vehicleStatus;
/* Params */ /* Params */
control::BlockParamFloat _param_commsholdwaittime; control::BlockParamFloat _param_commsholdwaittime;
control::BlockParamInt _param_commsholdlat; // * 1e7 control::BlockParamInt _param_commsholdlat; // * 1e7

Loading…
Cancel
Save