Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: added MANUAL_CONTROL support 

allows for joystick with QGC
This commit is contained in:
Andrew Tridgell 2017-09-10 07:21:18 +10:00
parent fd07a82c8f
commit 31af23cab6
1 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduPlane/GCS_Mavlink.cpp
View File

@ -1605,6 +1605,34 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
if (msg->sysid != plane.g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
bool override_active = false;
int16_t roll = (packet.y == INT16_MAX) ? 0 : plane.channel_roll->get_radio_min() + (plane.channel_roll->get_radio_max() - plane.channel_roll->get_radio_min()) * (packet.y + 1000) / 2000.0f;
int16_t pitch = (packet.x == INT16_MAX) ? 0 : plane.channel_pitch->get_radio_min() + (plane.channel_pitch->get_radio_max() - plane.channel_pitch->get_radio_min()) * (-packet.x + 1000) / 2000.0f;
int16_t throttle = (packet.z == INT16_MAX) ? 0 : plane.channel_throttle->get_radio_min() + (plane.channel_throttle->get_radio_max() - plane.channel_throttle->get_radio_min()) * (packet.z) / 1000.0f;
int16_t yaw = (packet.r == INT16_MAX) ? 0 : plane.channel_rudder->get_radio_min() + (plane.channel_rudder->get_radio_max() - plane.channel_rudder->get_radio_min()) * (packet.r + 1000) / 2000.0f;
override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.roll() - 1), roll);
override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.pitch() - 1), pitch);
override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.throttle() - 1), throttle);
override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.yaw() - 1), yaw);
if (override_active) {
plane.failsafe.last_valid_rc_ms = AP_HAL::millis();
plane.failsafe.AFS_last_valid_rc_ms = plane.failsafe.last_valid_rc_ms;
}
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
plane.failsafe.last_heartbeat_ms = AP_HAL::millis();
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from