Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsHeli: servo_test in range -1 to 1

Browse Source
This commit is contained in:
Randy Mackay 2016-02-06 10:38:18 +09:00
parent a42706bdcc
commit a39bbc5421
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -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;