diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index f720d4ae10..2f79d02b4c 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -512,6 +512,17 @@ void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode) _backends[instance]->set_mode(mode); } +// set_angle_targets - sets angle targets in degrees +void AP_Mount::set_angle_targets(uint8_t instance, float roll, float tilt, float pan) +{ + if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) { + return; + } + + // send command to backend + _backends[instance]->set_angle_targets(roll, tilt, pan); +} + /// Change the configuration of the mount /// triggered by a MavLink packet. void AP_Mount::configure_msg(uint8_t instance, mavlink_message_t* msg) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 338571ded6..bed953c396 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -96,6 +96,10 @@ public: void set_mode_to_default() { set_mode_to_default(_primary); } void set_mode_to_default(uint8_t instance); + // set_angle_targets - sets angle targets in degrees + void set_angle_targets(float roll, float tilt, float pan) { set_angle_targets(_primary, roll, tilt, pan); } + void set_angle_targets(uint8_t instance, float roll, float tilt, float pan); + // set_roi_target - sets target location that mount should attempt to point towards void set_roi_target(const struct Location &target_loc) { set_roi_target(_primary,target_loc); } void set_roi_target(uint8_t instance, const struct Location &target_loc); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index c5bc792056..87b4edb3c5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -4,6 +4,18 @@ extern const AP_HAL::HAL& hal; +// set_angle_targets - sets angle targets in degrees +void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan) +{ + // set angle targets + _angle_ef_target_rad.x = radians(roll); + _angle_ef_target_rad.y = radians(tilt); + _angle_ef_target_rad.z = radians(pan); + + // set the mode to mavlink targeting + _frontend.set_mode(_instance, MAV_MOUNT_MODE_MAVLINK_TARGETING); +} + // set_roi_target - sets target location that mount should attempt to point towards void AP_Mount_Backend::set_roi_target(const struct Location &target_loc) { @@ -44,9 +56,7 @@ void AP_Mount_Backend::control_msg(mavlink_message_t *msg) // set earth frame target angles from mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: - _angle_ef_target_rad.x = radians(packet.input_b*0.01f); - _angle_ef_target_rad.y = radians(packet.input_a*0.01f); - _angle_ef_target_rad.z = radians(packet.input_c*0.01f); + set_angle_targets(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f); break; // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index d494661910..f8f19007a8 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -50,6 +50,9 @@ public: // set_mode - sets mount's mode virtual void set_mode(enum MAV_MOUNT_MODE mode) = 0; + // set_angle_targets - sets angle targets in degrees + virtual void set_angle_targets(float roll, float tilt, float pan); + // set_roi_target - sets target location that mount should attempt to point towards virtual void set_roi_target(const struct Location &target_loc);