mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-14 01:20:35 +08:00
Implement code review feedback. Add get_id_static to MavlinkStream items. Add implementation of MAV_CMD_SET_MESSAGE_INTERVAL, MAV_CMD_GET_MESSAGE_INTERVAL. Add deprecation message to REQUEST_DATA_STREAM.
This commit is contained in:
Chris Lovett
committed by
Lorenz Meier
Lorenz Meier
parent
4ef4be2d70
commit
534e10c96a
@@ -1346,7 +1346,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||
}
|
||||
}
|
||||
|
||||
if (interval == 0) {
|
||||
if (interval <= 0) {
|
||||
/* stream was not active and is requested to be disabled, do nothing */
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -275,11 +275,16 @@ public:
|
||||
return "HEARTBEAT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HEARTBEAT;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamHeartbeat(mavlink);
|
||||
@@ -339,11 +344,16 @@ public:
|
||||
return "STATUSTEXT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_STATUSTEXT;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamStatustext(mavlink);
|
||||
@@ -471,11 +481,16 @@ public:
|
||||
return "COMMAND_LONG";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_COMMAND_LONG;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamCommandLong(mavlink);
|
||||
@@ -539,11 +554,16 @@ public:
|
||||
return "SYS_STATUS";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_SYS_STATUS;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamSysStatus(mavlink);
|
||||
@@ -645,11 +665,16 @@ public:
|
||||
return "HIGHRES_IMU";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIGHRES_IMU;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamHighresIMU(mavlink);
|
||||
@@ -751,11 +776,16 @@ public:
|
||||
return "ATTITUDE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ATTITUDE;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamAttitude(mavlink);
|
||||
@@ -815,11 +845,16 @@ public:
|
||||
return "ATTITUDE_QUATERNION";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamAttitudeQuaternion(mavlink);
|
||||
@@ -880,11 +915,16 @@ public:
|
||||
return "VFR_HUD";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_VFR_HUD;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamVFRHUD(mavlink);
|
||||
@@ -985,11 +1025,16 @@ public:
|
||||
return "GPS_RAW_INT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_GPS_RAW_INT;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamGPSRawInt(mavlink);
|
||||
@@ -1048,10 +1093,15 @@ public:
|
||||
return "SYSTEM_TIME";
|
||||
}
|
||||
|
||||
uint8_t get_id() {
|
||||
static uint8_t get_id_static() {
|
||||
return MAVLINK_MSG_ID_SYSTEM_TIME;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink) {
|
||||
return new MavlinkStreamSystemTime(mavlink);
|
||||
}
|
||||
@@ -1093,10 +1143,15 @@ public:
|
||||
return "TIMESYNC";
|
||||
}
|
||||
|
||||
uint8_t get_id() {
|
||||
static uint8_t get_id_static() {
|
||||
return MAVLINK_MSG_ID_TIMESYNC;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink) {
|
||||
return new MavlinkStreamTimesync(mavlink);
|
||||
}
|
||||
@@ -1137,11 +1192,16 @@ public:
|
||||
return "ADSB_VEHICLE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ADSB_VEHICLE;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamADSBVehicle(mavlink);
|
||||
@@ -1205,11 +1265,16 @@ public:
|
||||
return "CAMERA_TRIGGER";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_CAMERA_TRIGGER;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamCameraTrigger(mavlink);
|
||||
@@ -1265,11 +1330,16 @@ public:
|
||||
return "GLOBAL_POSITION_INT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamGlobalPositionInt(mavlink);
|
||||
@@ -1338,11 +1408,16 @@ public:
|
||||
return "VISION_POSITION_NED";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamVisionPositionNED(mavlink);
|
||||
@@ -1402,11 +1477,16 @@ public:
|
||||
return "LOCAL_POSITION_NED";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamLocalPositionNED(mavlink);
|
||||
@@ -1465,11 +1545,16 @@ public:
|
||||
return "LOCAL_POSITION_NED_COV";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamLocalPositionNEDCOV(mavlink);
|
||||
@@ -1537,11 +1622,16 @@ public:
|
||||
return "ESTIMATOR_STATUS";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_VIBRATION;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamEstimatorStatus(mavlink);
|
||||
@@ -1600,11 +1690,16 @@ public:
|
||||
return "ATT_POS_MOCAP";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ATT_POS_MOCAP;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamAttPosMocap(mavlink);
|
||||
@@ -1664,11 +1759,16 @@ public:
|
||||
return "HOME_POSITION";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HOME_POSITION;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamHomePosition(mavlink);
|
||||
@@ -1734,11 +1834,16 @@ public:
|
||||
return MavlinkStreamServoOutputRaw<N>::get_name_static();
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
switch (N) {
|
||||
@@ -1831,11 +1936,16 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamActuatorControlTarget<N>(mavlink);
|
||||
@@ -1912,11 +2022,16 @@ public:
|
||||
return "HIL_CONTROLS";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIL_CONTROLS;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamHILControls(mavlink);
|
||||
@@ -2071,11 +2186,16 @@ public:
|
||||
return "POSITION_TARGET_GLOBAL_INT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamPositionTargetGlobalInt(mavlink);
|
||||
@@ -2130,11 +2250,16 @@ public:
|
||||
return "POSITION_TARGET_LOCAL_NED";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamLocalPositionSetpoint(mavlink);
|
||||
@@ -2198,11 +2323,16 @@ public:
|
||||
return "ATTITUDE_TARGET";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ATTITUDE_TARGET;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamAttitudeTarget(mavlink);
|
||||
@@ -2270,11 +2400,16 @@ public:
|
||||
return "RC_CHANNELS";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_RC_CHANNELS;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamRCChannels(mavlink);
|
||||
@@ -2366,11 +2501,16 @@ public:
|
||||
return "MANUAL_CONTROL";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_MANUAL_CONTROL;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamManualControl(mavlink);
|
||||
@@ -2434,11 +2574,16 @@ public:
|
||||
return "OPTICAL_FLOW_RAD";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamOpticalFlowRad(mavlink);
|
||||
@@ -2502,11 +2647,16 @@ public:
|
||||
return "NAMED_VALUE_FLOAT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamNamedValueFloat(mavlink);
|
||||
@@ -2562,11 +2712,16 @@ public:
|
||||
return "NAV_CONTROLLER_OUTPUT";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamNavControllerOutput(mavlink);
|
||||
@@ -2629,11 +2784,16 @@ public:
|
||||
return "CAMERA_CAPTURE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamCameraCapture(mavlink);
|
||||
@@ -2693,11 +2853,16 @@ public:
|
||||
return "DISTANCE_SENSOR";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_DISTANCE_SENSOR;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamDistanceSensor(mavlink);
|
||||
@@ -2777,11 +2942,16 @@ public:
|
||||
return "EXTENDED_SYS_STATE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_EXTENDED_SYS_STATE;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamExtendedSysState(mavlink);
|
||||
@@ -2871,11 +3041,16 @@ public:
|
||||
return "ALTITUDE";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
static uint8_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_ALTITUDE;
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamAltitude(mavlink);
|
||||
@@ -2953,46 +3128,46 @@ protected:
|
||||
};
|
||||
|
||||
const StreamListItem *streams_list[] = {
|
||||
new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static, &MavlinkStreamAttitude::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static, &MavlinkStreamAttitudeQuaternion::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static, &MavlinkStreamVFRHUD::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static, &MavlinkStreamGPSRawInt::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static, &MavlinkStreamSystemTime::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static, &MavlinkStreamVisionPositionNED::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static, &MavlinkStreamHomePosition::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static, &MavlinkStreamServoOutputRaw<0>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static, &MavlinkStreamServoOutputRaw<1>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static, &MavlinkStreamServoOutputRaw<2>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static, &MavlinkStreamServoOutputRaw<3>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static, &MavlinkStreamHILControls::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static, &MavlinkStreamPositionTargetGlobalInt::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static, &MavlinkStreamLocalPositionSetpoint::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static, &MavlinkStreamAttitudeTarget::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static, &MavlinkStreamRCChannels::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static, &MavlinkStreamManualControl::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static, &MavlinkStreamOpticalFlowRad::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static, &MavlinkStreamActuatorControlTarget<0>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static, &MavlinkStreamActuatorControlTarget<1>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static, &MavlinkStreamActuatorControlTarget<2>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static, &MavlinkStreamActuatorControlTarget<3>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static, &MavlinkStreamDistanceSensor::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
|
||||
nullptr
|
||||
};
|
||||
|
||||
@@ -48,10 +48,12 @@ class StreamListItem {
|
||||
public:
|
||||
MavlinkStream* (*new_instance)(Mavlink *mavlink);
|
||||
const char* (*get_name)();
|
||||
uint8_t (*get_id)();
|
||||
|
||||
StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
|
||||
StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)(), uint8_t (*id)()) :
|
||||
new_instance(inst),
|
||||
get_name(name) {};
|
||||
get_name(name),
|
||||
get_id(id) {};
|
||||
|
||||
~StreamListItem() {};
|
||||
};
|
||||
|
||||
@@ -355,6 +355,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
||||
} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
|
||||
_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
|
||||
|
||||
} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
|
||||
set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2);
|
||||
|
||||
} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
|
||||
get_message_interval((int)cmd_mavlink.param1);
|
||||
|
||||
} else {
|
||||
|
||||
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
|
||||
@@ -1368,37 +1374,75 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
|
||||
mavlink_request_data_stream_t req;
|
||||
mavlink_msg_request_data_stream_decode(msg, &req);
|
||||
|
||||
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid
|
||||
&& req.req_message_rate != 0) {
|
||||
float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
|
||||
PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead");
|
||||
|
||||
MavlinkStream *stream;
|
||||
LL_FOREACH(_mavlink->get_streams(), stream) {
|
||||
if (req.req_stream_id == stream->get_id()) {
|
||||
_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
|
||||
// if the command here was to unsubscribe then the stream will be deleted
|
||||
// in which case we cannot continue this LL_FOREACH loop. We have to return.
|
||||
// Besides there shouldn't be more than one stream with the same id, right?
|
||||
return;
|
||||
}
|
||||
}
|
||||
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid &&
|
||||
req.req_stream_id != 0)
|
||||
{
|
||||
// compute interval in microseconds.
|
||||
float interval = 0;
|
||||
int rate = req.req_message_rate;
|
||||
if (req.start_stop == 0 || rate == 0) {
|
||||
rate = -1;
|
||||
}
|
||||
else if (rate > 0) {
|
||||
interval = 1000000.0f / rate;
|
||||
}
|
||||
|
||||
// If we get here then the stream may have been deleted, in which case we need to recreate it.
|
||||
// todo: we could simplify this code by adding a get_id_static (get_name_static) so that
|
||||
// we could do both subscribe and unsubscribe using the streams_list.
|
||||
// todo: note it is kind of bogus to interpret the req_stream_id as a message id
|
||||
// but this is how it was. MAVLINK defines separate MAV_DATA_STREAM ids which bundle
|
||||
// messages into groups, but that is all deprecated now anyway, and this code was
|
||||
// previously interpreting the req_stream_id as a message id, so we'll leave it that way.
|
||||
set_message_interval(req.req_stream_id, interval);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::set_message_interval(int msgId, float interval)
|
||||
{
|
||||
// configure_stream wants a rate (msgs/second), so convert here.
|
||||
float rate = 0;
|
||||
if (interval < 0) {
|
||||
// stop the stream.
|
||||
rate = 0;
|
||||
}
|
||||
else if (interval > 0) {
|
||||
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.
|
||||
}
|
||||
|
||||
// The interval between two messages is in microseconds.
|
||||
// Set to -1 to disable and 0 to request default rate
|
||||
if (msgId != 0)
|
||||
{
|
||||
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
|
||||
MavlinkStream *new_instance = streams_list[i]->new_instance(_mavlink);
|
||||
bool found = false;
|
||||
if (req.req_stream_id == new_instance->get_id()) {
|
||||
_mavlink->configure_stream_threadsafe(new_instance->get_name(), rate);
|
||||
found = true;
|
||||
}
|
||||
delete new_instance;
|
||||
if (found) {
|
||||
const StreamListItem* item = streams_list[i];
|
||||
if (msgId == item->get_id()) {
|
||||
_mavlink->configure_stream_threadsafe(item->get_name(), rate);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::get_message_interval(int msgId)
|
||||
{
|
||||
unsigned interval = 0;
|
||||
|
||||
MavlinkStream *stream;
|
||||
LL_FOREACH(_mavlink->get_streams(), stream) {
|
||||
if (stream->get_id() == msgId) {
|
||||
interval = stream->get_interval();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// send back this value...
|
||||
mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@@ -145,6 +145,13 @@ private:
|
||||
|
||||
void *receive_thread(void *arg);
|
||||
|
||||
/**
|
||||
* Set the interval at which the given message stream is published.
|
||||
* The rate is the number of messages per second.
|
||||
*/
|
||||
void set_message_interval(int msgId, float rate);
|
||||
void get_message_interval(int msgId);
|
||||
|
||||
/**
|
||||
* Convert remote timestamp to local hrt time (usec)
|
||||
* Use timesync if available, monotonic boot time otherwise
|
||||
|
||||
Reference in New Issue
Block a user