diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-08 21:38:32 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-08 21:38:32 +0200 |
commit | 00ef10f307d3c4a262a15ab5747d854eb4c568d5 (patch) | |
tree | 13d23b5b5afae17624d95b28be0f74e4e6c2c2f2 | |
parent | d55e64d1e54542762510387a22897f504c68a5a6 (diff) | |
parent | f0d043c06849a2f2f5e07d6f3bdfafc0e4e6f041 (diff) | |
download | px4-firmware-00ef10f307d3c4a262a15ab5747d854eb4c568d5.tar.gz px4-firmware-00ef10f307d3c4a262a15ab5747d854eb4c568d5.tar.bz2 px4-firmware-00ef10f307d3c4a262a15ab5747d854eb4c568d5.zip |
Merge pull request #816 from PX4/mavlink_commandlongstream
mavlink: COMMAND_LONG stream: listen to vehicle_command uorb topic and send mavlink_msg_command_long
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 46 |
1 files changed, 46 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b390..bb8bba037 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1271,6 +1271,51 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() + { + return "COMMAND_LONG"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCommandLong(); + } + +private: + MavlinkOrbSubscription *vehicle_command_sub; + struct vehicle_command_s *vehicle_command; + +protected: + void subscribe(Mavlink *mavlink) + { + vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (vehicle_command_sub->update(t)) { + if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { + mavlink_msg_command_long_send(_channel, + vehicle_command->target_system, + vehicle_command->target_component, + vehicle_command->command, + vehicle_command->confirmation, + vehicle_command->param1, + vehicle_command->param2, + vehicle_command->param3, + vehicle_command->param4, + vehicle_command->param5, + vehicle_command->param6, + vehicle_command->param7); + } + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1297,5 +1342,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamCommandLong(), nullptr }; |