zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Adopt high latency switching to new MAV_CMD design

sbg
acfloria 7 years ago committed by Beat Küng
parent
fa1586249a
commit
17a157b9da
4 changed files with 15 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      msg/vehicle_command.msg
  2. 28
      src/modules/commander/commander.cpp
  3. 16
      src/modules/mavlink/mavlink_main.cpp
  4. 2
      src/modules/mavlink/mavlink_messages.cpp

2
msg/vehicle_command.msg
Unescape 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_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_START = 2510 # start streaming ULog data
uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop 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_ACCEPTED = 0 # Command ACCEPTED and EXECUTED |
uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED |

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

@ -84,10 +84,6 @@
#include <cfloat> #include <cfloat>
#include <cstring> #include <cstring>
#ifndef __PX4_QURT
#include <mavlink/mavlink_main.h>
#endif
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
@ -146,11 +142,6 @@ typedef enum VEHICLE_MODE_FLAG
#define INAIR_RESTART_HOLDOFF_INTERVAL 500000 #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*/ /* 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) */ #define POSVEL_PROBATION_TAKEOFF 30 /**< probation duration set at takeoff (sec) */
@ -1315,9 +1306,7 @@ Commander::run()
/* command ack */ /* command ack */
orb_advert_t command_ack_pub = nullptr; orb_advert_t command_ack_pub = nullptr;
orb_advert_t commander_state_pub = nullptr; orb_advert_t commander_state_pub = nullptr;
#ifndef __PX4_QURT
orb_advert_t vehicle_cmd_pub = nullptr; orb_advert_t vehicle_cmd_pub = nullptr;
#endif
orb_advert_t vehicle_status_flags_pub = nullptr; 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 */ /* 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; status_changed = true;
} }
#ifndef __PX4_QURT
if (status.high_latency_data_link_active && have_low_latency_link) { if (status.high_latency_data_link_active && have_low_latency_link) {
// regained a low latency telemetry link, deactivate the high latency link // regained a low latency telemetry link, deactivate the high latency link
// to avoid transmitting unnecessary data over that 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"); mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING HIGH LATENCY LINK");
vehicle_command_s vehicle_cmd; vehicle_command_s vehicle_cmd;
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_MAVLINK_ENABLE_SENDING; vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
vehicle_cmd.param1 = Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM; vehicle_cmd.param1 = 0.0f;
vehicle_cmd.param2 = MAVLINK_DISABLE_TRANSMITTING;
if (vehicle_cmd_pub != nullptr) { if (vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd); 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); vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
} }
} }
#endif
} else { } else {
#ifndef __PX4_QURT
if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) { if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) {
// low latency telemetry lost and high latency link existing // low latency telemetry lost and high latency link existing
status.high_latency_data_link_active = true; status.high_latency_data_link_active = true;
status_changed = true; status_changed = true;
vehicle_command_s vehicle_cmd; vehicle_command_s vehicle_cmd;
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_MAVLINK_ENABLE_SENDING; vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
vehicle_cmd.param1 = Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM; vehicle_cmd.param1 = 1.0f;
vehicle_cmd.param2 = MAVLINK_ENABLE_TRANSMITTING;
if (vehicle_cmd_pub != nullptr) { if (vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd); 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"); mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK");
} }
} else if (!status.data_link_lost) { } else if (!status.data_link_lost) {
#else
if (!status.data_link_lost) {
#endif
if (armed.armed) { if (armed.armed) {
mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST"); mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
} }

16
src/modules/mavlink/mavlink_main.cpp
Unescape View File

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

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

@ -4202,7 +4202,7 @@ protected:
// failure flags, requires an initial value of 0, set by the default values // failure flags, requires an initial value of 0, set by the default values
if (_status_flags_time > 0) { 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; msg.failure_flags |= HL_FAILURE_FLAG_GPS;
} }

Write Preview
Loading…
Cancel
Save