diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 6e2d394152..5af616a727 100644 --- a/ArduSub/Sub.h +++ b/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(); diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp new file mode 100644 index 0000000000..0b1081a7ff --- /dev/null +++ b/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()); +} diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 4f998086d7..eb3e830893 100644 --- a/ArduSub/defines.h +++ b/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 { diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index a34852cdba..4e1a0b2461 100644 --- a/ArduSub/flight_mode.cpp +++ b/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; }