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: Create Servo Test functionality

Browse Source
This commit is contained in:
Robert Lefebvre 2015-10-21 19:47:24 -04:00 committed by Randy Mackay
parent ae1fbdb68a
commit 99212f71bf
3 changed files with 59 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -147,6 +147,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX),
// @Param: SV_TEST
// @DisplayName: Boot-up Servo Test Cycles
// @Description: Number of cycles to run servo test on boot-up
// @Range: 0 10
// @Increment: 1
// @User: Standard
AP_GROUPINFO("SV_TEST", 17, AP_MotorsHeli, _servo_test, 0),
AP_GROUPEND
};
@ -160,6 +168,9 @@ void AP_MotorsHeli::Init()
// set update rate
set_update_rate(_speed_hz);
// load boot-up servo test cycles into counter to be consumed
_servo_test_cycle_counter = _servo_test;
// ensure inputs are not passed through to servos on start-up
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED;
@ -246,43 +257,48 @@ void AP_MotorsHeli::output_armed_zero_throttle()
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
// manual override (i.e. when setting up swash)
switch (_servo_mode) {
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
// pass pilot commands straight through to swash
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
break;
case SERVO_CONTROL_MODE_MANUAL_CENTER:
// fixate mid collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = _collective_mid_pwm;
_yaw_control_input = 0;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
// fixate max collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 1000;
_yaw_control_input = 4500;
break;
case SERVO_CONTROL_MODE_MANUAL_MIN:
// fixate min collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 0;
_yaw_control_input = -4500;
break;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
// use servo_test function from child classes
servo_test();
break;
default:
// no manual override
break;
if (_servo_test_cycle_counter > 0){
// perform boot-up servo test cycle if enabled
servo_test();
} else {
// manual override (i.e. when setting up swash)
switch (_servo_mode) {
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
// pass pilot commands straight through to swash
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
break;
case SERVO_CONTROL_MODE_MANUAL_CENTER:
// fixate mid collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = _collective_mid_pwm;
_yaw_control_input = 0;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
// fixate max collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 1000;
_yaw_control_input = 4500;
break;
case SERVO_CONTROL_MODE_MANUAL_MIN:
// fixate min collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 0;
_yaw_control_input = -4500;
break;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
// use servo_test function from child classes
servo_test();
break;
default:
// no manual override
break;
}
}
// ensure swash servo endpoints haven't been moved

2
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -232,6 +232,7 @@ protected:
AP_Int16 _rsc_idle_output; // Rotor control output while at idle
AP_Int16 _rsc_power_low; // throttle value sent to throttle servo at zero collective pitch
AP_Int16 _rsc_power_high; // throttle value sent to throttle servo at maximum collective pitch
AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup
// internal variables
float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
@ -249,6 +250,7 @@ protected:
int16_t _throttle_radio_passthrough = 0; // throttle control PWM direct from radio, used for manual control
int16_t _yaw_radio_passthrough = 0; // yaw control PWM direct from radio, used for manual control
int16_t _collective_range = 0; // maximum absolute collective pitch range (500 - 1000)
uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup
};
#endif // AP_MOTORSHELI

4
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -513,6 +513,10 @@ void AP_MotorsHeli_Single::servo_test()
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--;
}
}
// over-ride servo commands to move servos through defined ranges