dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: changed "datalink regain" logic. 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2019-03-07 14:45:34 +01:00 committed by Beat Küng
parent ca53217175
commit a53594135b
3 changed files with 8 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/commander/Commander.cpp
View File

@ -3890,30 +3890,32 @@ void Commander::data_link_check(bool &status_changed)
// handle different remote types
switch (telemetry.remote_type) {
case telemetry_status_s::MAV_TYPE_GCS:
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
// Recover from data link lost
if (status.data_link_lost) {
if (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) < (_datalink_regain_threshold.get() * 1_s)) {
if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false;
status_changed = true;
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
}
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
break;
case telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER:
_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
if (_onboard_controller_lost) {
if (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) < _onboard_regain_threshold.get() * 1_s) {
if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false;
}
}
_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
if (telemetry.heartbeat_time != _datalink_last_heartbeat_avoidance_system) {
_avoidance_system_status_change = _datalink_last_status_avoidance_system != telemetry.remote_system_status;

4
src/modules/commander/Commander.hpp
View File

@ -96,7 +96,6 @@ private:
(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,
@ -120,8 +119,7 @@ private:
(ParamInt<px4::params::COM_OBS_AVOID>) _obs_avoid,
(ParamInt<px4::params::COM_ONB_LOSS_T>) _onboard_loss_timeout,
(ParamInt<px4::params::COM_ONB_REG_T>) _onboard_regain_threshold
(ParamInt<px4::params::COM_ONB_LOSS_T>) _onboard_loss_timeout
)
const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */

31
src/modules/commander/commander_params.c
View File

@ -103,21 +103,6 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
/**
* Datalink regain time threshold
*
* After a data link loss: Amount of time between two heartbeats to consider datalink healthy again and
* set 'datalink loss' flag back to false.
*
* @group Commander
* @unit s
* @min 0
* @max 3
* @decimal 1
* @increment 0.5
*/
PARAM_DEFINE_INT32(COM_DL_REG_T, 2);
/**
* High Latency Datalink loss time threshold
*
@ -831,18 +816,4 @@ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
* @max 60
* @increment 1
*/
PARAM_DEFINE_INT32(COM_ONB_LOSS_T, 5);
/**
* Onboard link regain time threshold
*
* After a onboard link loss: Amount of time between two heartbeats to consider connection healthy again.
*
* @group Commander
* @unit s
* @min 0
* @max 3
* @decimal 1
* @increment 0.5
*/
PARAM_DEFINE_INT32(COM_ONB_REG_T, 1);
PARAM_DEFINE_INT32(COM_ONB_LOSS_T, 5);