dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merged rate config changes

This commit is contained in:
Lorenz Meier 2014-06-23 14:35:59 +02:00
parent f02de30c32 662a7403b2
commit 194aa49110
5 changed files with 148 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/mavlink/mavlink_main.h
View File

@ -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);

120
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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();

22
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;
}
@ -491,6 +495,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)
{

1
src/modules/mavlink/mavlink_receiver.h
View File

@ -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);

1
src/modules/mavlink/mavlink_stream.h
View File

@ -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;