From 08a56ee6d2c8fc168c5192d556319f6f9a90b164 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Sun, 10 Jan 2016 16:23:03 -0800 Subject: [PATCH] Sub: Added function to transform MANUAL_CONTROL messages to RC override. This allows MANUAL_CONTROL to work properly without changing much else. --- ArduSub/Copter.h | 1 + ArduSub/GCS_Mavlink.cpp | 37 +++++++++---------------------------- ArduSub/radio.cpp | 21 +++++++++++++++++++++ 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/ArduSub/Copter.h b/ArduSub/Copter.h index 71a063c48b..61c431118b 100644 --- a/ArduSub/Copter.h +++ b/ArduSub/Copter.h @@ -914,6 +914,7 @@ private: void init_rc_out(); void enable_motor_output(); void read_radio(); + void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); void init_barometer(bool full_calibration); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index b78818cd6c..0285c75583 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1107,36 +1107,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } case MAVLINK_MSG_ID_MANUAL_CONTROL: // MAV ID: 69 - { - if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs - mavlink_manual_control_t packet; - int16_t v[8]; - uint16_t b; - mavlink_msg_manual_control_decode(msg, &packet); + { + if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_manual_control_t packet; + mavlink_msg_manual_control_decode(msg, &packet); - static uint16_t skip = 0; + copter.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons); - if ( skip++ % 25 == 0 ) { - copter.gcs_send_text_fmt(MAV_SEVERITY_INFO,"%4.0f,%4.0f,%4.0f",packet.x,packet.y,packet.z); - } - - v[0] = packet.x/2+1500; - v[1] = packet.y/2+1500; - v[2] = packet.z+1000; - v[3] = packet.r/2+1500; - v[4] = packet.buttons; - v[5] = 0; - v[6] = 0; - v[7] = 0; - b = packet.buttons; - - // 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); - - // 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(); - break; - } + // 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(); + break; + } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // MAV ID: 70 { diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index 9e4b206106..c875244eb9 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -141,6 +141,27 @@ void Copter::read_radio() } } +void Copter::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) { + int16_t channels[8]; + + float rpyScale = 0.4; + float throttleScale = 0.8; + int16_t rpyCenter = 1500; + int16_t throttleBase = 1100; + + channels[0] = x*rpyScale+rpyCenter; // pitch (forward for ROV) + channels[1] = y*rpyScale+rpyCenter; // roll (strafe for ROV) + channels[2] = z*throttleScale+throttleBase; // throttle + channels[3] = r*rpyScale+rpyCenter; // yaw + channels[4] = buttons; // for testing only + channels[5] = 1100; + channels[6] = 1100; + channels[7] = 1500; // camera tilt + + // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation + failsafe.rc_override_active = hal.rcin->set_overrides(channels, 8); +} + #define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm) {