mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mavlink: allow resetting mavlink streams to default via MAV_CMD_SET_MESSAGE_INTERVAL
This implementation does not need more resources. It's not super efficient in terms of runtime, but it's also not something that is called often.
This commit is contained in:
Beat Küng
Lorenz Meier
parent
333fd9cf45
commit
84841236cb
@ -1366,6 +1366,8 @@ Mavlink::interval_from_rate(float rate)
|
||||
int
|
||||
Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||
{
|
||||
PX4_DEBUG("configure_stream(%s, %.3f)", stream_name, (double)rate);
|
||||
|
||||
/* calculate interval in us, -1 means unlimited stream, 0 means disabled */
|
||||
int interval = interval_from_rate(rate);
|
||||
|
||||
@ -1722,6 +1724,189 @@ Mavlink::update_rate_mult()
|
||||
_rate_mult = fmaxf(0.05f, _rate_mult);
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||
{
|
||||
int ret = 0;
|
||||
bool stream_configured = false;
|
||||
|
||||
auto configure_stream_local =
|
||||
[&stream_configured, configure_single_stream, &ret, this](const char *stream_name, float rate) {
|
||||
if (!configure_single_stream || strcmp(configure_single_stream, stream_name) == 0) {
|
||||
int ret_local = configure_stream(stream_name, rate);
|
||||
|
||||
if (ret_local != 0) {
|
||||
ret = ret_local;
|
||||
}
|
||||
|
||||
stream_configured = true;
|
||||
}
|
||||
};
|
||||
|
||||
const float unlimited_rate = -1.f;
|
||||
|
||||
switch (_mode) {
|
||||
case MAVLINK_MODE_NORMAL:
|
||||
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
||||
configure_stream_local("ALTITUDE", 1.0f);
|
||||
configure_stream_local("ATTITUDE", 20.0f);
|
||||
configure_stream_local("ATTITUDE_TARGET", 2.0f);
|
||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
||||
configure_stream_local("COLLISION", unlimited_rate);
|
||||
configure_stream_local("DEBUG", 1.0f);
|
||||
configure_stream_local("DEBUG_VECT", 1.0f);
|
||||
configure_stream_local("DISTANCE_SENSOR", 0.5f);
|
||||
configure_stream_local("ESTIMATOR_STATUS", 0.5f);
|
||||
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
|
||||
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
|
||||
configure_stream_local("GPS_RAW_INT", 1.0f);
|
||||
configure_stream_local("HIGHRES_IMU", 1.5f);
|
||||
configure_stream_local("HOME_POSITION", 0.5f);
|
||||
configure_stream_local("LOCAL_POSITION_NED", 1.0f);
|
||||
configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
|
||||
configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
|
||||
configure_stream_local("OPTICAL_FLOW_RAD", 1.0f);
|
||||
configure_stream_local("PING", 0.1f);
|
||||
configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
|
||||
configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f);
|
||||
configure_stream_local("RC_CHANNELS", 5.0f);
|
||||
configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
|
||||
configure_stream_local("SYS_STATUS", 1.0f);
|
||||
configure_stream_local("VFR_HUD", 4.0f);
|
||||
configure_stream_local("VISION_POSITION_ESTIMATE", 1.0f);
|
||||
configure_stream_local("WIND_COV", 1.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_ONBOARD:
|
||||
configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f);
|
||||
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
||||
configure_stream_local("ALTITUDE", 10.0f);
|
||||
configure_stream_local("ATTITUDE", 100.0f);
|
||||
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
|
||||
configure_stream_local("ATTITUDE_TARGET", 10.0f);
|
||||
configure_stream_local("CAMERA_CAPTURE", 2.0f);
|
||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
||||
configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
|
||||
configure_stream_local("COLLISION", unlimited_rate);
|
||||
configure_stream_local("DEBUG", 10.0f);
|
||||
configure_stream_local("DEBUG_VECT", 10.0f);
|
||||
configure_stream_local("DISTANCE_SENSOR", 10.0f);
|
||||
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
||||
configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
|
||||
configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
|
||||
configure_stream_local("GPS_RAW_INT", unlimited_rate);
|
||||
configure_stream_local("HIGHRES_IMU", 50.0f);
|
||||
configure_stream_local("HOME_POSITION", 0.5f);
|
||||
configure_stream_local("LOCAL_POSITION_NED", 30.0f);
|
||||
configure_stream_local("NAMED_VALUE_FLOAT", 10.0f);
|
||||
configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
|
||||
configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
|
||||
configure_stream_local("PING", 1.0f);
|
||||
configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
|
||||
configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f);
|
||||
configure_stream_local("RC_CHANNELS", 20.0f);
|
||||
configure_stream_local("SCALED_IMU", 50.0f);
|
||||
configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f);
|
||||
configure_stream_local("SYS_STATUS", 5.0f);
|
||||
configure_stream_local("SYSTEM_TIME", 1.0f);
|
||||
configure_stream_local("TIMESYNC", 10.0f);
|
||||
configure_stream_local("VFR_HUD", 10.0f);
|
||||
configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f);
|
||||
configure_stream_local("WIND_COV", 10.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_OSD:
|
||||
configure_stream_local("ALTITUDE", 1.0f);
|
||||
configure_stream_local("ATTITUDE", 25.0f);
|
||||
configure_stream_local("ATTITUDE_TARGET", 10.0f);
|
||||
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("RC_CHANNELS", 5.0f);
|
||||
configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
|
||||
configure_stream_local("SYS_STATUS", 5.0f);
|
||||
configure_stream_local("SYSTEM_TIME", 1.0f);
|
||||
configure_stream_local("VFR_HUD", 25.0f);
|
||||
configure_stream_local("WIND_COV", 2.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_MAGIC:
|
||||
|
||||
/* fallthrough */
|
||||
case MAVLINK_MODE_CUSTOM:
|
||||
//stream nothing
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_CONFIG:
|
||||
// Enable a number of interesting streams we want via USB
|
||||
configure_stream_local("ACTUATOR_CONTROL_TARGET0", 30.0f);
|
||||
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
||||
configure_stream_local("ALTITUDE", 10.0f);
|
||||
configure_stream_local("ATTITUDE", 50.0f);
|
||||
configure_stream_local("ATTITUDE_TARGET", 8.0f);
|
||||
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
|
||||
configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
|
||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
||||
configure_stream_local("COLLISION", unlimited_rate);
|
||||
configure_stream_local("DEBUG", 50.0f);
|
||||
configure_stream_local("DEBUG_VECT", 50.0f);
|
||||
configure_stream_local("DISTANCE_SENSOR", 10.0f);
|
||||
configure_stream_local("GPS_RAW_INT", unlimited_rate);
|
||||
configure_stream_local("ESTIMATOR_STATUS", 5.0f);
|
||||
configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
|
||||
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
|
||||
configure_stream_local("HIGHRES_IMU", 50.0f);
|
||||
configure_stream_local("HOME_POSITION", 0.5f);
|
||||
configure_stream_local("LOCAL_POSITION_NED", 30.0f);
|
||||
configure_stream_local("MANUAL_CONTROL", 5.0f);
|
||||
configure_stream_local("NAMED_VALUE_FLOAT", 50.0f);
|
||||
configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
|
||||
configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
|
||||
configure_stream_local("PING", 1.0f);
|
||||
configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
|
||||
configure_stream_local("RC_CHANNELS", 10.0f);
|
||||
configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
|
||||
configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
|
||||
configure_stream_local("SYS_STATUS", 1.0f);
|
||||
configure_stream_local("SYSTEM_TIME", 1.0f);
|
||||
configure_stream_local("TIMESYNC", 10.0f);
|
||||
configure_stream_local("VFR_HUD", 20.0f);
|
||||
configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f);
|
||||
configure_stream_local("WIND_COV", 10.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_IRIDIUM:
|
||||
configure_stream_local("HIGH_LATENCY2", 0.015f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_MINIMAL:
|
||||
configure_stream_local("ALTITUDE", 0.5f);
|
||||
configure_stream_local("ATTITUDE", 10.0f);
|
||||
configure_stream_local("EXTENDED_SYS_STATE", 0.1f);
|
||||
configure_stream_local("GPS_RAW_INT", 0.5f);
|
||||
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
|
||||
configure_stream_local("HOME_POSITION", 0.1f);
|
||||
configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
|
||||
configure_stream_local("RC_CHANNELS", 0.5f);
|
||||
configure_stream_local("SYS_STATUS", 0.1f);
|
||||
configure_stream_local("VFR_HUD", 1.0f);
|
||||
break;
|
||||
|
||||
default:
|
||||
ret = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
if (configure_single_stream && !stream_configured && strcmp(configure_single_stream, "HEARTBEAT") != 0) {
|
||||
// stream was not found, assume it is disabled by default
|
||||
return configure_stream(configure_single_stream, 0.f);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::task_main(int argc, char *argv[])
|
||||
{
|
||||
@ -1999,154 +2184,8 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case MAVLINK_MODE_NORMAL:
|
||||
configure_stream("ADSB_VEHICLE");
|
||||
configure_stream("ALTITUDE", 1.0f);
|
||||
configure_stream("ATTITUDE", 20.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 2.0f);
|
||||
configure_stream("CAMERA_IMAGE_CAPTURED");
|
||||
configure_stream("COLLISION");
|
||||
configure_stream("DEBUG", 1.0f);
|
||||
configure_stream("DEBUG_VECT", 1.0f);
|
||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||
configure_stream("ESTIMATOR_STATUS", 0.5f);
|
||||
configure_stream("EXTENDED_SYS_STATE", 1.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 5.0f);
|
||||
configure_stream("GPS_RAW_INT", 1.0f);
|
||||
configure_stream("HIGHRES_IMU", 1.5f);
|
||||
configure_stream("HOME_POSITION", 0.5f);
|
||||
configure_stream("LOCAL_POSITION_NED", 1.0f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
|
||||
configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f);
|
||||
configure_stream("OPTICAL_FLOW_RAD", 1.0f);
|
||||
configure_stream("PING", 0.1f);
|
||||
configure_stream("POSITION_TARGET_LOCAL_NED", 1.5f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f);
|
||||
configure_stream("RC_CHANNELS", 5.0f);
|
||||
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
|
||||
configure_stream("SYS_STATUS", 1.0f);
|
||||
configure_stream("VFR_HUD", 4.0f);
|
||||
configure_stream("VISION_POSITION_ESTIMATE", 1.0f);
|
||||
configure_stream("WIND_COV", 1.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_ONBOARD:
|
||||
configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
|
||||
configure_stream("ADSB_VEHICLE");
|
||||
configure_stream("ALTITUDE", 10.0f);
|
||||
configure_stream("ATTITUDE", 100.0f);
|
||||
configure_stream("ATTITUDE_QUATERNION", 50.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 10.0f);
|
||||
configure_stream("CAMERA_CAPTURE", 2.0f);
|
||||
configure_stream("CAMERA_IMAGE_CAPTURED");
|
||||
configure_stream("CAMERA_TRIGGER");
|
||||
configure_stream("COLLISION");
|
||||
configure_stream("DEBUG", 10.0f);
|
||||
configure_stream("DEBUG_VECT", 10.0f);
|
||||
configure_stream("DISTANCE_SENSOR", 10.0f);
|
||||
configure_stream("ESTIMATOR_STATUS", 1.0f);
|
||||
configure_stream("EXTENDED_SYS_STATE", 5.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 50.0f);
|
||||
configure_stream("GPS_RAW_INT");
|
||||
configure_stream("HIGHRES_IMU", 50.0f);
|
||||
configure_stream("HOME_POSITION", 0.5f);
|
||||
configure_stream("LOCAL_POSITION_NED", 30.0f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
|
||||
configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
|
||||
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
||||
configure_stream("PING", 1.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
|
||||
configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
|
||||
configure_stream("RC_CHANNELS", 20.0f);
|
||||
configure_stream("SCALED_IMU", 50.0f);
|
||||
configure_stream("SERVO_OUTPUT_RAW_0", 10.0f);
|
||||
configure_stream("SYS_STATUS", 5.0f);
|
||||
configure_stream("SYSTEM_TIME", 1.0f);
|
||||
configure_stream("TIMESYNC", 10.0f);
|
||||
configure_stream("VFR_HUD", 10.0f);
|
||||
configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
|
||||
configure_stream("WIND_COV", 10.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_OSD:
|
||||
configure_stream("ALTITUDE", 1.0f);
|
||||
configure_stream("ATTITUDE", 25.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 10.0f);
|
||||
configure_stream("ESTIMATOR_STATUS", 1.0f);
|
||||
configure_stream("EXTENDED_SYS_STATE", 1.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 10.0f);
|
||||
configure_stream("GPS_RAW_INT", 1.0f);
|
||||
configure_stream("HOME_POSITION", 0.5f);
|
||||
configure_stream("RC_CHANNELS", 5.0f);
|
||||
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
|
||||
configure_stream("SYS_STATUS", 5.0f);
|
||||
configure_stream("SYSTEM_TIME", 1.0f);
|
||||
configure_stream("VFR_HUD", 25.0f);
|
||||
configure_stream("WIND_COV", 2.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_MAGIC:
|
||||
//stream nothing
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_CONFIG:
|
||||
// Enable a number of interesting streams we want via USB
|
||||
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
|
||||
configure_stream("ADSB_VEHICLE");
|
||||
configure_stream("ALTITUDE", 10.0f);
|
||||
configure_stream("ATTITUDE", 50.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 8.0f);
|
||||
configure_stream("ATTITUDE_QUATERNION", 50.0f);
|
||||
configure_stream("CAMERA_TRIGGER");
|
||||
configure_stream("CAMERA_IMAGE_CAPTURED");
|
||||
configure_stream("COLLISION");
|
||||
configure_stream("DEBUG", 50.0f);
|
||||
configure_stream("DEBUG_VECT", 50.0f);
|
||||
configure_stream("DISTANCE_SENSOR", 10.0f);
|
||||
configure_stream("GPS_RAW_INT");
|
||||
configure_stream("ESTIMATOR_STATUS", 5.0f);
|
||||
configure_stream("EXTENDED_SYS_STATE", 2.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 10.0f);
|
||||
configure_stream("HIGHRES_IMU", 50.0f);
|
||||
configure_stream("HOME_POSITION", 0.5f);
|
||||
configure_stream("LOCAL_POSITION_NED", 30.0f);
|
||||
configure_stream("MANUAL_CONTROL", 5.0f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
|
||||
configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
|
||||
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
||||
configure_stream("PING", 1.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
|
||||
configure_stream("RC_CHANNELS", 10.0f);
|
||||
configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
|
||||
configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
|
||||
configure_stream("SYS_STATUS", 1.0f);
|
||||
configure_stream("SYSTEM_TIME", 1.0f);
|
||||
configure_stream("TIMESYNC", 10.0f);
|
||||
configure_stream("VFR_HUD", 20.0f);
|
||||
configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
|
||||
configure_stream("WIND_COV", 10.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_IRIDIUM:
|
||||
configure_stream("HIGH_LATENCY2", 0.015f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_MINIMAL:
|
||||
configure_stream("ALTITUDE", 0.5f);
|
||||
configure_stream("ATTITUDE", 10.0f);
|
||||
configure_stream("EXTENDED_SYS_STATE", 0.1f);
|
||||
configure_stream("GPS_RAW_INT", 0.5f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 5.0f);
|
||||
configure_stream("HOME_POSITION", 0.1f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
|
||||
configure_stream("RC_CHANNELS", 0.5f);
|
||||
configure_stream("SYS_STATUS", 0.1f);
|
||||
configure_stream("VFR_HUD", 1.0f);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
if (configure_streams_to_default() != 0) {
|
||||
PX4_ERR("configure_streams_to_default() failed");
|
||||
}
|
||||
|
||||
/* set main loop delay depending on data rate to minimize CPU overhead */
|
||||
@ -2331,7 +2370,20 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
|
||||
/* check for requested subscriptions */
|
||||
if (_subscribe_to_stream != nullptr) {
|
||||
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
|
||||
if (_subscribe_to_stream_rate < -1.5f) {
|
||||
if (configure_streams_to_default(_subscribe_to_stream) == 0) {
|
||||
if (get_protocol() == SERIAL) {
|
||||
PX4_DEBUG("stream %s on device %s set to default rate", _subscribe_to_stream, _device_name);
|
||||
|
||||
} else if (get_protocol() == UDP) {
|
||||
PX4_DEBUG("stream %s on UDP port %d set to default rate", _subscribe_to_stream, _network_port);
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("setting stream %s to default failed", _subscribe_to_stream);
|
||||
}
|
||||
|
||||
} else if (configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate) == 0) {
|
||||
if (fabsf(_subscribe_to_stream_rate) > 0.00001f) {
|
||||
if (get_protocol() == SERIAL) {
|
||||
PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
|
||||
@ -2344,19 +2396,19 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
if (get_protocol() == SERIAL) {
|
||||
PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
|
||||
PX4_DEBUG("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
|
||||
|
||||
} else if (get_protocol() == UDP) {
|
||||
PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
|
||||
PX4_DEBUG("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
if (get_protocol() == SERIAL) {
|
||||
PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name);
|
||||
PX4_ERR("stream %s on device %s not found", _subscribe_to_stream, _device_name);
|
||||
|
||||
} else if (get_protocol() == UDP) {
|
||||
PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
|
||||
PX4_ERR("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -559,7 +559,7 @@ private:
|
||||
bool mavlink_link_termination_allowed;
|
||||
|
||||
char *_subscribe_to_stream;
|
||||
float _subscribe_to_stream_rate;
|
||||
float _subscribe_to_stream_rate; ///< rate of stream to subscribe to (0=disable, -1=unlimited, -2=default)
|
||||
bool _udp_initialised;
|
||||
|
||||
enum FLOW_CONTROL_MODE _flow_control_mode;
|
||||
@ -637,8 +637,22 @@ private:
|
||||
static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
|
||||
static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50;
|
||||
|
||||
/**
|
||||
* Configure a single stream.
|
||||
* @param stream_name
|
||||
* @param rate streaming rate in Hz, -1 = unlimited rate
|
||||
* @return 0 on success, <0 on error
|
||||
*/
|
||||
int configure_stream(const char *stream_name, const float rate = -1.0f);
|
||||
|
||||
/**
|
||||
* Configure default streams according to _mode for either all streams or only a single
|
||||
* stream.
|
||||
* @param configure_single_stream: if nullptr, configure all streams, else only a single stream
|
||||
* @return 0 on success, <0 on error
|
||||
*/
|
||||
int configure_streams_to_default(const char *configure_single_stream = nullptr);
|
||||
|
||||
/**
|
||||
* Adjust the stream rates based on the current rate
|
||||
*
|
||||
|
||||
@ -1844,7 +1844,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
int
|
||||
MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
|
||||
{
|
||||
if (msgId == 0) {
|
||||
if (msgId == MAVLINK_MSG_ID_HEARTBEAT) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
@ -1853,18 +1853,16 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
|
||||
}
|
||||
|
||||
// configure_stream wants a rate (msgs/second), so convert here.
|
||||
float rate = 0;
|
||||
float rate = 0.f;
|
||||
|
||||
if (interval < 0) {
|
||||
// stop the stream.
|
||||
rate = 0;
|
||||
if (interval < -0.00001f) {
|
||||
rate = 0.f; // stop the stream
|
||||
|
||||
} else if (interval > 0) {
|
||||
} else if (interval > 0.00001f) {
|
||||
rate = 1000000.0f / interval;
|
||||
|
||||
} else {
|
||||
// note: mavlink spec says rate == 0 is requesting a default rate but our streams
|
||||
// don't publish a default rate so for now let's pick a default rate of zero.
|
||||
rate = -2.f; // set default rate
|
||||
}
|
||||
|
||||
bool found_id = false;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user