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

Copter: handle SET_POSITION_TARGET mavlink messages

Browse Source
This commit is contained in:
Jonathan Challinger 2014-11-14 17:18:17 -08:00 committed by Randy Mackay
parent 055d3bee1f
commit 387f3276cb
2 changed files with 71 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
ArduCopter/GCS_Mavlink.pde
View File

@ -879,7 +879,6 @@ void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
wp_nav.set_desired_alt(cmd.content.location.alt);
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
uint8_t result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required
@ -1315,8 +1314,26 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
// create 3-axis velocity vector and sent to guided controller
guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
/*
* for future use:
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
* bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
* bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
*/
if (!pos_ignore && !vel_ignore && acc_ignore) {
guided_set_destination_spline(Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f), Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (pos_ignore && !vel_ignore && acc_ignore) {
guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (!pos_ignore && vel_ignore && acc_ignore) {
guided_set_destination(Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f));
} else {
result = MAV_RESULT_FAILED;
}
break;
}
@ -1337,8 +1354,50 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
// create 3-axis velocity vector and sent to guided controller
guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
/*
* for future use:
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
* bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
* bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
*/
Vector3f pos_ned;
if(!pos_ignore) {
Location loc;
loc.lat = packet.lat_int;
loc.lng = packet.lon_int;
loc.alt = packet.alt;
switch (packet.coordinate_frame) {
case MAV_FRAME_GLOBAL_INT:
loc.flags.relative_alt = false;
loc.flags.terrain_alt = false;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
loc.flags.relative_alt = true;
loc.flags.terrain_alt = false;
break;
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
loc.flags.relative_alt = true;
loc.flags.terrain_alt = true;
break;
}
pos_ned = pv_location_to_vector(loc);
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
guided_set_destination_spline(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (pos_ignore && !vel_ignore && acc_ignore) {
guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (!pos_ignore && vel_ignore && acc_ignore) {
guided_set_destination(pos_ned);
} else {
result = MAV_RESULT_FAILED;
}
break;
}

7
ArduCopter/defines.h
View File

@ -387,6 +387,13 @@ enum FlipState {
#define FS_GPS_ALTHOLD 2 // switch to ALTHOLD mode on GPS failsafe
#define FS_GPS_LAND_EVEN_STABILIZE 3 // switch to LAND mode on GPS failsafe even if in a manual flight mode like Stabilize
// for mavlink SET_POSITION_TARGET messages
#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1) | (1<<2))
#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ((1<<3) | (1<<4) | (1<<5))
#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ((1<<6) | (1<<7) | (1<<8))
#define MAVLINK_SET_POS_TYPE_MASK_FORCE (1<<10)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<11)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<12)
enum Serial2Protocol {
SERIAL2_MAVLINK = 1,