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

Mount: add support for do_mount_control via command_long

Browse Source
This commit is contained in:
squilter 2015-08-12 22:31:31 -07:00 committed by Randy Mackay
parent 079161ef3a
commit cc58ec917c
4 changed files with 59 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Mount/AP_Mount.cpp
View File

@ -625,6 +625,16 @@ void AP_Mount::configure_msg(uint8_t instance, mavlink_message_t* msg)
_backends[instance]->configure_msg(msg);
}
void AP_Mount::configure(uint8_t instance, enum MAV_MOUNT_MODE mount_mode, uint8_t stab_roll, uint8_t stab_tilt, uint8_t stab_pan)
{
if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) {
return;
}
// send message to backend
_backends[instance]->configure(mount_mode, stab_roll, stab_tilt, stab_pan);
}
/// Control the mount (depends on the previously set mount configuration)
/// triggered by a MavLink packet.
void AP_Mount::control_msg(uint8_t instance, mavlink_message_t *msg)
@ -637,6 +647,16 @@ void AP_Mount::control_msg(uint8_t instance, mavlink_message_t *msg)
_backends[instance]->control_msg(msg);
}
void AP_Mount::control(uint8_t instance, int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, enum MAV_MOUNT_MODE mount_mode)
{
if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) {
return;
}
// send message to backend
_backends[instance]->control(pitch_or_lat, roll_or_lon, yaw_or_alt, mount_mode);
}
/// Return mount status information
void AP_Mount::status_msg(mavlink_channel_t chan)
{

8
libraries/AP_Mount/AP_Mount.h
View File

@ -114,6 +114,14 @@ public:
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);
// configure - configure the mount
void configure(enum MAV_MOUNT_MODE mount_mode, uint8_t stab_roll, uint8_t stab_tilt, uint8_t stab_pan) { configure(_primary, mount_mode, stab_roll, stab_tilt, stab_pan); }
void configure(uint8_t instance, enum MAV_MOUNT_MODE mount_mode, uint8_t stab_roll, uint8_t stab_tilt, uint8_t stab_pan);
// control - control the mount
void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, enum MAV_MOUNT_MODE mount_mode) { control(_primary, pitch_or_lat, roll_or_lon, yaw_or_alt, mount_mode); }
void control(uint8_t instance, int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, enum MAV_MOUNT_MODE mount_mode);
// configure_msg - process MOUNT_CONFIGURE messages received from GCS
void configure_msg(mavlink_message_t* msg) { configure_msg(_primary, msg); }
void configure_msg(uint8_t instance, mavlink_message_t* msg);

39
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -26,27 +26,40 @@ void AP_Mount_Backend::set_roi_target(const struct Location &target_loc)
_frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT);
}
// configure_msg - process MOUNT_CONFIGURE messages received from GCS
// configure_msg - process MOUNT_CONFIGURE messages received from GCS. deprecated.
void AP_Mount_Backend::configure_msg(mavlink_message_t* msg)
{
__mavlink_mount_configure_t packet;
mavlink_msg_mount_configure_decode(msg, &packet);
// set mode
_frontend.set_mode(_instance,(enum MAV_MOUNT_MODE)packet.mount_mode);
// set which axis are stabilized
_state._stab_roll = packet.stab_roll;
_state._stab_tilt = packet.stab_pitch;
_state._stab_pan = packet.stab_yaw;
configure((MAV_MOUNT_MODE)packet.mount_mode, packet.stab_roll, packet.stab_pitch, packet.stab_yaw);
}
// control_msg - process MOUNT_CONTROL messages received from GCS
void AP_Mount_Backend::configure(MAV_MOUNT_MODE mount_mode, uint8_t stab_roll, uint8_t stab_tilt, uint8_t stab_pan)
{
// set mode
_frontend.set_mode(_instance, mount_mode);
// set which axis are stabilized
_state._stab_roll = stab_roll;
_state._stab_tilt = stab_tilt;
_state._stab_pan = stab_pan;
}
// control_msg - process MOUNT_CONTROL messages received from GCS. deprecated.
void AP_Mount_Backend::control_msg(mavlink_message_t *msg)
{
__mavlink_mount_control_t packet;
mavlink_msg_mount_control_decode(msg, &packet);
control((int32_t) packet.input_a, (int32_t) packet.input_b, (int32_t) packet.input_c, 0);
}
void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, uint8_t mount_mode)
{
if(mount_mode > 0) {
_frontend.set_mode(_instance, (MAV_MOUNT_MODE) mount_mode);
}
// interpret message fields based on mode
switch (_frontend.get_mode(_instance)) {
case MAV_MOUNT_MODE_RETRACT:
@ -56,7 +69,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:
set_angle_targets(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
set_angle_targets(roll_or_lon*0.01f, pitch_or_lat*0.01f, yaw_or_alt*0.01f);
break;
// Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
@ -68,9 +81,9 @@ void AP_Mount_Backend::control_msg(mavlink_message_t *msg)
case MAV_MOUNT_MODE_GPS_POINT:
Location target_location;
memset(&target_location, 0, sizeof(target_location));
target_location.lat = packet.input_a;
target_location.lng = packet.input_b;
target_location.alt = packet.input_c;
target_location.lat = pitch_or_lat;
target_location.lng = roll_or_lon;
target_location.alt = yaw_or_alt;
target_location.flags.relative_alt = true;
set_roi_target(target_location);
break;

5
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -56,6 +56,11 @@ public:
// set_roi_target - sets target location that mount should attempt to point towards
virtual void set_roi_target(const struct Location &target_loc);
// configure - configure the mount
virtual void configure(MAV_MOUNT_MODE mount_mode, uint8_t stab_roll, uint8_t stab_tilt, uint8_t stab_pan);
// control - control the mount
virtual void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, uint8_t mount_mode);
// configure_msg - process MOUNT_CONFIGURE messages received from GCS
virtual void configure_msg(mavlink_message_t* msg);