From a39bbc542166db53832c2722784a284f226f748c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 6 Feb 2016 10:38:18 +0900 Subject: [PATCH] AP_MotorsHeli: servo_test in range -1 to 1 --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 22 ++++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index ee762d0aab..217dfe23bb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -462,28 +462,28 @@ 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 += (4500 / (_loop_rate/2)); + _pitch_test += (1.0f / (_loop_rate/2)); _oscillate_angle += 8 * M_PI / _loop_rate; - _yaw_test = 2250 * sinf(_oscillate_angle); + _yaw_test = 0.5f * sinf(_oscillate_angle); } else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around (_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){ _oscillate_angle += M_PI / (2 * _loop_rate); - _roll_test = 4500 * sinf(_oscillate_angle); - _pitch_test = 4500 * cosf(_oscillate_angle); - _yaw_test = 4500 * sinf(_oscillate_angle); + _roll_test = sinf(_oscillate_angle); + _pitch_test = cosf(_oscillate_angle); + _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 -= (4500 / (_loop_rate/2)); + _pitch_test -= (1.0f / (_loop_rate/2)); _oscillate_angle += 8 * M_PI / _loop_rate; - _yaw_test = 2250 * sinf(_oscillate_angle); + _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 - _collective_test += (1000 / _loop_rate); + _collective_test += (1.0f / _loop_rate); _oscillate_angle += 2 * M_PI / _loop_rate; - _yaw_test = 4500 * sinf(_oscillate_angle); + _yaw_test = sinf(_oscillate_angle); } else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){ // Lower swash to bottom - _collective_test -= (1000 / _loop_rate); + _collective_test -= (1.0f / _loop_rate); _oscillate_angle += 2 * M_PI / _loop_rate; - _yaw_test = 4500 * sinf(_oscillate_angle); + _yaw_test = sinf(_oscillate_angle); } else { // reset cycle _servo_test_cycle_time = 0.0f; _oscillate_angle = 0.0f;