From ccc6f0b0200ee40a31a08006c0955c823ef3fa51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Feb 2015 09:26:55 +0100 Subject: Improve multi-stream handling by template and index usage. Can be consolidated slightly once multiplatform code knows about multi-topics --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 160 +++++++++++++++++++------------ 3 files changed, 99 insertions(+), 64 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index a6e45c959..dcc1d9dc5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -11,7 +11,7 @@ mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 -mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET -r 30 +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 SERVO_OUTPUT_RAW_0 -r 20 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index efa622765..024dfd6fb 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1403,6 +1403,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 179195d3a..9711d8fc3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1368,6 +1368,99 @@ protected: } }; +template +class MavlinkStreamActuatorControlTarget : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamActuatorControlTarget::get_name_static(); + } + + static const char *get_name_static() + { + switch (N) { + case 0: + return "ACTUATOR_CONTROL_TARGET0"; + + case 1: + return "ACTUATOR_CONTROL_TARGET1"; + + case 2: + return "ACTUATOR_CONTROL_TARGET2"; + + case 3: + return "ACTUATOR_CONTROL_TARGET3"; + } + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamActuatorControlTarget(mavlink); + } + + unsigned get_size() + { + return _att_ctrl_sub->is_published() ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_att_ctrl_sub; + uint64_t _att_ctrl_time; + + /* do not allow top copying this class */ + MavlinkStreamActuatorControlTarget(MavlinkStreamActuatorControlTarget &); + MavlinkStreamActuatorControlTarget& operator = (const MavlinkStreamActuatorControlTarget &); + +protected: + explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(nullptr), + _att_ctrl_time(0) + { + // XXX this can be removed once the multiplatform system remaps topics + switch (N) { + case 0: + _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + break; + + case 1: + _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_1)); + break; + + case 2: + _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_2)); + break; + + case 3: + _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_3)); + break; + } + } + + void send(const hrt_abstime t) + { + struct actuator_controls_s att_ctrl; + + if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { + mavlink_actuator_control_target_t msg; + + msg.time_usec = att_ctrl.timestamp; + msg.group_mlx = N; + + for (unsigned i = 0; i < sizeof(msg.controls) / sizeof(msg.controls[0]); i++) { + msg.controls[i] = att_ctrl.control[i]; + } + + _mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg); + } + } +}; + class MavlinkStreamHILControls : public MavlinkStream { @@ -2006,68 +2099,6 @@ protected: } }; -class MavlinkStreamAttitudeControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitudeControls::get_name_static(); - } - - static const char *get_name_static() - { - return "ACTUATOR_CONTROL_TARGET"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamAttitudeControls(mavlink); - } - - unsigned get_size() - { - return _att_ctrl_sub[0]->is_published() ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; - } - -private: - MavlinkOrbSubscription *_att_ctrl_sub[1]; - uint64_t _att_ctrl_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); - -protected: - explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), - _att_ctrl_sub{_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)}, - _att_ctrl_time(0) - {} - - void send(const hrt_abstime t) - { - struct actuator_controls_s att_ctrl; - - if (_att_ctrl_sub[0]->update(&_att_ctrl_time, &att_ctrl)) { - mavlink_actuator_control_target_t msg; - - msg.time_usec = att_ctrl.timestamp; - msg.group_mlx = 0; - - for (unsigned i = 0; i < sizeof(msg.controls) / sizeof(msg.controls[0]); i++) { - msg.controls[i] = att_ctrl.control[i]; - } - - _mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg); - } - } -}; - - class MavlinkStreamNamedValueFloat : public MavlinkStream { public: @@ -2296,7 +2327,10 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::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(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), -- cgit v1.2.3