diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 4e07080626..bdf64e7e20 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -949,19 +949,16 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) break; // Only accept control from our gcs } mavlink_rc_channels_override_t packet; - int16_t v[8]; 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; - - hal.rcin->set_overrides(v, 8); + RC_Channels::set_override(0, packet.chan1_raw); + RC_Channels::set_override(1, packet.chan2_raw); + RC_Channels::set_override(2, packet.chan3_raw); + RC_Channels::set_override(3, packet.chan4_raw); + RC_Channels::set_override(4, packet.chan5_raw); + RC_Channels::set_override(5, packet.chan6_raw); + RC_Channels::set_override(6, packet.chan7_raw); + RC_Channels::set_override(7, packet.chan8_raw); sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 0550862f40..19d18ffe22 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -121,7 +121,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t y_last = y; z_last = z; - hal.rcin->set_overrides(channels, 11); + RC_Channels::set_overrides(channels, 11); } void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) @@ -690,7 +690,7 @@ void Sub::set_neutral_controls() channels[i] = 0xffff; } - hal.rcin->set_overrides(channels, 10); + RC_Channels::set_overrides(channels, 10); // Clear pitch/roll trim settings pitchTrim = 0; diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index 37cf701889..56e955641d 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -28,21 +28,21 @@ void Sub::init_rc_in() #if CONFIG_HAL_BOARD != HAL_BOARD_SITL // initialize rc input to 1500 on control channels (rather than 0) for (int i = 0; i < 6; i++) { - hal.rcin->set_override(i, 1500); + RC_Channels::set_override(i, 1500); } - hal.rcin->set_override(6, 1500); // camera pan channel - hal.rcin->set_override(7, 1500); // camera tilt channel + RC_Channels::set_override(6, 1500); // camera pan channel + RC_Channels::set_override(7, 1500); // camera tilt channel RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); - hal.rcin->set_override(8, min); // lights 1 channel + RC_Channels::set_override(8, min); // lights 1 channel chan = RC_Channels::rc_channel(9); min = chan->get_radio_min(); - hal.rcin->set_override(9, min); // lights 2 channel + RC_Channels::set_override(9, min); // lights 2 channel - hal.rcin->set_override(10, 1100); // video switch + RC_Channels::set_override(10, 1100); // video switch #endif }