aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Mount_MAVLink: use sysid from vehicle, compid of 10

Browse Source
This commit is contained in:
Randy Mackay 2015-01-13 15:09:27 +09:00 committed by Andrew Tridgell
parent 6af5a6687f
commit 8fabacf5ff
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -38,7 +38,7 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
// prepare and send command_long message with DO_SET_MODE command // prepare and send command_long message with DO_SET_MODE command
mavlink_msg_command_long_send( mavlink_msg_command_long_send(
_chan, _sysid, _compid, // channel, system id, component id _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id
MAV_CMD_DO_SET_MODE, // command number MAV_CMD_DO_SET_MODE, // command number
0, // confirmation: 0=first confirmation of this command 0, // confirmation: 0=first confirmation of this command
mode, // param1: mode mode, // param1: mode
@ -59,7 +59,7 @@ void AP_Mount_MAVLink::set_roi_target(const struct Location &target_loc)
// prepare and send command_long message // prepare and send command_long message
mavlink_msg_command_long_send( mavlink_msg_command_long_send(
_chan, _sysid, _compid, // channel, system id, component id _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id
MAV_CMD_DO_MOUNT_CONTROL, // command number MAV_CMD_DO_MOUNT_CONTROL, // command number
0, // confirmation: 0=first confirmation of this command 0, // confirmation: 0=first confirmation of this command
target_loc.lat, // param1: pitch (in degrees) or lat (as int32_t) target_loc.lat, // param1: pitch (in degrees) or lat (as int32_t)
@ -79,7 +79,7 @@ void AP_Mount_MAVLink::configure_msg(mavlink_message_t* msg)
// forward on message with updated channel, sysid, compid // forward on message with updated channel, sysid, compid
mavlink_msg_mount_configure_send( mavlink_msg_mount_configure_send(
_chan, _sysid, _compid, _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID,
mavlink_msg_mount_configure_get_mount_mode(msg), mavlink_msg_mount_configure_get_mount_mode(msg),
mavlink_msg_mount_configure_get_stab_roll(msg), mavlink_msg_mount_configure_get_stab_roll(msg),
mavlink_msg_mount_configure_get_stab_pitch(msg), mavlink_msg_mount_configure_get_stab_pitch(msg),
@ -96,7 +96,7 @@ void AP_Mount_MAVLink::control_msg(mavlink_message_t* msg)
// forward on message with updated channel, sysid, compid // forward on message with updated channel, sysid, compid
mavlink_msg_mount_control_send( mavlink_msg_mount_control_send(
_chan, _sysid, _compid, _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID,
mavlink_msg_mount_control_get_input_a(msg), mavlink_msg_mount_control_get_input_a(msg),
mavlink_msg_mount_control_get_input_b(msg), mavlink_msg_mount_control_get_input_b(msg),
mavlink_msg_mount_control_get_input_c(msg), mavlink_msg_mount_control_get_input_c(msg),
@ -133,7 +133,7 @@ void AP_Mount_MAVLink::send_angle_target(const Vector3f& target_deg)
// prepare and send command_long message with DO_MOUNT_CONTROL command // prepare and send command_long message with DO_MOUNT_CONTROL command
mavlink_msg_command_long_send( mavlink_msg_command_long_send(
_chan, _sysid, _compid, // channel, system id, component id _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id
MAV_CMD_DO_MOUNT_CONTROL, // command number MAV_CMD_DO_MOUNT_CONTROL, // command number
0, // confirmation: 0=first confirmation of this command 0, // confirmation: 0=first confirmation of this command
target_deg.y, // param1: pitch (in degrees) or lat (as int32_t) target_deg.y, // param1: pitch (in degrees) or lat (as int32_t)

8
libraries/AP_Mount/AP_Mount_MAVLink.h
View File

@ -15,6 +15,8 @@
#include <RC_Channel.h> #include <RC_Channel.h>
#include <AP_Mount_Backend.h> #include <AP_Mount_Backend.h>
#define AP_MOUNT_MAVLINK_COMPID 10 // Use hard-coded component id to communicate with gimbal, sysid is taken from globally defined mavlink_system.sysid
class AP_Mount_MAVLink : public AP_Mount_Backend class AP_Mount_MAVLink : public AP_Mount_Backend
{ {
@ -23,9 +25,7 @@ public:
AP_Mount_MAVLink(AP_Mount &frontend, uint8_t instance) : AP_Mount_MAVLink(AP_Mount &frontend, uint8_t instance) :
AP_Mount_Backend(frontend, instance), AP_Mount_Backend(frontend, instance),
_enabled(false), _enabled(false),
_chan(MAVLINK_COMM_0), _chan(MAVLINK_COMM_0)
_sysid(0),
_compid(0)
{} {}
// init - performs any required initialisation for this instance // init - performs any required initialisation for this instance
@ -63,8 +63,6 @@ private:
// internal variables // internal variables
bool _enabled; // gimbal becomes enabled once the mount has been discovered bool _enabled; // gimbal becomes enabled once the mount has been discovered
mavlink_channel_t _chan; // telemetry channel used to communicate with mount mavlink_channel_t _chan; // telemetry channel used to communicate with mount
uint8_t _sysid; // system id of MAVLink enabled mount
uint8_t _compid; // component id of MAVLink enabled mount
}; };
#endif // __AP_MOUNT_MAVLINK_H__ #endif // __AP_MOUNT_MAVLINK_H__