diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index baa6ef35d1..8c6dd76b5a 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -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 | diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fcf1bc24ff..849d65b7c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,10 +84,6 @@ #include #include -#ifndef __PX4_QURT -#include -#endif - #include #include #include @@ -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"); } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3e382d7a14..c570ec12de 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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); - - } 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; - } + 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 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; } } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1407cc86f0..c5072de03b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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; }