From 662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 22:36:28 +0400 Subject: [PATCH] mavlink: REQUEST_DATA_STREAM hadling implemented --- src/modules/mavlink/mavlink_main.h | 3 +- src/modules/mavlink/mavlink_messages.cpp | 112 ++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 21 +++++ src/modules/mavlink/mavlink_receiver.h | 1 + src/modules/mavlink/mavlink_stream.h | 1 + 5 files changed, 135 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad9..427b9ad358 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -194,6 +194,8 @@ public: mavlink_channel_t get_channel(); + void configure_stream_threadsafe(const char *stream_name, const float rate); + bool _task_should_exit; /**< if true, mavlink task should exit */ protected: @@ -313,7 +315,6 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); - void configure_stream_threadsafe(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c89031fccf..5b285dc9bd 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -193,6 +193,11 @@ public: return "HEARTBEAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HEARTBEAT; + } + MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); @@ -244,6 +249,11 @@ public: return "SYS_STATUS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SYS_STATUS; + } + MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); @@ -293,6 +303,11 @@ public: return "HIGHRES_IMU"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); @@ -364,6 +379,11 @@ public: return "ATTITUDE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); @@ -400,6 +420,11 @@ public: return "ATTITUDE_QUATERNION"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); @@ -441,6 +466,11 @@ public: return "VFR_HUD"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); @@ -514,6 +544,11 @@ public: return "GPS_RAW_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); @@ -557,6 +592,11 @@ public: return "GLOBAL_POSITION_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionInt(); @@ -608,6 +648,11 @@ public: return "LOCAL_POSITION_NED"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionNED(); @@ -648,6 +693,11 @@ public: return "GPS_GLOBAL_ORIGIN"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + MavlinkStream *new_instance() { return new MavlinkStreamGPSGlobalOrigin(); @@ -689,6 +739,11 @@ public: sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + const char *get_name() { return _name; @@ -747,6 +802,11 @@ public: return "HIL_CONTROLS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + MavlinkStream *new_instance() { return new MavlinkStreamHILControls(); @@ -874,6 +934,11 @@ public: return "GLOBAL_POSITION_SETPOINT_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionSetpointInt(); @@ -912,6 +977,11 @@ public: return "LOCAL_POSITION_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionSetpoint(); @@ -950,6 +1020,11 @@ public: return "ROLL_PITCH_YAW_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawThrustSetpoint(); @@ -988,6 +1063,11 @@ public: return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); @@ -1026,6 +1106,11 @@ public: return "RC_CHANNELS_RAW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + MavlinkStream *new_instance() { return new MavlinkStreamRCChannelsRaw(); @@ -1075,6 +1160,11 @@ public: return "MANUAL_CONTROL"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + MavlinkStream *new_instance() { return new MavlinkStreamManualControl(); @@ -1114,6 +1204,11 @@ public: return "OPTICAL_FLOW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + MavlinkStream *new_instance() { return new MavlinkStreamOpticalFlow(); @@ -1152,6 +1247,11 @@ public: return "ATTITUDE_CONTROLS"; } + uint8_t get_id() + { + return 0; + } + MavlinkStream *new_instance() { return new MavlinkStreamAttitudeControls(); @@ -1200,6 +1300,11 @@ public: return "NAMED_VALUE_FLOAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + MavlinkStream *new_instance() { return new MavlinkStreamNamedValueFloat(); @@ -1238,6 +1343,11 @@ public: return "CAMERA_CAPTURE"; } + uint8_t get_id() + { + return 0; + } + MavlinkStream *new_instance() { return new MavlinkStreamCameraCapture(); @@ -1252,8 +1362,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ec40ee0a9..cf8b23bc3f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -152,6 +152,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_message_request_data_stream(msg); + break; + default: break; } @@ -457,6 +461,23 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } } +void +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) { + float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; + + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (req.req_stream_id == 0 || req.req_stream_id == streams_list[i]->get_id()) { + _mavlink->configure_stream_threadsafe(streams_list[i]->get_name(), rate); + } + } + } +} + void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8ccb2a0354..eab8f071d4 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce08..aa8ca450b4 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -70,6 +70,7 @@ public: virtual MavlinkStream *new_instance() = 0; virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() = 0; + virtual uint8_t get_id() = 0; };