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

AP_MotorsHeli_Single: Move Servo_Test static variables to be class members

Browse Source
This commit is contained in:
Robert Lefebvre 2015-10-30 13:38:04 -04:00 committed by Randy Mackay
parent 6e93f111c9
commit 792e989cd6
2 changed files with 44 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -473,46 +473,39 @@ void AP_MotorsHeli_Single::write_aux(int16_t servo_out)
// servo_test - move servos through full range of movement
void AP_MotorsHeli_Single::servo_test()
{
static float oscillate_angle = 0.0f;
static float servo_test_cycle_time = 0.0f;
static float collective_test = 0.0f;
static float roll_test = 0.0f;
static float pitch_test = 0.0f;
static float yaw_test = 0.0f;
_servo_test_cycle_time += 1.0f / _loop_rate;
servo_test_cycle_time += 1.0f / _loop_rate;
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));
oscillate_angle += 8 * M_PI_F / _loop_rate;
yaw_test = 2250 * 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_F / (2 * _loop_rate);
roll_test = 4500 * sinf(oscillate_angle);
pitch_test = 4500 * cosf(oscillate_angle);
yaw_test = 4500 * 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));
oscillate_angle += 8 * M_PI_F / _loop_rate;
yaw_test = 2250 * 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);
oscillate_angle += 2 * M_PI_F / _loop_rate;
yaw_test = 4500 * 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);
oscillate_angle += 2 * M_PI_F / _loop_rate;
yaw_test = 4500 * sinf(oscillate_angle);
} else { // reset cycle
servo_test_cycle_time = 0.0f;
oscillate_angle = 0.0f;
collective_test = 0.0f;
roll_test = 0.0f;
pitch_test = 0.0f;
yaw_test = 0.0f;
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));
_oscillate_angle += 8 * M_PI_F / _loop_rate;
_yaw_test = 2250 * 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_F / (2 * _loop_rate);
_roll_test = 4500 * sinf(_oscillate_angle);
_pitch_test = 4500 * cosf(_oscillate_angle);
_yaw_test = 4500 * 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));
_oscillate_angle += 8 * M_PI_F / _loop_rate;
_yaw_test = 2250 * 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);
_oscillate_angle += 2 * M_PI_F / _loop_rate;
_yaw_test = 4500 * 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);
_oscillate_angle += 2 * M_PI_F / _loop_rate;
_yaw_test = 4500 * sinf(_oscillate_angle);
} else { // reset cycle
_servo_test_cycle_time = 0.0f;
_oscillate_angle = 0.0f;
_collective_test = 0.0f;
_roll_test = 0.0f;
_pitch_test = 0.0f;
_yaw_test = 0.0f;
// decrement servo test cycle counter at the end of the cycle
if (_servo_test_cycle_counter > 0){
_servo_test_cycle_counter--;
@ -520,8 +513,8 @@ void AP_MotorsHeli_Single::servo_test()
}
// over-ride servo commands to move servos through defined ranges
_throttle_control_input = collective_test;
_roll_control_input = roll_test;
_pitch_control_input = pitch_test;
_yaw_control_input = yaw_test;
_throttle_control_input = _collective_test;
_roll_control_input = _roll_test;
_pitch_control_input = _pitch_test;
_yaw_control_input = _yaw_test;
}

8
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -158,6 +158,14 @@ protected:
AP_MotorsHeli_RSC _main_rotor; // main rotor
AP_MotorsHeli_RSC _tail_rotor; // tail rotor
// internal variables
float _oscillate_angle = 0.0f; // cyclic oscillation angle, used by servo_test function
float _servo_test_cycle_time = 0.0f; // cycle time tracker, used by servo_test function
float _collective_test = 0.0f; // over-ride for collective output, used by servo_test function
float _roll_test = 0.0f; // over-ride for roll output, used by servo_test function
float _pitch_test = 0.0f; // over-ride for pitch output, used by servo_test function
float _yaw_test = 0.0f; // over-ride for yaw output, used by servo_test function
// parameters
AP_Int16 _servo1_pos; // Angular location of swash servo #1
AP_Int16 _servo2_pos; // Angular location of swash servo #2