From c642d1ed74e0b5978add64f020b9cd06192c1823 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 28 May 2018 23:26:16 -0700 Subject: [PATCH] Sub: Rework to support override changes --- ArduSub/GCS_Mavlink.cpp | 23 +++++++++++++---------- ArduSub/joystick.cpp | 33 ++++++++++++++++++--------------- 2 files changed, 31 insertions(+), 25 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 502d0478d9..06b2969aac 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -713,21 +713,24 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) if (msg->sysid != sub.g.sysid_my_gcs) { break; // Only accept control from our gcs } + + uint32_t tnow = AP_HAL::millis(); + mavlink_rc_channels_override_t packet; mavlink_msg_rc_channels_override_decode(msg, &packet); - 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); + 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); - sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); + sub.failsafe.last_pilot_input_ms = tnow; // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes - sub.failsafe.last_heartbeat_ms = AP_HAL::millis(); + sub.failsafe.last_heartbeat_ms = tnow; break; } diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 6d2026a9f4..c7f03b89b4 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -91,28 +91,30 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t rollTrim = y * rpyScale; } - RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900)); // pitch - RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900)); // roll + uint32_t tnow = AP_HAL::millis(); - RC_Channels::set_override(2, constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900)); // throttle - RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900)); // yaw + RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900), tnow); // pitch + RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900), tnow); // roll + + RC_Channels::set_override(2, constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900), tnow); // throttle + RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow); // yaw // maneuver mode: if (roll_pitch_flag == 0) { // adjust forward and lateral with joystick input instead of roll and pitch - RC_Channels::set_override(4, constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900)); // forward for ROV - RC_Channels::set_override(5, constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900)); // lateral for ROV + RC_Channels::set_override(4, constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV + RC_Channels::set_override(5, constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV } else { // neutralize forward and lateral input while we are adjusting roll and pitch - RC_Channels::set_override(4, constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900)); // forward for ROV - RC_Channels::set_override(5, constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900)); // lateral for ROV + RC_Channels::set_override(4, constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV + RC_Channels::set_override(5, constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV } - RC_Channels::set_override(6, cam_pan); // camera pan - RC_Channels::set_override(7, cam_tilt); // camera tilt - RC_Channels::set_override(8, lights1); // lights 1 - RC_Channels::set_override(9, lights2); // lights 2 - RC_Channels::set_override(10, video_switch); // video switch + RC_Channels::set_override(6, cam_pan, tnow); // camera pan + RC_Channels::set_override(7, cam_tilt, tnow); // camera tilt + RC_Channels::set_override(8, lights1, tnow); // lights 1 + RC_Channels::set_override(9, lights2, tnow); // lights 2 + RC_Channels::set_override(10, video_switch, tnow); // video switch // Store old x, y, z values for use in input hold logic x_last = x; @@ -678,13 +680,14 @@ void Sub::default_js_buttons() void Sub::set_neutral_controls() { + uint32_t tnow = AP_HAL::millis(); for (uint8_t i = 0; i < 6; i++) { - RC_Channels::set_override(i, 1500); + RC_Channels::set_override(i, 1500, tnow); } for (uint8_t i = 6; i < 11; i++) { - RC_Channels::set_override(i, 0xffff); + RC_Channels::set_override(i, 0xffff, tnow); } // Clear pitch/roll trim settings