|
|
|
@ -512,7 +512,7 @@ void AP_MotorsHeli_Single::servo_test()
@@ -512,7 +512,7 @@ void AP_MotorsHeli_Single::servo_test()
|
|
|
|
|
|
|
|
|
|
if ((_servo_test_cycle_time >= 0.0f && _servo_test_cycle_time < 0.5f)|| // Tilt swash back
|
|
|
|
|
(_servo_test_cycle_time >= 6.0f && _servo_test_cycle_time < 6.5f)){ |
|
|
|
|
_pitch_test += (1.0f / (_loop_rate/2)); |
|
|
|
|
_pitch_test += (1.0f / (_loop_rate / 2.0f)); |
|
|
|
|
_oscillate_angle += 8 * M_PI / _loop_rate; |
|
|
|
|
_yaw_test = 0.5f * sinf(_oscillate_angle); |
|
|
|
|
} else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around
|
|
|
|
@ -523,7 +523,7 @@ void AP_MotorsHeli_Single::servo_test()
@@ -523,7 +523,7 @@ void AP_MotorsHeli_Single::servo_test()
|
|
|
|
|
_yaw_test = sinf(_oscillate_angle); |
|
|
|
|
} else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)|| // Return swash to level
|
|
|
|
|
(_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){ |
|
|
|
|
_pitch_test -= (1.0f / (_loop_rate/2)); |
|
|
|
|
_pitch_test -= (1.0f / (_loop_rate / 2.0f)); |
|
|
|
|
_oscillate_angle += 8 * M_PI / _loop_rate; |
|
|
|
|
_yaw_test = 0.5f * sinf(_oscillate_angle); |
|
|
|
|
} else if (_servo_test_cycle_time >= 5.0f && _servo_test_cycle_time < 6.0f){ // Raise swash to top
|
|
|
|
|