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

AP_MotorsHeli: Change RSC output() function into a state machine.

Browse Source
This commit is contained in:
Robert Lefebvre 2015-08-07 19:52:22 -04:00 committed by Randy Mackay
parent bf0fd3b3f2
commit 718397c772
3 changed files with 64 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -43,54 +43,64 @@ void AP_MotorsHeli_RSC::recalc_scalers()
_runup_increment = 1000.0f / (_runup_time * _loop_rate);
}
void AP_MotorsHeli_RSC::output_armed()
void AP_MotorsHeli_RSC::output(uint8_t state)
{
// ramp rotor esc output towards target
if (_speed_out < _desired_speed) {
// allow rotor out to jump to rotor's current speed
if (_speed_out < _estimated_speed) {
_speed_out = _estimated_speed;
}
switch (state){
// ramp up slowly to target
_speed_out += _ramp_increment;
if (_speed_out > _desired_speed) {
case ROTOR_CONTROL_STOP:
_estimated_speed = 0;
_speed_out = 0;
write_rsc(0);
break;
case ROTOR_CONTROL_IDLE:
case ROTOR_CONTROL_ACTIVE:
// ramp rotor esc output towards target
if (_speed_out < _desired_speed) {
// allow rotor out to jump to rotor's current speed
if (_speed_out < _estimated_speed) {
_speed_out = _estimated_speed;
}
// ramp up slowly to target
_speed_out += _ramp_increment;
if (_speed_out > _desired_speed) {
_speed_out = _desired_speed;
}
} else {
// ramping down happens instantly
_speed_out = _desired_speed;
}
} else {
// ramping down happens instantly
_speed_out = _desired_speed;
}
// ramp rotor speed estimate towards speed out
if (_estimated_speed < _speed_out) {
_estimated_speed += _runup_increment;
if (_estimated_speed > _speed_out) {
_estimated_speed = _speed_out;
}
} else {
_estimated_speed -= _runup_increment;
// ramp rotor speed estimate towards speed out
if (_estimated_speed < _speed_out) {
_estimated_speed = _speed_out;
_estimated_speed += _runup_increment;
if (_estimated_speed > _speed_out) {
_estimated_speed = _speed_out;
}
} else {
_estimated_speed -= _runup_increment;
if (_estimated_speed < _speed_out) {
_estimated_speed = _speed_out;
}
}
// set runup complete flag
if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) {
_runup_complete = true;
}
if (_runup_complete && _estimated_speed <= _critical_speed) {
_runup_complete = false;
}
// output to rsc servo
write_rsc(_speed_out);
break;
}
// set runup complete flag
if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) {
_runup_complete = true;
}
if (_runup_complete && _estimated_speed <= _critical_speed) {
_runup_complete = false;
}
// output to rsc servo
write_rsc(_speed_out);
}
void AP_MotorsHeli_RSC::output_disarmed()
{
write_rsc(0);
}
// write_rsc - outputs pwm onto output rsc channel

10
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -7,6 +7,11 @@
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
// rotor controller states
#define ROTOR_CONTROL_STOP 0
#define ROTOR_CONTROL_IDLE 1
#define ROTOR_CONTROL_ACTIVE 2
class AP_MotorsHeli_RSC {
public:
AP_MotorsHeli_RSC(RC_Channel& servo_output,
@ -45,10 +50,7 @@ public:
void recalc_scalers();
// output_armed
void output_armed();
// output_disarmed
void output_disarmed();
void output(uint8_t state);
private:

16
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -266,7 +266,7 @@ void AP_MotorsHeli_Single::output_armed_stabilizing()
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();
_tail_rotor.output(ROTOR_CONTROL_ACTIVE);
if (!_tail_rotor.is_runup_complete())
{
@ -275,7 +275,7 @@ void AP_MotorsHeli_Single::output_armed_stabilizing()
}
}
_main_rotor.output_armed();
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
@ -291,7 +291,7 @@ void AP_MotorsHeli_Single::output_armed_not_stabilizing()
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();
_tail_rotor.output(ROTOR_CONTROL_ACTIVE);
if (!_tail_rotor.is_runup_complete())
{
@ -300,7 +300,7 @@ void AP_MotorsHeli_Single::output_armed_not_stabilizing()
}
}
_main_rotor.output_armed();
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
@ -317,7 +317,7 @@ void AP_MotorsHeli_Single::output_armed_zero_throttle()
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();
_tail_rotor.output(ROTOR_CONTROL_IDLE);
if (!_tail_rotor.is_runup_complete())
{
@ -326,7 +326,7 @@ void AP_MotorsHeli_Single::output_armed_zero_throttle()
}
}
_main_rotor.output_armed();
_main_rotor.output(ROTOR_CONTROL_IDLE);
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
@ -346,10 +346,10 @@ void AP_MotorsHeli_Single::output_disarmed()
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_disarmed();
_tail_rotor.output(ROTOR_CONTROL_STOP);
}
_main_rotor.output_disarmed();
_main_rotor.output(ROTOR_CONTROL_STOP);
_heliflags.rotor_runup_complete = false;
}