AP_MotorsHeli: Fully detail tradheli output functions and move manual servo handling

This commit is contained in:
Robert Lefebvre 2015-08-07 19:17:59 -04:00 committed by Randy Mackay
parent ec400e06d8
commit bf0fd3b3f2
4 changed files with 70 additions and 25 deletions

View File

@ -201,21 +201,6 @@ bool AP_MotorsHeli::rotor_runup_complete() const
return _heliflags.rotor_runup_complete;
}
void AP_MotorsHeli::output_armed_not_stabilizing()
{
// stabilizing servos always operate for helicopters
output_armed_stabilizing();
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli::output_armed_zero_throttle()
{
// stabilizing servos always operate for helicopters
// ToDo: Bring RSC Master On/Off into this function
output_armed_stabilizing();
}
// reset_swash - free up swash for maximum movements. Used for set-up
void AP_MotorsHeli::reset_swash()
{

View File

@ -168,8 +168,8 @@ protected:
// output - sends commands to the motors
virtual void output_armed_stabilizing() = 0;
void output_armed_not_stabilizing();
void output_armed_zero_throttle();
virtual void output_armed_not_stabilizing() = 0;
virtual void output_armed_zero_throttle() = 0;
virtual void output_disarmed() = 0;
// update the throttle input filter

View File

@ -257,6 +257,12 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask()
// sends commands to the motors
void AP_MotorsHeli_Single::output_armed_stabilizing()
{
// if manual override active after arming, deactivate it.
if (_servo_manual == 1) {
reset_radio_passthrough();
_servo_manual = 0;
}
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
@ -274,9 +280,69 @@ void AP_MotorsHeli_Single::output_armed_stabilizing()
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
void AP_MotorsHeli_Single::output_armed_not_stabilizing()
{
// if manual override active after arming, deactivate it.
if (_servo_manual == 1) {
reset_radio_passthrough();
_servo_manual = 0;
}
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.output_armed();
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output_armed();
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli_Single::output_armed_zero_throttle()
{
// if manual override active after arming, deactivate it.
if (_servo_manual == 1) {
reset_radio_passthrough();
_servo_manual = 0;
}
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.output_armed();
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output_armed();
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli_Single::output_disarmed()
{
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
if (_servo_manual == 1) {
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
}
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
@ -355,14 +421,6 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
//
void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
{
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
if (_servo_manual == 1) {
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
}
int16_t yaw_offset = 0;
int16_t coll_out_scaled;

View File

@ -126,6 +126,8 @@ protected:
// output - sends commands to the motors
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_armed_zero_throttle();
void output_disarmed();
// reset_servos - free up the swash servos for maximum movement