From 63a866b3926661fa355a4e476e2075cc5993fb0a Mon Sep 17 00:00:00 2001
From: Rustom Jehangir <rustyj110@gmail.com>
Date: Fri, 1 Jan 2016 16:59:01 -0800
Subject: [PATCH] Added control_rov file to hold the two methods needed for
 simple ROV control and basic testing of this project. Also added the code
 needed to pass the forward and strafe inputs to the motors class.

---
 ArduSub/ArduSub.cpp     |  3 +++
 ArduSub/Copter.h        |  2 ++
 ArduSub/control_rov.cpp | 57 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 62 insertions(+)
 create mode 100644 ArduSub/control_rov.cpp

diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index c1d129245a..f02d8e084a 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -261,6 +261,9 @@ void Copter::fast_loop()
     // run low level rate controllers that only require IMU data
     attitude_control.rate_controller_run();
     
+    motors.set_forward(channel_forward->control_in);
+    motors.set_strafe(channel_strafe->control_in);
+
 #if FRAME_CONFIG == HELI_FRAME
     update_heli_control_dynamics();
 #endif //HELI_FRAME
diff --git a/ArduSub/Copter.h b/ArduSub/Copter.h
index 32460b1e62..38b4a7f3f5 100644
--- a/ArduSub/Copter.h
+++ b/ArduSub/Copter.h
@@ -811,6 +811,8 @@ private:
     void sport_run();
     bool stabilize_init(bool ignore_checks);
     void stabilize_run();
+    bool rov_init(bool ignore_checks);
+    void rov_run();
     void crash_check();
     void parachute_check();
     void parachute_release();
diff --git a/ArduSub/control_rov.cpp b/ArduSub/control_rov.cpp
new file mode 100644
index 0000000000..50f327fd59
--- /dev/null
+++ b/ArduSub/control_rov.cpp
@@ -0,0 +1,57 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Copter.h"
+
+/*
+ * control_rov.cpp - Control for basic ROV operation
+ */
+
+// stabilize_init - initialise stabilize controller
+bool Copter::rov_init(bool ignore_checks)
+{
+    // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
+    //if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
+    //    return false;
+    //}
+    // set target altitude to zero for reporting
+    pos_control.set_alt_target(0);
+
+    return true;
+}
+
+// stabilize_run - runs the main stabilize controller
+// should be called at 100hz or more
+void Copter::rov_run()
+{
+    float target_roll, target_pitch;
+    float target_yaw_rate;
+    int16_t pilot_throttle_scaled;
+
+    // if not armed or throttle at zero, set throttle to zero and exit immediately
+    if(!motors.armed() || ap.throttle_zero) {
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        // slow start if landed
+        if (ap.land_complete) {
+            motors.slow_start(true);
+        }
+        return;
+    }
+
+    // convert pilot input to lean angles
+    // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
+    get_pilot_desired_lean_angles(0, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
+
+    // get pilot's desired yaw rate
+    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
+
+    // get pilot's desired throttle
+    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
+
+    // call attitude controller
+    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
+
+    // body-frame rate controller is run directly from 100hz loop
+
+    // output pilot's throttle
+    attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
+}