Browse Source

add mavlink mode external vision MAVLINK_MODE_EXTVISION

the mode can be used for VIO algorithms and obstacle avoidance to close the loop
sbg
Anna Dai 6 years ago committed by Beat Küng
parent
commit
3db901b238
  1. 26
      src/modules/mavlink/mavlink_main.cpp
  2. 4
      src/modules/mavlink/mavlink_main.h
  3. 1
      src/modules/mavlink/module.yaml

26
src/modules/mavlink/mavlink_main.cpp

@ -1784,6 +1784,27 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) @@ -1784,6 +1784,27 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("WIND_COV", 10.0f);
break;
case MAVLINK_MODE_EXTVISION:
configure_stream_local("ALTITUDE", 10.0f); // for avoidance
configure_stream_local("ATTITUDE", 25.0f);
configure_stream_local("ATTITUDE_TARGET", 10.0f);
configure_stream_local("CAMERA_TRIGGER", unlimited_rate); // for VIO
configure_stream_local("DISTANCE_SENSOR", 10.0f); // for avoidance
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GPS_RAW_INT", 1.0f);
configure_stream_local("HOME_POSITION", 0.5f);
configure_stream_local("HIGHRES_IMU", unlimited_rate); // for VIO
configure_stream_local("LOCAL_POSITION_NED", 30.0f); //for VIO and avoidance
configure_stream_local("SYS_STATUS", 5.0f);
configure_stream_local("SYSTEM_TIME", 1.0f);
configure_stream_local("TIMESYNC", 10.0f);
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); // for avoidance
configure_stream_local("VFR_HUD", 25.0f);
configure_stream_local("WIND_COV", 2.0f);
break;
case MAVLINK_MODE_OSD:
configure_stream_local("ALTITUDE", 1.0f);
configure_stream_local("ATTITUDE", 25.0f);
@ -2068,6 +2089,9 @@ Mavlink::task_main(int argc, char *argv[]) @@ -2068,6 +2089,9 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(myoptarg, "minimal") == 0) {
_mode = MAVLINK_MODE_MINIMAL;
} else if (strcmp(myoptarg, "extvision") == 0) {
_mode = MAVLINK_MODE_EXTVISION;
} else {
PX4_ERR("invalid mode");
err_flag = true;
@ -3049,7 +3073,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 @@ -3049,7 +3073,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr,
"Partner IP (broadcasting can be enabled via MAV_BROADCAST param)", true);
#endif
PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal",
PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvsision",
"Mode: sets default streams and rates", true);
PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "<interface_name>", "wifi/ethernet interface name", true);
#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)

4
src/modules/mavlink/mavlink_main.h

@ -181,6 +181,7 @@ public: @@ -181,6 +181,7 @@ public:
MAVLINK_MODE_CONFIG,
MAVLINK_MODE_IRIDIUM,
MAVLINK_MODE_MINIMAL,
MAVLINK_MODE_EXTVISION,
MAVLINK_MODE_COUNT
};
@ -224,6 +225,9 @@ public: @@ -224,6 +225,9 @@ public:
case MAVLINK_MODE_MINIMAL:
return "Minimal";
case MAVLINK_MODE_EXTVISION:
return "ExtVision";
default:
return "Unknown";
}

1
src/modules/mavlink/module.yaml

@ -39,6 +39,7 @@ parameters: @@ -39,6 +39,7 @@ parameters:
5: Config
#6: Iridium # as the user does not need to configure this, hide it from the UI
7: Minimal
8: External Vision
reboot_required: true
num_instances: *max_num_config_instances
default: [0, 2, 0]

Loading…
Cancel
Save