From 387f3276cb0a04f36255c6128371fc572f0aaaee Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 14 Nov 2014 17:18:17 -0800 Subject: [PATCH] Copter: handle SET_POSITION_TARGET mavlink messages --- ArduCopter/GCS_Mavlink.pde | 69 +++++++++++++++++++++++++++++++++++--- ArduCopter/defines.h | 7 ++++ 2 files changed, 71 insertions(+), 5 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 5a81de2886..5706a4978a 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6e3f96e226..fcff903ba3 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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,