diff --git a/ArduSub/mode_manual.cpp b/ArduSub/mode_manual.cpp index a1d235e044..169ec547d7 100644 --- a/ArduSub/mode_manual.cpp +++ b/ArduSub/mode_manual.cpp @@ -29,7 +29,7 @@ void ModeManual::run() sub.motors.set_roll(channel_roll->norm_input()); sub.motors.set_pitch(channel_pitch->norm_input()); sub.motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P); - sub.motors.set_throttle(channel_throttle->norm_input()); + sub.motors.set_throttle((channel_throttle->norm_input() + 1.0f) / 2.0f); sub.motors.set_forward(channel_forward->norm_input()); sub.motors.set_lateral(channel_lateral->norm_input()); } diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp index 29b8c8fcba..7eb7b39f89 100644 --- a/ArduSub/mode_stabilize.cpp +++ b/ArduSub/mode_stabilize.cpp @@ -60,7 +60,7 @@ void ModeStabilize::run() } // output pilot's throttle - attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); + attitude_control->set_throttle_out((channel_throttle->norm_input() + 1.0f) / 2.0f, false, g.throttle_filt); //control_in is range -1000-1000 //radio_in is raw pwm value