fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 11:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: REQUEST_DATA_STREAM hadling implemented

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-07 22:36:28 +04:00
parent e4628fbed6
commit 662a7403b2
5 changed files with 135 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.h
+2 -1
View File
@@ -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);
src/modules/mavlink/mavlink_messages.cpp
+110 -2
View File
@@ -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)
src/modules/mavlink/mavlink_receiver.cpp
+21
View File
@@ -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)
{
src/modules/mavlink/mavlink_receiver.h
+1
View File
@@ -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);
src/modules/mavlink/mavlink_stream.h
+1
View File
@@ -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;
};