mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Mount_MAVLink: remove unused _enable and find_mount
This commit is contained in:
parent
b5127b680f
commit
bf82e82282
@ -60,11 +60,6 @@ bool AP_Mount_MAVLink::has_pan_control() const
|
||||
// set_mode - sets mount's mode
|
||||
void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
|
||||
{
|
||||
// exit immediately if mount has not been found
|
||||
if (!_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
// map requested mode to mode that mount can actually support
|
||||
enum MAV_MOUNT_MODE mode_to_send = mode;
|
||||
switch (mode) {
|
||||
@ -97,31 +92,12 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
|
||||
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
|
||||
void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
|
||||
{
|
||||
// exit immediately if mount has not been found
|
||||
if (!_enabled) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// find_mount - search for MAVLink enabled mount
|
||||
void AP_Mount_MAVLink::find_mount()
|
||||
{
|
||||
// return immediately if target has already been found
|
||||
if (_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
// To-Do: search for MAVLink enabled mount using MAVLink_routing table
|
||||
// do nothing - we rely on the mount sending the messages directly
|
||||
}
|
||||
|
||||
// send_angle_target - send earth-frame angle targets to mount
|
||||
void AP_Mount_MAVLink::send_angle_target(const Vector3f& target, bool target_in_degrees)
|
||||
{
|
||||
// exit immediately if mount has not been found
|
||||
if (!_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
// convert to degrees if necessary
|
||||
Vector3f target_deg = target;
|
||||
if (!target_in_degrees) {
|
||||
|
@ -24,7 +24,6 @@ public:
|
||||
// Constructor
|
||||
AP_Mount_MAVLink(AP_Mount &frontend, uint8_t instance) :
|
||||
AP_Mount_Backend(frontend, instance),
|
||||
_enabled(false),
|
||||
_chan(MAVLINK_COMM_0),
|
||||
_last_mode(MAV_MOUNT_MODE_RETRACT)
|
||||
{}
|
||||
@ -45,16 +44,11 @@ public:
|
||||
virtual void status_msg(mavlink_channel_t chan);
|
||||
|
||||
private:
|
||||
|
||||
// find_mount - search for MAVLink enabled mount
|
||||
void find_mount();
|
||||
|
||||
// send_angle_target - send earth-frame angle targets to mount
|
||||
// set target_in_degrees to true to send degree targets, false if target in radians
|
||||
void send_angle_target(const Vector3f& target, bool target_in_degrees);
|
||||
|
||||
// internal variables
|
||||
bool _enabled; // gimbal becomes enabled once the mount has been discovered
|
||||
mavlink_channel_t _chan; // telemetry channel used to communicate with mount
|
||||
enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount
|
||||
Vector3f _last_angle_target; // last angle target sent to mount
|
||||
|
Loading…
Reference in New Issue
Block a user