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

Adopt high latency switching to new MAV_CMD design

Browse Source
This commit is contained in:
acfloria
2018-02-26 15:54:38 +01:00
committed by Beat Küng
parent fa1586249a
commit 17a157b9da
4 changed files with 14 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_command.msg
View File

@@ -72,7 +72,7 @@ uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed pay
uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data
uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data
uint16 VEHICLE_CMD_MAVLINK_ENABLE_SENDING = 2600 # Start/Stop transmitting data from all instances in mavlink from a certain type
uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # control starting/stopping transmitting data over the high latency link
uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED |
uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED |

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

@@ -84,10 +84,6 @@
#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>
#include <uORB/topics/battery_status.h>
@@ -146,11 +142,6 @@ 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) */
@@ -1315,9 +1306,7 @@ 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 */
@@ -2607,7 +2596,6 @@ 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
@@ -2616,9 +2604,8 @@ Commander::run()
mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING 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;
vehicle_cmd.param2 = MAVLINK_DISABLE_TRANSMITTING;
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
vehicle_cmd.param1 = 0.0f;
if (vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd);
@@ -2626,18 +2613,14 @@ Commander::run()
vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
}
}
#endif
} else {
#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
status.high_latency_data_link_active = true;
status_changed = true;
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;
vehicle_cmd.param2 = MAVLINK_ENABLE_TRANSMITTING;
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
vehicle_cmd.param1 = 1.0f;
if (vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd);
@@ -2651,9 +2634,6 @@ Commander::run()
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");
}

20
src/modules/mavlink/mavlink_main.cpp
View File

@@ -990,6 +990,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
return;
}
_last_write_try_time = hrt_absolute_time();
if (_mavlink_start_time == 0) {
@@ -1036,7 +1037,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
}
void
@@ -2200,18 +2200,14 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_command_s vehicle_cmd;
if (cmd_sub->update(&cmd_time, &vehicle_cmd)) {
if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_MAVLINK_ENABLE_SENDING) {
if (_mode == (int)roundf(vehicle_cmd.param1)) {
if (_transmitting_enabled != (int)vehicle_cmd.param2) {
if ((int)vehicle_cmd.param2) {
PX4_INFO("Enable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && (_mode == MAVLINK_MODE_IRIDIUM)) {
if (!_transmitting_enabled && (vehicle_cmd.param1 > 0.5f)) {
PX4_INFO("Enable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
_transmitting_enabled = true;
} else {
PX4_INFO("Disable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
}
_transmitting_enabled = (int)vehicle_cmd.param2;
}
} else if (_transmitting_enabled && (vehicle_cmd.param1 <= 0.5f)) {
PX4_INFO("Disable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
_transmitting_enabled = false;
}
}
}

2
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -4202,7 +4202,7 @@ protected:
// failure flags, requires an initial value of 0, set by the default values
if (_status_flags_time > 0) {
if (status_flags.gps_failure || !status_flags.condition_global_position_valid) {
if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure
msg.failure_flags |= HL_FAILURE_FLAG_GPS;
}