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

Plane: Can now change current WP alt via mavlink.

Browse Source 
Uses the mavlink msg SET_POSITION_TARGET_GLOBAL_INT
This commit is contained in:
Michael Day 2016-05-09 10:08:21 -04:00 committed by Tom Pittenger
parent 01e636065d
commit 429e348824
2 changed files with 54 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
ArduPlane/GCS_Mavlink.cpp
View File

@ -2110,6 +2110,58 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
{
//Only want to allow companion computer position control when
// in a certain mode to avoid inadvertently sending these
// kinds of commands when the autopilot is responding to problems
// in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode
// for companion computer control is more safe (provided
// one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
if (plane.control_mode != GUIDED) { //don't screw up failsafes
break;
}
mavlink_set_position_target_global_int_t pos_target;
mavlink_msg_set_position_target_global_int_decode(msg, &pos_target);
//Unexepectedly, the mask is expecting "ones" for dimensions that should
//be IGNORNED rather than INCLUDED. See mavlink documentation of the
//SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3)
bool msg_valid = true;
AP_Mission::Mission_Command cmd = {0};
if (pos_target.type_mask & alt_mask)
{
cmd.content.location.alt = pos_target.alt * 100;
cmd.content.location.flags.relative_alt = false;
cmd.content.location.flags.terrain_alt = false;
switch (pos_target.coordinate_frame)
{
case MAV_FRAME_GLOBAL_INT:
break; //default to MSL altitude
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
cmd.content.location.flags.relative_alt = true;
break;
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
cmd.content.location.flags.relative_alt = true;
cmd.content.location.flags.terrain_alt = true;
break;
default:
plane.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Invalid coord fr. (SET_POSTION_TARGET_GLOBAL_INT)");
msg_valid = false;
break;
}
if (msg_valid) {
handle_change_alt_request(cmd);
}
} // end if alt_mask
break;
}
case MAVLINK_MSG_ID_ADSB_VEHICLE:
plane.adsb.update_vehicle(msg);
break;

2
ArduPlane/capabilities.cpp
View File

@ -8,4 +8,6 @@ void Plane::init_capabilities(void)
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_COMMAND_INT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
}