Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -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 // @User: Advanced
AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX), 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 AP_GROUPEND
}; };
@ -160,6 +168,9 @@ void AP_MotorsHeli::Init()
// set update rate // set update rate
set_update_rate(_speed_hz); 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 // ensure inputs are not passed through to servos on start-up
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED; _servo_mode = SERVO_CONTROL_MODE_AUTOMATED;
@ -246,43 +257,48 @@ void AP_MotorsHeli::output_armed_zero_throttle()
// output_disarmed - sends commands to the motors // output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed() void AP_MotorsHeli::output_disarmed()
{ {
// manual override (i.e. when setting up swash) if (_servo_test_cycle_counter > 0){
switch (_servo_mode) { // perform boot-up servo test cycle if enabled
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH: servo_test();
// pass pilot commands straight through to swash } else {
_roll_control_input = _roll_radio_passthrough; // manual override (i.e. when setting up swash)
_pitch_control_input = _pitch_radio_passthrough; switch (_servo_mode) {
_throttle_control_input = _throttle_radio_passthrough; case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
_yaw_control_input = _yaw_radio_passthrough; // pass pilot commands straight through to swash
break; _roll_control_input = _roll_radio_passthrough;
case SERVO_CONTROL_MODE_MANUAL_CENTER: _pitch_control_input = _pitch_radio_passthrough;
// fixate mid collective _throttle_control_input = _throttle_radio_passthrough;
_roll_control_input = 0; _yaw_control_input = _yaw_radio_passthrough;
_pitch_control_input = 0; break;
_throttle_control_input = _collective_mid_pwm; case SERVO_CONTROL_MODE_MANUAL_CENTER:
_yaw_control_input = 0; // fixate mid collective
break; _roll_control_input = 0;
case SERVO_CONTROL_MODE_MANUAL_MAX: _pitch_control_input = 0;
// fixate max collective _throttle_control_input = _collective_mid_pwm;
_roll_control_input = 0; _yaw_control_input = 0;
_pitch_control_input = 0; break;
_throttle_control_input = 1000; case SERVO_CONTROL_MODE_MANUAL_MAX:
_yaw_control_input = 4500; // fixate max collective
break; _roll_control_input = 0;
case SERVO_CONTROL_MODE_MANUAL_MIN: _pitch_control_input = 0;
// fixate min collective _throttle_control_input = 1000;
_roll_control_input = 0; _yaw_control_input = 4500;
_pitch_control_input = 0; break;
_throttle_control_input = 0; case SERVO_CONTROL_MODE_MANUAL_MIN:
_yaw_control_input = -4500; // fixate min collective
break; _roll_control_input = 0;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE: _pitch_control_input = 0;
// use servo_test function from child classes _throttle_control_input = 0;
servo_test(); _yaw_control_input = -4500;
break; break;
default: case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
// no manual override // use servo_test function from child classes
break; servo_test();
break;
default:
// no manual override
break;
}
} }
// ensure swash servo endpoints haven't been moved // 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_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_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_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 // internal variables
float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; 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 _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 _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) 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 #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; roll_test = 0.0f;
pitch_test = 0.0f; pitch_test = 0.0f;
yaw_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 // over-ride servo commands to move servos through defined ranges