Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Mount: add set_angle_target method

Browse Source
This commit is contained in:
Randy Mackay 2015-03-21 20:58:57 +09:00
parent f6021010c5
commit 8a3a7bdcd1
4 changed files with 31 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Mount/AP_Mount.cpp
View File

@ -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)

4
libraries/AP_Mount/AP_Mount.h
View File

@ -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);

16
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -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

3
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -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);