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

Copter: Update to support new RC override conventions

Browse Source
This commit is contained in:
Michael du Breuil 2018-04-26 16:45:46 -07:00 committed by Andrew Tridgell
parent a98a8d0789
commit 6c94811e00
1 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
ArduCopter/GCS_Mavlink.cpp
View File

@ -638,7 +638,9 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs
if(msg->sysid != copter.g.sysid_my_gcs) {
break; // Only accept control from our gcs
}
if (!copter.ap.rc_override_enable) {
if (copter.failsafe.rc_override_active) { // if overrides were active previously, disable them
copter.failsafe.rc_override_active = false;
@ -646,24 +648,25 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
}
break;
}
uint32_t tnow = AP_HAL::millis();
mavlink_rc_channels_override_t packet;
bool override_active = false;
mavlink_msg_rc_channels_override_decode(msg, &packet);
override_active |= RC_Channels::set_override(0, packet.chan1_raw);
override_active |= RC_Channels::set_override(1, packet.chan2_raw);
override_active |= RC_Channels::set_override(2, packet.chan3_raw);
override_active |= RC_Channels::set_override(3, packet.chan4_raw);
override_active |= RC_Channels::set_override(4, packet.chan5_raw);
override_active |= RC_Channels::set_override(5, packet.chan6_raw);
override_active |= RC_Channels::set_override(6, packet.chan7_raw);
override_active |= RC_Channels::set_override(7, packet.chan8_raw);
RC_Channels::set_override(0, packet.chan1_raw, tnow);
RC_Channels::set_override(1, packet.chan2_raw, tnow);
RC_Channels::set_override(2, packet.chan3_raw, tnow);
RC_Channels::set_override(3, packet.chan4_raw, tnow);
RC_Channels::set_override(4, packet.chan5_raw, tnow);
RC_Channels::set_override(5, packet.chan6_raw, tnow);
RC_Channels::set_override(6, packet.chan7_raw, tnow);
RC_Channels::set_override(7, packet.chan8_raw, tnow);
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
copter.failsafe.rc_override_active = override_active;
copter.failsafe.rc_override_active = RC_Channels::has_active_overrides();
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
copter.failsafe.last_heartbeat_ms = AP_HAL::millis();
copter.failsafe.last_heartbeat_ms = tnow;
break;
}
@ -684,22 +687,23 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
}
bool override_active = false;
uint32_t tnow = AP_HAL::millis();
int16_t roll = (packet.y == INT16_MAX) ? 0 : copter.channel_roll->get_radio_min() + (copter.channel_roll->get_radio_max() - copter.channel_roll->get_radio_min()) * (packet.y + 1000) / 2000.0f;
int16_t pitch = (packet.x == INT16_MAX) ? 0 : copter.channel_pitch->get_radio_min() + (copter.channel_pitch->get_radio_max() - copter.channel_pitch->get_radio_min()) * (-packet.x + 1000) / 2000.0f;
int16_t throttle = (packet.z == INT16_MAX) ? 0 : copter.channel_throttle->get_radio_min() + (copter.channel_throttle->get_radio_max() - copter.channel_throttle->get_radio_min()) * (packet.z) / 1000.0f;
int16_t yaw = (packet.r == INT16_MAX) ? 0 : copter.channel_yaw->get_radio_min() + (copter.channel_yaw->get_radio_max() - copter.channel_yaw->get_radio_min()) * (packet.r + 1000) / 2000.0f;
override_active |= RC_Channels::set_override(uint8_t(copter.rcmap.roll() - 1), roll);
override_active |= RC_Channels::set_override(uint8_t(copter.rcmap.pitch() - 1), pitch);
override_active |= RC_Channels::set_override(uint8_t(copter.rcmap.throttle() - 1), throttle);
override_active |= RC_Channels::set_override(uint8_t(copter.rcmap.yaw() - 1), yaw);
RC_Channels::set_override(uint8_t(copter.rcmap.roll() - 1), roll, tnow);
RC_Channels::set_override(uint8_t(copter.rcmap.pitch() - 1), pitch, tnow);
RC_Channels::set_override(uint8_t(copter.rcmap.throttle() - 1), throttle, tnow);
RC_Channels::set_override(uint8_t(copter.rcmap.yaw() - 1), yaw, tnow);
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
copter.failsafe.rc_override_active = override_active;
copter.failsafe.rc_override_active = RC_Channels::has_active_overrides();
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
copter.failsafe.last_heartbeat_ms = AP_HAL::millis();
copter.failsafe.last_heartbeat_ms = tnow;
break;
}