mirror of https://github.com/ArduPilot/ardupilot
AP_MotorsHeli: Fully detail tradheli output functions and move manual servo handling
This commit is contained in:
parent
7dda0f651a
commit
10b0be9e75
|
@ -201,21 +201,6 @@ bool AP_MotorsHeli::rotor_runup_complete() const
|
||||||
return _heliflags.rotor_runup_complete;
|
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
|
// reset_swash - free up swash for maximum movements. Used for set-up
|
||||||
void AP_MotorsHeli::reset_swash()
|
void AP_MotorsHeli::reset_swash()
|
||||||
{
|
{
|
||||||
|
|
|
@ -168,8 +168,8 @@ protected:
|
||||||
|
|
||||||
// output - sends commands to the motors
|
// output - sends commands to the motors
|
||||||
virtual void output_armed_stabilizing() = 0;
|
virtual void output_armed_stabilizing() = 0;
|
||||||
void output_armed_not_stabilizing();
|
virtual void output_armed_not_stabilizing() = 0;
|
||||||
void output_armed_zero_throttle();
|
virtual void output_armed_zero_throttle() = 0;
|
||||||
virtual void output_disarmed() = 0;
|
virtual void output_disarmed() = 0;
|
||||||
|
|
||||||
// update the throttle input filter
|
// update the throttle input filter
|
||||||
|
|
|
@ -257,6 +257,12 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask()
|
||||||
// sends commands to the motors
|
// sends commands to the motors
|
||||||
void AP_MotorsHeli_Single::output_armed_stabilizing()
|
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);
|
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
||||||
|
|
||||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
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();
|
_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
|
// output_disarmed - sends commands to the motors
|
||||||
void AP_MotorsHeli_Single::output_disarmed()
|
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);
|
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
||||||
|
|
||||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
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)
|
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 yaw_offset = 0;
|
||||||
int16_t coll_out_scaled;
|
int16_t coll_out_scaled;
|
||||||
|
|
||||||
|
|
|
@ -126,6 +126,8 @@ protected:
|
||||||
|
|
||||||
// output - sends commands to the motors
|
// output - sends commands to the motors
|
||||||
void output_armed_stabilizing();
|
void output_armed_stabilizing();
|
||||||
|
void output_armed_not_stabilizing();
|
||||||
|
void output_armed_zero_throttle();
|
||||||
void output_disarmed();
|
void output_disarmed();
|
||||||
|
|
||||||
// reset_servos - free up the swash servos for maximum movement
|
// reset_servos - free up the swash servos for maximum movement
|
||||||
|
|
Loading…
Reference in New Issue