From 75a56fb7364e50fdf95caae49b37238e1c0cb4ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jul 2015 09:11:28 +0200 Subject: [PATCH 1/3] Rename RC channels raw to proper RC channels --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 6d8294b967..9dcb854488 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -13,7 +13,7 @@ mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 -mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 +mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS -r 5 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 From 01de0f9af7374c1d41882c792f8b97d31a58b7d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jul 2015 09:11:48 +0200 Subject: [PATCH 2/3] MAVLink: Request right channel --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index faba108e0a..d94978147b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1597,7 +1597,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("RC_CHANNELS", 1.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); @@ -1617,7 +1617,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); - configure_stream("RC_CHANNELS_RAW", 20.0f); + configure_stream("RC_CHANNELS", 20.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); @@ -1634,7 +1634,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("BATTERY_STATUS", 1.0f); configure_stream("SYSTEM_TIME", 1.0f); - configure_stream("RC_CHANNELS_RAW", 5.0f); + configure_stream("RC_CHANNELS", 5.0f); break; default: From c020c6ed9d34cde98611f51e6d57a8f0e95ec948 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jul 2015 09:12:11 +0200 Subject: [PATCH 3/3] MAVLink: Fix name of RC channel to correct message name --- src/modules/mavlink/mavlink_messages.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6876f8dde8..faa6f25d23 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1914,33 +1914,32 @@ protected: }; -class MavlinkStreamRCChannelsRaw : public MavlinkStream +class MavlinkStreamRCChannels : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRCChannelsRaw::get_name_static(); + return MavlinkStreamRCChannels::get_name_static(); } static const char *get_name_static() { - return "RC_CHANNELS_RAW"; + return "RC_CHANNELS"; } uint8_t get_id() { - return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + return MAVLINK_MSG_ID_RC_CHANNELS; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRCChannelsRaw(mavlink); + return new MavlinkStreamRCChannels(mavlink); } unsigned get_size() { - return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _rc_sub->is_published() ? (MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1948,11 +1947,11 @@ private: uint64_t _rc_time; /* do not allow top copying this class */ - MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannels(MavlinkStreamRCChannels &); + MavlinkStreamRCChannels& operator = (const MavlinkStreamRCChannels &); protected: - explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink), _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), _rc_time(0) {} @@ -2358,7 +2357,7 @@ const StreamListItem *streams_list[] = { 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(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::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),