fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 15:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

telemetry_statur: use 4 separate topics

Browse Source
This commit is contained in:
Anton Babushkin
2014-07-06 16:08:37 +02:00
parent d4eae37e47
commit bd5d3ebf70
7 changed files with 163 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+55 -32
View File
@@ -767,7 +767,6 @@ int commander_thread_main(int argc, char *argv[])
hrt_abstime last_idle_time = 0;
hrt_abstime start_time = 0;
hrt_abstime latest_heartbeat = 0;
bool status_changed = true;
bool param_init_forced = true;
@@ -797,10 +796,16 @@ int commander_thread_main(int argc, char *argv[])
struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
/* Subscribe to telemetry status */
int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
struct telemetry_status_s telemetry;
memset(&telemetry, 0, sizeof(telemetry));
/* Subscribe to telemetry status topics */
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
telemetry_last_heartbeat[i] = 0;
telemetry_lost[i] = true;
}
/* Subscribe to global position */
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -882,7 +887,6 @@ int commander_thread_main(int argc, char *argv[])
bool arming_state_changed = false;
bool main_state_changed = false;
bool failsafe_old = false;
bool system_checked = false;
while (!thread_should_exit) {
@@ -939,15 +943,6 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
}
/* Perform system checks (again) once params are loaded and MAVLink is up. */
if (!system_checked && mavlink_fd &&
(telemetry.heartbeat_time > 0) &&
(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
(void)rc_calibration_check(mavlink_fd);
system_checked = true;
}
orb_check(sp_man_sub, &updated);
if (updated) {
@@ -960,10 +955,26 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
}
orb_check(telemetry_sub, &updated);
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
orb_check(telemetry_subs[i], &updated);
if (updated) {
orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
if (updated) {
struct telemetry_status_s telemetry;
memset(&telemetry, 0, sizeof(telemetry));
orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
/* perform system checks when new telemetry link connected */
if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
(void)rc_calibration_check(mavlink_fd);
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
}
orb_check(sensor_sub, &updated);
@@ -1367,28 +1378,40 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* data link check */
if (telemetry.heartbeat_time >= latest_heartbeat) {
if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
/* handle the case where data link was regained */
if (status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: data link regained");
status.data_link_lost = false;
status_changed = true;
if (telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
telemetry_lost[i] = false;
}
/* Only consider data link with most recent heartbeat */
latest_heartbeat = telemetry.heartbeat_time;
have_link = true;
} else {
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
status.data_link_lost = true;
status_changed = true;
if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
telemetry_lost[i] = true;
}
}
}
if (have_link) {
/* handle the case where data link was regained */
if (status.data_link_lost) {
status.data_link_lost = false;
status_changed = true;
}
} else {
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
status.data_link_lost = true;
status_changed = true;
}
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);