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

Copter: support SET_POSITION_TARGET messages

Browse Source
This commit is contained in:
Randy Mackay 2014-10-13 20:31:40 +09:00
parent 992ce9b69c
commit c636ea9101
1 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/GCS_Mavlink.pde
View File

@ -1283,6 +1283,50 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
} }
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
break;
}
// exit if vehicle is not in guided mode
if (control_mode != GUIDED) {
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));
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
{
// decode packet
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
break;
}
// exit if vehicle is not in guided mode
if (control_mode != GUIDED) {
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));
break;
}
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE: // MAV ID: 90 case MAVLINK_MSG_ID_HIL_STATE: // MAV ID: 90
{ {