Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues 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 84102c3e3f
commit 28318c4116
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 // ensure inputs are not passed through to servos on start-up
_servo_manual = 0; _servo_manual = 0;
// initialise swash plate // initialise Servo/PWM ranges and endpoints
init_outputs(); init_outputs();
// calculate all scalars // calculate all scalars
calculate_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 // output - sends commands to the servos
void AP_MotorsHeli::output() 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 // parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli::parameter_check() const bool AP_MotorsHeli::parameter_check() const
{ {
@ -257,3 +334,12 @@ void AP_MotorsHeli::reset_radio_passthrough()
_throttle_radio_passthrough = 500; _throttle_radio_passthrough = 500;
_yaw_radio_passthrough = 0; _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 // enable - starts allowing signals to be sent to motors
virtual void enable() = 0; virtual void enable() = 0;
// output_min - sets servos to neutral point // output_min - sets servos to neutral point with motors stopped
virtual void output_min() = 0; void output_min();
// output_test - spin a motor at the pwm value specified // 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 // motor_seq is the motor's sequence number from 1 to the number of motors on the frame
@ -172,10 +172,16 @@ public:
protected: protected:
// output - sends commands to the motors // output - sends commands to the motors
virtual void output_armed_stabilizing() = 0; void output_armed_stabilizing();
virtual void output_armed_not_stabilizing() = 0; void output_armed_not_stabilizing();
virtual void output_armed_zero_throttle() = 0; void output_armed_zero_throttle();
virtual void output_disarmed() = 0; 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 // update the throttle input filter
void update_throttle_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); 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 // update_motor_controls - sends commands to motor controllers
void AP_MotorsHeli_Single::output_min() void AP_MotorsHeli_Single::update_motor_control(uint8_t state)
{ {
// move swash to mid // Send state update to motors
move_actuators(0,0,500,0); _tail_rotor.output(state);
_main_rotor.output(state);
_main_rotor.output(ROTOR_CONTROL_STOP); // Check if both rotors are run-up, tail rotor controller always returns true if not enabled
_tail_rotor.output(ROTOR_CONTROL_STOP); _heliflags.rotor_runup_complete = ( _main_rotor.is_runup_complete() && _tail_rotor.is_runup_complete() );
// 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;
} }
// set_delta_phase_angle for setting variable phase angle compensation and force // 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 // 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); 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 // allow_arming - returns true if main rotor is spinning and it is ok to arm
bool allow_arming() const; bool allow_arming() const;
@ -128,15 +125,12 @@ public:
protected: 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 // init_outputs - initialise Servo/PWM ranges and endpoints
void init_outputs(); 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 // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void calculate_roll_pitch_collective_factors(); void calculate_roll_pitch_collective_factors();