zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.

sbg
Chris Lovett 9 years ago committed by Lorenz Meier
parent
4ef4be2d70
commit
534e10c96a
5 changed files with 333 additions and 105 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/modules/mavlink/mavlink_main.cpp
  2. 327
      src/modules/mavlink/mavlink_messages.cpp
  3. 6
      src/modules/mavlink/mavlink_messages.h
  4. 96
      src/modules/mavlink/mavlink_receiver.cpp
  5. 7
      src/modules/mavlink/mavlink_receiver.h

2
src/modules/mavlink/mavlink_main.cpp
Unescape View File

@ -1346,7 +1346,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) @@ -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;
}

327
src/modules/mavlink/mavlink_messages.cpp
Unescape View File

@ -275,11 +275,16 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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
};

6
src/modules/mavlink/mavlink_messages.h
Unescape View File

@ -48,10 +48,12 @@ class StreamListItem { @@ -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() {};
};

96
src/modules/mavlink/mavlink_receiver.cpp
Unescape View File

@ -355,6 +355,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) @@ -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) @@ -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;
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;
}
}
PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead");
// 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.
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;
}
// 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

7
src/modules/mavlink/mavlink_receiver.h
Unescape View File

@ -145,6 +145,13 @@ private: @@ -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

Write Preview
Loading…
Cancel
Save