fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-14 16:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

HEARTBEAT and commander failsafe handling cleanup

Browse Source
This commit is contained in:
Daniel Agar
2019-03-12 11:24:33 +01:00
committed by Beat Küng
parent f794ee0c8a
commit 6dec451bab
10 changed files with 316 additions and 407 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.hpp
+22 -18
View File
@@ -93,6 +93,17 @@ public:
private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::NAV_DLL_ACT>) _datalink_loss_action,
(ParamInt<px4::params::COM_DL_LOSS_T>) _datalink_loss_threshold,
(ParamInt<px4::params::COM_DL_REG_T>) _datalink_regain_threshold,
(ParamInt<px4::params::COM_HLDL_LOSS_T>) _high_latency_datalink_loss_threshold,
(ParamInt<px4::params::COM_HLDL_REG_T>) _high_latency_datalink_regain_threshold,
(ParamInt<px4::params::NAV_RCL_ACT>) _rc_loss_action,
(ParamFloat<px4::params::COM_RC_LOSS_T>) _rc_loss_threshold,
(ParamFloat<px4::params::COM_HOME_H_T>) _home_eph_threshold,
(ParamFloat<px4::params::COM_HOME_V_T>) _home_epv_threshold,
@@ -155,27 +166,21 @@ private:
void mission_init();
/**
* Update the telemetry status and the corresponding status variables.
* Perform system checks when new telemetry link connected.
*/
void poll_telemetry_status();
/**
* Checks the status of all available data links and handles switching between different system telemetry states.
*/
void data_link_checks(int32_t highlatencydatalink_loss_timeout, int32_t highlatencydatalink_regain_timeout,
int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed);
void data_link_check(bool &status_changed);
int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {};
// telemetry variables
struct telemetry_data {
int subscriber = -1;
uint64_t last_heartbeat = 0u;
uint64_t last_dl_loss = 0u;
bool preflight_checks_reported = false;
bool lost = true;
bool high_latency = false;
} _telemetry[ORB_MULTI_MAX_INSTANCES];
uint64_t _datalink_last_heartbeat_gcs{0};
uint64_t _datalink_lost{0};
uint64_t _datalink_last_heartbeat_onboard_controller{0};
uint64_t _onboard_controller_lost{0};
int _iridiumsbd_status_sub{-1};
uint64_t _high_latency_datalink_heartbeat{0};
uint64_t _high_latency_datalink_lost{0};
void estimator_check(bool *status_changed);
@@ -190,7 +195,6 @@ private:
// Subscriptions
Subscription<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
Subscription<iridiumsbd_status_s> _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
Subscription<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
Subscription<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
Subscription<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};