fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Combine telemetry variables into struct

Browse Source
This commit is contained in:
acfloria 2018-04-05 15:06:26 +02:00 committed by Beat Küng
parent 6f0d3ffe9b
commit 219980cec1
2 changed files with 35 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -127,12 +127,14 @@ private:
int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed);
// telemetry variables
int _telemetry_subs[ORB_MULTI_MAX_INSTANCES] = {};
uint64_t _telemetry_last_heartbeat[ORB_MULTI_MAX_INSTANCES] = {};
uint64_t _telemetry_last_dl_loss[ORB_MULTI_MAX_INSTANCES] = {};
bool _telemetry_preflight_checks_reported[ORB_MULTI_MAX_INSTANCES] = {};
bool _telemetry_lost[ORB_MULTI_MAX_INSTANCES] = {};
bool _telemetry_high_latency[ORB_MULTI_MAX_INSTANCES] = {};
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];
// publisher
orb_advert_t _vehicle_cmd_pub = nullptr;

62
src/modules/commander/commander.cpp
View File

@ -627,14 +627,6 @@ Commander::Commander() :
ModuleParams(nullptr),
_mission_result_sub(ORB_ID(mission_result))
{
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
_telemetry_subs[i] = -1;
_telemetry_last_heartbeat[i] = 0;
_telemetry_last_dl_loss[i] = 0;
_telemetry_lost[i] = true;
_telemetry_preflight_checks_reported[i] = false;
_telemetry_high_latency[i] = false;
}
}
bool
@ -4095,20 +4087,20 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout)
bool updated = false;
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_telemetry_subs[i] < 0) {
_telemetry_subs[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i);
if (_telemetry[i].subscriber < 0) {
_telemetry[i].subscriber = orb_subscribe_multi(ORB_ID(telemetry_status), i);
}
orb_check(_telemetry_subs[i], &updated);
orb_check(_telemetry[i].subscriber, &updated);
if (updated) {
telemetry_status_s telemetry = {};
orb_copy(ORB_ID(telemetry_status), _telemetry_subs[i], &telemetry);
orb_copy(ORB_ID(telemetry_status), _telemetry[i].subscriber, &telemetry);
/* perform system checks when new telemetry link connected */
if (/* we first connect a link or re-connect a link after loosing it or haven't yet reported anything */
(_telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&_telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)
|| !_telemetry_preflight_checks_reported[i]) &&
(_telemetry[i].last_heartbeat == 0 || (hrt_elapsed_time(&_telemetry[i].last_heartbeat) > 3 * 1000 * 1000)
|| !_telemetry[i].preflight_checks_reported) &&
/* and this link has a communication partner */
(telemetry.heartbeat_time > 0) &&
/* and it is still connected */
@ -4118,7 +4110,7 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout)
*hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* flag the checks as reported for this link when we actually report them */
_telemetry_preflight_checks_reported[i] = *hotplug_timeout;
_telemetry[i].preflight_checks_reported = *hotplug_timeout;
/* provide RC and sensor status feedback to the user */
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
@ -4149,13 +4141,13 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout)
/* set latency type of the telemetry connection */
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) {
_telemetry_high_latency[i] = true;
_telemetry[i].high_latency = true;
} else {
_telemetry_high_latency[i] = false;
_telemetry[i].high_latency = false;
}
if (telemetry.heartbeat_time > 0 && (_telemetry_last_heartbeat[i] < telemetry.heartbeat_time)) {
_telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
if (telemetry.heartbeat_time > 0 && (_telemetry[i].last_heartbeat < telemetry.heartbeat_time)) {
_telemetry[i].last_heartbeat = telemetry.heartbeat_time;
}
}
}
@ -4172,7 +4164,7 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
int32_t dl_regain_timeout_local = 0;
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_telemetry_high_latency[i]) {
if (_telemetry[i].high_latency) {
high_latency_link_exists = true;
if (status.high_latency_data_link_active) {
dl_loss_timeout_local = highlatencydatalink_loss_timeout;
@ -4187,18 +4179,18 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
dl_regain_timeout_local = datalink_regain_timeout;
}
if (_telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&_telemetry_last_heartbeat[i]) < dl_loss_timeout_local * 1e6) {
if (_telemetry[i].last_heartbeat != 0 &&
hrt_elapsed_time(&_telemetry[i].last_heartbeat) < dl_loss_timeout_local * 1e6) {
/* handle the case where data link was gained first time or regained,
* accept datalink as healthy only after datalink_regain_timeout seconds
* */
if (_telemetry_lost[i] &&
hrt_elapsed_time(&_telemetry_last_dl_loss[i]) > dl_regain_timeout_local * 1e6) {
if (_telemetry[i].lost &&
hrt_elapsed_time(&_telemetry[i].last_dl_loss) > dl_regain_timeout_local * 1e6) {
/* report a regain */
if (_telemetry_last_dl_loss[i] > 0) {
if (_telemetry[i].last_dl_loss > 0) {
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i);
} else if (_telemetry_last_dl_loss[i] == 0) {
} else if (_telemetry[i].last_dl_loss == 0) {
/* new link */
}
@ -4206,29 +4198,29 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
status_flags.condition_system_prearm_error_reported = false;
*status_changed = true;
_telemetry_lost[i] = false;
_telemetry[i].lost = false;
have_link = true;
if (!_telemetry_high_latency[i]) {
if (!_telemetry[i].high_latency) {
have_low_latency_link = true;
}
} else if (!_telemetry_lost[i]) {
} else if (!_telemetry[i].lost) {
/* telemetry was healthy also in last iteration
* we don't have to check a timeout */
have_link = true;
if (!_telemetry_high_latency[i]) {
if (!_telemetry[i].high_latency) {
have_low_latency_link = true;
}
}
} else {
if (!_telemetry_lost[i]) {
if (!_telemetry[i].lost) {
/* only reset the timestamp to a different time on state change */
_telemetry_last_dl_loss[i] = hrt_absolute_time();
_telemetry[i].last_dl_loss = hrt_absolute_time();
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i);
_telemetry_lost[i] = true;
_telemetry[i].lost = true;
}
}
}
@ -4276,8 +4268,8 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
// set heartbeat to current time for high latency so that the first message can be transmitted
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_telemetry_high_latency[i]) {
_telemetry_last_heartbeat[i] = hrt_absolute_time();
if (_telemetry[i].high_latency) {
_telemetry[i].last_heartbeat = hrt_absolute_time();
}
}