Sub: Add manual mode functions

ArduSub/Sub.h

@@ -826,6 +826,8 @@ private:
     void sport_run();
     bool stabilize_init(bool ignore_checks);
     void stabilize_run();
+    bool manual_init(bool ignore_checks);
+    void manual_run();
     void crash_check();
     void parachute_check();
     void parachute_release();

ArduSub/control_manual.cpp

@@ -0,0 +1,39 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include "Sub.h"
+
+// manual_init - initialise manual controller
+bool Sub::manual_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) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
+        return false;
+    }
+    // set target altitude to zero for reporting
+    pos_control.set_alt_target(0);
+
+    return true;
+}
+
+// manual_run - runs the manual (passthrough) controller
+// should be called at 100hz or more
+void Sub::manual_run()
+{
+    float target_roll, target_pitch, target_yaw;
+    float pilot_throttle_scaled;
+
+    // if not armed set throttle to zero and exit immediately
+    if (!motors.armed() || !motors.get_interlock()) {
+        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        return;
+    }
+
+    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+
+    motors.set_roll(channel_roll->norm_input());
+    motors.set_pitch(channel_pitch->norm_input());
+    motors.set_yaw(channel_yaw->norm_input());
+    motors.set_throttle(channel_throttle->norm_input());
+    motors.set_forward(channel_forward->norm_input());
+    motors.set_lateral(channel_lateral->norm_input());
+}

ArduSub/defines.h

@@ -113,7 +113,8 @@ enum control_mode_t {
     AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
     POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
     BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
-	THROW =        18   // throw to launch mode using inertial/GPS system, no pilot input
+	THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
+	MANUAL =	   19   // Pass-through input with no stabilization
 };
 
 enum mode_reason_t {

ArduSub/flight_mode.cpp

@@ -95,6 +95,10 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
         	success = throw_init(ignore_checks);
         	break;
 
+        case MANUAL:
+        	success = manual_init(ignore_checks);
+        	break;
+
         default:
             success = false;
             break;
@@ -207,6 +211,11 @@ void Sub::update_flight_mode()
         case THROW:
         	throw_run();
         	break;
+
+        case MANUAL:
+        	manual_run();
+        	break;
+
         default:
         	break;
     }