Sub: 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

@@ -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

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();

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);
+}