diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index e358b33c3c..1b1dd4eecb 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -787,25 +787,25 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) 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; - hal.rcin->clear_overrides(); + RC_Channels::clear_overrides(); } break; } mavlink_rc_channels_override_t packet; - int16_t v[8]; + bool override_active = false; mavlink_msg_rc_channels_override_decode(msg, &packet); - v[0] = packet.chan1_raw; - v[1] = packet.chan2_raw; - v[2] = packet.chan3_raw; - v[3] = packet.chan4_raw; - v[4] = packet.chan5_raw; - v[5] = packet.chan6_raw; - v[6] = packet.chan7_raw; - v[7] = packet.chan8_raw; + 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); // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation - copter.failsafe.rc_override_active = hal.rcin->set_overrides(v, 8); + copter.failsafe.rc_override_active = override_active; // 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(); @@ -835,10 +835,10 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) 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 |= hal.rcin->set_override(uint8_t(copter.rcmap.roll() - 1), roll); - override_active |= hal.rcin->set_override(uint8_t(copter.rcmap.pitch() - 1), pitch); - override_active |= hal.rcin->set_override(uint8_t(copter.rcmap.throttle() - 1), throttle); - override_active |= hal.rcin->set_override(uint8_t(copter.rcmap.yaw() - 1), yaw); + 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); // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation copter.failsafe.rc_override_active = override_active; diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 1de335389e..e9e370714c 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -119,7 +119,7 @@ void Copter::failsafe_gcs_check() Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED); // clear overrides so that RC control can be regained with radio. - hal.rcin->clear_overrides(); + RC_Channels::clear_overrides(); failsafe.rc_override_active = false; if (should_disarm_on_failsafe()) { diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 5e8de4ae86..75c247f644 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -95,7 +95,7 @@ void Copter::read_radio() { uint32_t tnow_ms = millis(); - if (hal.rcin->new_input()) { + if (RC_Channels::has_new_input()) { ap.new_radio_frame = true; RC_Channels::set_pwm_all(); diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index d883be1c0f..0c18cc7ece 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -226,9 +226,9 @@ void ToyMode::update() bool power_button = false; bool left_change = false; - uint16_t ch5_in = hal.rcin->read(CH_5); - uint16_t ch6_in = hal.rcin->read(CH_6); - uint16_t ch7_in = hal.rcin->read(CH_7); + uint16_t ch5_in = RC_Channels::get_radio_in(CH_5); + uint16_t ch6_in = RC_Channels::get_radio_in(CH_6); + uint16_t ch7_in = RC_Channels::get_radio_in(CH_7); if (copter.failsafe.radio || ch5_in < 900) { // failsafe handling is outside the scope of toy mode, it does @@ -715,7 +715,7 @@ void ToyMode::trim_update(void) } uint16_t chan[4]; - if (hal.rcin->read(chan, 4) != 4) { + if (RC_Channels::get_radio_in(chan, 4) != 4) { trim.start_ms = 0; return; }