yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

commander: Fix for qurt and improve high latency switch logic

Browse Source
This commit is contained in:
acfloria
2018-01-11 10:42:37 +01:00
committed by Beat Küng
parent 1cc84ad8c9
commit a026575e37
1 changed files with 19 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -84,7 +84,9 @@
#include <cfloat>
#include <cstring>
#ifndef __PX4_QURT
#include <mavlink/mavlink_main.h>
#endif
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
@@ -144,8 +146,10 @@ typedef enum VEHICLE_MODE_FLAG
#define INAIR_RESTART_HOLDOFF_INTERVAL 500000
#ifndef __PX4_QURT
#define MAVLINK_ENABLE_TRANSMITTING 1 /**< parameter value to enable transmitting for a mavlink instance */
#define MAVLINK_DISABLE_TRANSMITTING 0 /**< parameter value to disable transmitting for a mavlink instance */
#endif
/* Controls the probation period which is the amount of time required for position and velocity checks to pass before the validity can be changed from false to true*/
#define POSVEL_PROBATION_TAKEOFF 30 /**< probation duration set at takeoff (sec) */
@@ -1311,7 +1315,9 @@ Commander::run()
/* command ack */
orb_advert_t command_ack_pub = nullptr;
orb_advert_t commander_state_pub = nullptr;
#ifndef __PX4_QURT
orb_advert_t vehicle_cmd_pub = nullptr;
#endif
orb_advert_t vehicle_status_flags_pub = nullptr;
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
@@ -2601,6 +2607,7 @@ Commander::run()
status_changed = true;
}
#ifndef __PX4_QURT
if (status.high_latency_data_link_active && have_low_latency_link) {
// regained a low latency telemetry link, deactivate the high latency link
// to avoid transmitting unnecessary data over that link
@@ -2619,20 +2626,14 @@ Commander::run()
vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
}
}
#endif
} else {
if (!status.data_link_lost && high_latency_link_exists && !status.high_latency_data_link_active && armed.armed &&
internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL ) {
#ifndef __PX4_QURT
if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) {
// low latency telemetry lost and high latency link existing
// only go to the high latency status if armed and not in a manual mode to avoid unnecessary transmitting data
status.high_latency_data_link_active = true;
status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK");
vehicle_command_s vehicle_cmd;
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_MAVLINK_ENABLE_SENDING;
vehicle_cmd.param1 = Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM;
@@ -2643,7 +2644,16 @@ Commander::run()
} else {
vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
}
if (!status.data_link_lost) {
mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK");
} else {
mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK");
}
} else if (!status.data_link_lost) {
#else
if (!status.data_link_lost) {
#endif
if (armed.armed) {
mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
}