diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-23 14:35:59 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-23 14:35:59 +0200 |
commit | 194aa491107ca10a65065edef3977c20c02a507c (patch) | |
tree | f47e88a1e7c58093f862d38dc4e75e27f8fd0460 /src/modules/mavlink | |
parent | f02de30c32acd313dcb4d9e5561634d5a3ab758c (diff) | |
parent | 662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf (diff) | |
download | px4-firmware-194aa491107ca10a65065edef3977c20c02a507c.tar.gz px4-firmware-194aa491107ca10a65065edef3977c20c02a507c.tar.bz2 px4-firmware-194aa491107ca10a65065edef3977c20c02a507c.zip |
Merged rate config changes
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 5 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 120 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 22 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 1 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_stream.h | 1 |
5 files changed, 148 insertions, 1 deletions
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d3065e251..f74b7daa7 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -201,10 +201,14 @@ 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 */ int get_mavlink_fd() { return _mavlink_fd; } + MavlinkStream * get_streams() { return _streams; } const + /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -356,7 +360,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); int message_buffer_init(int size); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b443b8af8..0aa4e005a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -232,6 +232,11 @@ public: return "HEARTBEAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HEARTBEAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); @@ -283,6 +288,11 @@ public: return "SYS_STATUS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SYS_STATUS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); @@ -334,6 +344,11 @@ public: return "HIGHRES_IMU"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); @@ -419,6 +434,11 @@ public: return "ATTITUDE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); @@ -465,6 +485,11 @@ public: return "ATTITUDE_QUATERNION"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); @@ -517,6 +542,11 @@ public: return "VFR_HUD"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + static MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); @@ -600,6 +630,11 @@ public: return "GPS_RAW_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); @@ -653,6 +688,11 @@ public: return "GLOBAL_POSITION_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionInt(); @@ -714,6 +754,11 @@ public: return "LOCAL_POSITION_NED"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionNED(); @@ -765,6 +810,11 @@ public: return "VICON_POSITION_ESTIMATE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamViconPositionEstimate(); @@ -815,6 +865,11 @@ public: return "GPS_GLOBAL_ORIGIN"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSGlobalOrigin(); @@ -855,6 +910,11 @@ public: return MavlinkStreamServoOutputRaw<N>::get_name_static(); } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + static const char *get_name_static() { switch (N) { @@ -932,6 +992,11 @@ public: return "HIL_CONTROLS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHILControls(); @@ -1069,6 +1134,11 @@ public: return "GLOBAL_POSITION_SETPOINT_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionSetpointInt(); @@ -1112,6 +1182,11 @@ public: return "LOCAL_POSITION_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionSetpoint(); @@ -1160,6 +1235,11 @@ public: return "ROLL_PITCH_YAW_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawThrustSetpoint(); @@ -1208,6 +1288,11 @@ public: return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); @@ -1256,6 +1341,11 @@ public: return "RC_CHANNELS_RAW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRCChannelsRaw(); @@ -1340,6 +1430,11 @@ public: return "MANUAL_CONTROL"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + static MavlinkStream *new_instance() { return new MavlinkStreamManualControl(); @@ -1389,6 +1484,11 @@ public: return "OPTICAL_FLOW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamOpticalFlow(); @@ -1437,6 +1537,11 @@ public: return "ATTITUDE_CONTROLS"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeControls(); @@ -1495,6 +1600,11 @@ public: return "NAMED_VALUE_FLOAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamNamedValueFloat(); @@ -1543,6 +1653,11 @@ public: return "CAMERA_CAPTURE"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamCameraCapture(); @@ -1588,6 +1703,11 @@ public: return "DISTANCE_SENSOR"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_DISTANCE_SENSOR; + } + static MavlinkStream *new_instance() { return new MavlinkStreamDistanceSensor(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0862f6bf0..425fc79a5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -156,6 +156,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_heartbeat(msg); break; + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_message_request_data_stream(msg); + break; + default: break; } @@ -492,6 +496,24 @@ MavlinkReceiver::handle_message_heartbeat(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; + + MavlinkStream *stream; + LL_FOREACH(_mavlink->get_streams(), stream) { + if (req.req_stream_id == stream->get_id()) { + _mavlink->configure_stream_threadsafe(stream->get_name(), rate); + } + } + } +} + +void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { mavlink_hil_sensor_t imu; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index cd1dab365..af81e8a31 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -113,6 +113,7 @@ private: void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(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 a41ace48e..69809a386 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -67,6 +67,7 @@ public: static const char *get_name_static(); virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() const = 0; + virtual uint8_t get_id() = 0; protected: mavlink_channel_t _channel; |