diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index 4f9cf85ae8..583c668b8f 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -47,6 +47,11 @@ void Copter::acro_run() // output pilot's throttle without angle boost attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); + + //control_in is range 0-1000 + //radio_in is raw pwm value + motors.set_forward(channel_forward->radio_in); + motors.set_strafe(channel_strafe->radio_in); } diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index ec652e214f..b38f2cad4b 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -158,4 +158,9 @@ void Copter::althold_run() pos_control.update_z_controller(); break; } + + //control_in is range 0-1000 + //radio_in is raw pwm value + motors.set_forward(channel_forward->radio_in); + motors.set_strafe(channel_strafe->radio_in); } diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 9ad9fce66f..fc0c5e2001 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -57,4 +57,9 @@ void Copter::stabilize_run() // output pilot's throttle attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); + + //control_in is range 0-1000 + //radio_in is raw pwm value + motors.set_forward(channel_forward->radio_in); + motors.set_strafe(channel_strafe->radio_in); }