Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -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 02b8dd5ffd
commit b89b0d7f9e
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

@ -482,46 +482,39 @@ void AP_MotorsHeli_Single::write_aux(int16_t servo_out)
// servo_test - move servos through full range of movement // servo_test - move servos through full range of movement
void AP_MotorsHeli_Single::servo_test() void AP_MotorsHeli_Single::servo_test()
{ {
static float oscillate_angle = 0.0f; _servo_test_cycle_time += 1.0f / _loop_rate;
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; 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)){
if ((servo_test_cycle_time >= 0.0f && servo_test_cycle_time < 0.5f)|| // Tilt swash back _pitch_test += (4500 / (_loop_rate/2));
(servo_test_cycle_time >= 6.0f && servo_test_cycle_time < 6.5f)){ _oscillate_angle += 8 * M_PI_F / _loop_rate;
pitch_test += (4500 / (_loop_rate/2)); _yaw_test = 2250 * sinf(_oscillate_angle);
oscillate_angle += 8 * M_PI_F / _loop_rate; } else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around
yaw_test = 2250 * sinf(oscillate_angle); (_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){
} else if ((servo_test_cycle_time >= 0.5f && servo_test_cycle_time < 4.5f)|| // Roll swash around _oscillate_angle += M_PI_F / (2 * _loop_rate);
(servo_test_cycle_time >= 6.5f && servo_test_cycle_time < 10.5f)){ _roll_test = 4500 * sinf(_oscillate_angle);
oscillate_angle += M_PI_F / (2 * _loop_rate); _pitch_test = 4500 * cosf(_oscillate_angle);
roll_test = 4500 * sinf(oscillate_angle); _yaw_test = 4500 * sinf(_oscillate_angle);
pitch_test = 4500 * cosf(oscillate_angle); } else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)|| // Return swash to level
yaw_test = 4500 * sinf(oscillate_angle); (_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){
} else if ((servo_test_cycle_time >= 4.5f && servo_test_cycle_time < 5.0f)|| // Return swash to level _pitch_test -= (4500 / (_loop_rate/2));
(servo_test_cycle_time >= 10.5f && servo_test_cycle_time < 11.0f)){ _oscillate_angle += 8 * M_PI_F / _loop_rate;
pitch_test -= (4500 / (_loop_rate/2)); _yaw_test = 2250 * sinf(_oscillate_angle);
oscillate_angle += 8 * M_PI_F / _loop_rate; } else if (_servo_test_cycle_time >= 5.0f && _servo_test_cycle_time < 6.0f){ // Raise swash to top
yaw_test = 2250 * sinf(oscillate_angle); _collective_test += (1000 / _loop_rate);
} else if (servo_test_cycle_time >= 5.0f && servo_test_cycle_time < 6.0f){ // Raise swash to top _oscillate_angle += 2 * M_PI_F / _loop_rate;
collective_test += (1000 / _loop_rate); _yaw_test = 4500 * sinf(_oscillate_angle);
oscillate_angle += 2 * M_PI_F / _loop_rate; } else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){ // Lower swash to bottom
yaw_test = 4500 * sinf(oscillate_angle); _collective_test -= (1000 / _loop_rate);
} else if (servo_test_cycle_time >= 11.0f && servo_test_cycle_time < 12.0f){ // Lower swash to bottom _oscillate_angle += 2 * M_PI_F / _loop_rate;
collective_test -= (1000 / _loop_rate); _yaw_test = 4500 * sinf(_oscillate_angle);
oscillate_angle += 2 * M_PI_F / _loop_rate; } else { // reset cycle
yaw_test = 4500 * sinf(oscillate_angle); _servo_test_cycle_time = 0.0f;
} else { // reset cycle _oscillate_angle = 0.0f;
servo_test_cycle_time = 0.0f; _collective_test = 0.0f;
oscillate_angle = 0.0f; _roll_test = 0.0f;
collective_test = 0.0f; _pitch_test = 0.0f;
roll_test = 0.0f; _yaw_test = 0.0f;
pitch_test = 0.0f;
yaw_test = 0.0f;
// decrement servo test cycle counter at the end of the cycle // decrement servo test cycle counter at the end of the cycle
if (_servo_test_cycle_counter > 0){ if (_servo_test_cycle_counter > 0){
_servo_test_cycle_counter--; _servo_test_cycle_counter--;
@ -529,8 +522,8 @@ void AP_MotorsHeli_Single::servo_test()
} }
// over-ride servo commands to move servos through defined ranges // over-ride servo commands to move servos through defined ranges
_throttle_control_input = collective_test; _throttle_control_input = _collective_test;
_roll_control_input = roll_test; _roll_control_input = _roll_test;
_pitch_control_input = pitch_test; _pitch_control_input = _pitch_test;
_yaw_control_input = yaw_test; _yaw_control_input = _yaw_test;
} }

8
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -157,6 +157,14 @@ protected:
AP_MotorsHeli_RSC _main_rotor; // main rotor AP_MotorsHeli_RSC _main_rotor; // main rotor
AP_MotorsHeli_RSC _tail_rotor; // tail 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 // parameters
AP_Int16 _servo1_pos; // Angular location of swash servo #1 AP_Int16 _servo1_pos; // Angular location of swash servo #1
AP_Int16 _servo2_pos; // Angular location of swash servo #2 AP_Int16 _servo2_pos; // Angular location of swash servo #2