aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_MotorsHeli: Move Output functions into parent class

Browse Source
This commit is contained in:
Robert Lefebvre 2015-08-12 13:22:39 -04:00 committed by Randy Mackay
parent e23e57cc16
commit ec8afbec53
4 changed files with 109 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -168,13 +168,28 @@ void AP_MotorsHeli::Init()
// ensure inputs are not passed through to servos on start-up
_servo_manual = 0;
// initialise swash plate
// initialise Servo/PWM ranges and endpoints
init_outputs();
// calculate all scalars
calculate_scalars();
}
// output_min - sets servos to neutral point with motors stopped
void AP_MotorsHeli::output_min()
{
// move swash to mid
move_actuators(0,0,500,0);
update_motor_control(ROTOR_CONTROL_STOP);
// override limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = false;
}
// output - sends commands to the servos
void AP_MotorsHeli::output()
{
@ -194,6 +209,68 @@ void AP_MotorsHeli::output()
}
};
// sends commands to the motors
void AP_MotorsHeli::output_armed_stabilizing()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_manual == 1) {
reset_flight_controls();
}
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_ACTIVE);
}
void AP_MotorsHeli::output_armed_not_stabilizing()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_manual == 1) {
reset_flight_controls();
}
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_ACTIVE);
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli::output_armed_zero_throttle()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_manual == 1) {
reset_flight_controls();
}
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_IDLE);
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::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;
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_STOP);
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli::parameter_check() const
{
@ -257,3 +334,12 @@ void AP_MotorsHeli::reset_radio_passthrough()
_throttle_radio_passthrough = 500;
_yaw_radio_passthrough = 0;
}
// reset_flight_controls - resets all controls and scalars to flight status
void AP_MotorsHeli::reset_flight_controls()
{
reset_radio_passthrough();
_servo_manual = 0;
init_outputs();
calculate_scalars();
}

18
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -89,8 +89,8 @@ public:
// enable - starts allowing signals to be sent to motors
virtual void enable() = 0;
// output_min - sets servos to neutral point
virtual void output_min() = 0;
// output_min - sets servos to neutral point with motors stopped
void output_min();
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
@ -172,10 +172,16 @@ public:
protected:
// output - sends commands to the motors
virtual void output_armed_stabilizing() = 0;
virtual void output_armed_not_stabilizing() = 0;
virtual void output_armed_zero_throttle() = 0;
virtual void output_disarmed() = 0;
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_armed_zero_throttle();
void output_disarmed();
// update_motor_controls - sends commands to motor controllers
virtual void update_motor_control(uint8_t state) = 0;
// reset_flight_controls - resets all controls and scalars to flight status
void reset_flight_controls();
// update the throttle input filter
void update_throttle_filter();

130
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -317,131 +317,15 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask()
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
}
// output_min - sets servos to neutral point
void AP_MotorsHeli_Single::output_min()
// update_motor_controls - sends commands to motor controllers
void AP_MotorsHeli_Single::update_motor_control(uint8_t state)
{
// move swash to mid
move_actuators(0,0,500,0);
// Send state update to motors
_tail_rotor.output(state);
_main_rotor.output(state);
_main_rotor.output(ROTOR_CONTROL_STOP);
_tail_rotor.output(ROTOR_CONTROL_STOP);
// override limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = false;
}
// sends commands to the motors
void AP_MotorsHeli_Single::output_armed_stabilizing()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_manual == 1) {
reset_radio_passthrough();
_servo_manual = 0;
init_outputs();
calculate_scalars();
}
move_actuators(_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(ROTOR_CONTROL_ACTIVE);
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
_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 and reinitialize servos
if (_servo_manual == 1) {
reset_radio_passthrough();
_servo_manual = 0;
init_outputs();
calculate_scalars();
}
move_actuators(_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(ROTOR_CONTROL_ACTIVE);
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
_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 and reinitialize servos
if (_servo_manual == 1) {
reset_radio_passthrough();
_servo_manual = 0;
init_outputs();
calculate_scalars();
}
move_actuators(_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(ROTOR_CONTROL_IDLE);
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output(ROTOR_CONTROL_IDLE);
_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;
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
move_actuators(_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(ROTOR_CONTROL_STOP);
}
_main_rotor.output(ROTOR_CONTROL_STOP);
_heliflags.rotor_runup_complete = false;
// Check if both rotors are run-up, tail rotor controller always returns true if not enabled
_heliflags.rotor_runup_complete = ( _main_rotor.is_runup_complete() && _tail_rotor.is_runup_complete() );
}
// set_delta_phase_angle for setting variable phase angle compensation and force

12
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -78,9 +78,6 @@ public:
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void output_test(uint8_t motor_seq, int16_t pwm);
// output_min - sets servos to neutral point
void output_min();
// allow_arming - returns true if main rotor is spinning and it is ok to arm
bool allow_arming() const;
@ -128,15 +125,12 @@ public:
protected:
// output - sends commands to the motors
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_armed_zero_throttle();
void output_disarmed();
// init_outputs - initialise Servo/PWM ranges and endpoints
void init_outputs();
// update_motor_controls - sends commands to motor controllers
void update_motor_control(uint8_t state);
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void calculate_roll_pitch_collective_factors();