diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp
index 94b08030c9..b37fe011c3 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli.cpp
@@ -200,7 +200,7 @@ bool AP_MotorsHeli::parameter_check() const
}
// returns false if RSC Mode is not set to a valid control mode
- if (_rsc_mode <= AP_MOTORS_HELI_RSC_MODE_NONE || _rsc_mode > AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
+ if (_rsc_mode <= AP_MOTORS_HELI_RSC_MODE_DISABLED || _rsc_mode > AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
return false;
}
diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h
index c143d8f557..f9cb8055b1 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.h
+++ b/libraries/AP_Motors/AP_MotorsHeli.h
@@ -37,7 +37,7 @@
#define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0
// main rotor speed control types (ch8 out)
-#define AP_MOTORS_HELI_RSC_MODE_NONE 0 // not a valid RSC Mode
+#define AP_MOTORS_HELI_RSC_MODE_DISABLED 0 // not a valid RSC Mode
#define AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH 1 // main rotor ESC is connected to RC8 (out), pilot desired rotor speed provided by CH8 input
#define AP_MOTORS_HELI_RSC_MODE_SETPOINT 2 // main rotor ESC is connected to RC8 (out), desired speed is held in RSC_SETPOINT parameter
@@ -120,7 +120,7 @@ public:
// set_collective_for_landing - limits collective from going too low if we know we are landed
void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
- // get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_NONE, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
+ // get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
uint8_t get_rsc_mode() const { return _rsc_mode; }
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1000)
@@ -132,8 +132,8 @@ public:
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
virtual int16_t get_desired_rotor_speed() const = 0;
- // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
- virtual int16_t get_estimated_rotor_speed() const = 0;
+ // get_main_rotor_speed - gets estimated or measured main rotor speed
+ virtual int16_t get_main_rotor_speed() const = 0;
// return true if the main rotor is up to speed
bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; }
diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
index 20f3de3715..552a6acb4e 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
@@ -36,7 +36,7 @@ void AP_MotorsHeli_RSC::recalc_scalers()
_ramp_time = 1;
}
- _ramp_increment = 1000.0f / (_ramp_time * _loop_rate);
+ _ramp_increment = 1.0f / (_ramp_time * _loop_rate);
// recalculate rotor runup increment
if (_runup_time <= 0 ) {
@@ -47,7 +47,7 @@ void AP_MotorsHeli_RSC::recalc_scalers()
_runup_time = _ramp_time;
}
- _runup_increment = 1000.0f / (_runup_time * _loop_rate);
+ _runup_increment = 1.0f / (_runup_time * _loop_rate);
}
// output - update value to send to ESC/Servo
@@ -55,89 +55,113 @@ void AP_MotorsHeli_RSC::output(uint8_t state)
{
switch (state){
case ROTOR_CONTROL_STOP:
- _control_speed = 0; // ramp input to zero
- _control_out = 0; // force ramp output to zero
- _estimated_speed = 0; // force speed estimate to zero
+ // set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
+ update_rotor_ramp(0.0f);
+
+ // control output forced to zero
+ _control_speed = 0;
break;
case ROTOR_CONTROL_IDLE:
- _control_speed = _idle_speed; // set control speed to idle speed
- if (_control_out < _idle_speed){
- _control_out = _idle_speed; // if control output is less than idle speed, force ramp function to jump to idle speed
- }
+ // set rotor ramp to decrease speed to zero
+ update_rotor_ramp(0.0f);
+
+ // set rotor control speed to idle speed parameter, this happens instantly and ignore ramping
+ _control_speed = _idle_speed;
break;
case ROTOR_CONTROL_ACTIVE:
- _control_speed = _desired_speed; // set control speed to desired speed
+ // set main rotor ramp to increase to full speed
+ update_rotor_ramp(1.0f);
+
+ if ((_control_mode == ROTOR_CONTROL_MODE_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SETPOINT)) {
+ // set control rotor speed to ramp slewed value between idle and desired speed
+ _control_speed = _idle_speed + (_rotor_ramp_output * (_desired_speed - _idle_speed));
+ }
break;
}
- // run speed ramp function to slew output smoothly
- speed_ramp(_control_speed);
-
- // update rotor speed estimate
- update_speed_estimate();
+ // update rotor speed run-up estimate
+ update_rotor_runup();
// output to rsc servo
- write_rsc(_control_out);
+ write_rsc(_control_speed);
}
-// speed_ramp - ramps speed towards target, result put in _control_out
-void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target)
+// update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
+void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input)
{
- // range check speed_target
- speed_target = constrain_int16(speed_target,0,1000);
-
// ramp output upwards towards target
- if (_control_out < speed_target) {
+ if (_rotor_ramp_output < rotor_ramp_input) {
// allow control output to jump to estimated speed
- if (_control_out < _estimated_speed) {
- _control_out = _estimated_speed;
+ if (_rotor_ramp_output < _rotor_runup_output) {
+ _rotor_ramp_output = _rotor_runup_output;
}
// ramp up slowly to target
- _control_out += _ramp_increment;
- if (_control_out > speed_target) {
- _control_out = speed_target;
+ _rotor_ramp_output += _ramp_increment;
+ if (_rotor_ramp_output > rotor_ramp_input) {
+ _rotor_ramp_output = rotor_ramp_input;
}
}else{
// ramping down happens instantly
- _control_out = speed_target;
+ _rotor_ramp_output = rotor_ramp_input;
}
-
-
}
-// update_speed_estimate - function to estimate speed
-void AP_MotorsHeli_RSC::update_speed_estimate()
+// update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
+void AP_MotorsHeli_RSC::update_rotor_runup()
{
// ramp speed estimate towards control out
- if (_estimated_speed < _control_out) {
- _estimated_speed += _runup_increment;
- if (_estimated_speed > _control_out) {
- _estimated_speed = _control_out;
+ if (_rotor_runup_output < _rotor_ramp_output) {
+ _rotor_runup_output += _runup_increment;
+ if (_rotor_runup_output > _rotor_ramp_output) {
+ _rotor_runup_output = _rotor_ramp_output;
}
}else{
- _estimated_speed -= _runup_increment;
- if (_estimated_speed < _control_out) {
- _estimated_speed = _control_out;
+ _rotor_runup_output -= _runup_increment;
+ if (_rotor_runup_output < _rotor_ramp_output) {
+ _rotor_runup_output = _rotor_ramp_output;
}
}
// update run-up complete flag
- if (!_runup_complete && _control_out > _idle_speed && _estimated_speed >= _control_out) {
+
+ // if control mode is disabled, then run-up complete always returns true
+ if ( _control_mode == ROTOR_CONTROL_MODE_DISABLED ){
+ _runup_complete = true;
+ return;
+ }
+
+ // if rotor ramp and runup are both at full speed, then run-up has been completed
+ if (!_runup_complete && (_rotor_ramp_output >= 1.0f) && (_rotor_runup_output >= 1.0f)) {
_runup_complete = true;
}
- if (_runup_complete && _estimated_speed <= _critical_speed) {
+ // if rotor speed is less than critical speed, then run-up is not complete
+ // this will prevent the case where the target rotor speed is less than critical speed
+ if (_runup_complete && (get_rotor_speed() <= _critical_speed)) {
_runup_complete = false;
}
}
+// get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value
+int16_t AP_MotorsHeli_RSC::get_rotor_speed() const
+{
+ // if no actual measured rotor speed is available, estimate speed based on rotor runup scalar.
+ return (_rotor_runup_output * _max_speed);
+}
+
// write_rsc - outputs pwm onto output rsc channel
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli_RSC::write_rsc(int16_t servo_out)
{
- _servo_output.servo_out = servo_out;
- _servo_output.calc_pwm();
+ if (_control_mode == ROTOR_CONTROL_MODE_DISABLED){
+ // do not do servo output to avoid conflicting with other output on the channel
+ // ToDo: We should probably use RC_Channel_Aux to avoid this problem
+ return;
+ } else {
+ _servo_output.servo_out = servo_out;
+ _servo_output.calc_pwm();
- hal.rcout->write(_servo_output_channel, _servo_output.radio_out);
+ hal.rcout->write(_servo_output_channel, _servo_output.radio_out);
+ }
}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h
index f06c4eddf2..fab3e6fc23 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h
@@ -12,6 +12,11 @@
#define ROTOR_CONTROL_IDLE 1
#define ROTOR_CONTROL_ACTIVE 2
+// rotor control modes
+#define ROTOR_CONTROL_MODE_DISABLED 0
+#define ROTOR_CONTROL_MODE_PASSTHROUGH 1
+#define ROTOR_CONTROL_MODE_SETPOINT 2
+
class AP_MotorsHeli_RSC {
public:
AP_MotorsHeli_RSC(RC_Channel& servo_output,
@@ -25,6 +30,9 @@ public:
// init_servo - servo initialization on start-up
void init_servo();
+ // set_control_mode - sets control mode
+ void set_control_mode(int8_t mode) { _control_mode = mode; }
+
// set_critical_speed
void set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; }
@@ -43,8 +51,8 @@ public:
// get_control_speed
int16_t get_control_speed() const { return _control_speed; }
- // get_estimated_speed
- int16_t get_estimated_speed() const { return _estimated_speed; }
+ // get_rotor_speed - return estimated or measured rotor speed
+ int16_t get_rotor_speed() const;
// is_runup_complete
bool is_runup_complete() const { return _runup_complete; }
@@ -69,23 +77,25 @@ private:
float _loop_rate; // main loop rate
// internal variables
+ int8_t _control_mode = 0; // motor control mode, Passthrough or Setpoint
int16_t _critical_speed = 0; // rotor speed below which flight is not possible
int16_t _idle_speed = 0; // motor output idle speed
+ int16_t _max_speed = 1000; // rotor maximum speed. Placeholder value until we have measured speed input (ToDo)
int16_t _desired_speed = 0; // latest desired rotor speed from pilot
int16_t _control_speed = 0; // latest logic controlled rotor speed
- float _control_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
- float _estimated_speed = 0; // estimated speed of the main rotor (0~1000)
- float _ramp_increment = 0; // the amount we can increase the rotor output during each 100hz iteration
+ float _rotor_ramp_output = 0; // scalar used to ramp rotor speed between _rsc_idle and full speed (0.0-1.0f)
+ float _rotor_runup_output = 0; // scalar used to store status of rotor run-up time (0.0-1.0f)
+ float _ramp_increment = 0; // the amount to increase/decrease the rotor ramp scalar during each iteration
int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed
int8_t _runup_time = 0; // time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
- float _runup_increment = 0; // the amount we can increase the rotor's estimated speed during each 100hz iteration
+ float _runup_increment = 0; // the amount to increase/decrease the rotor run-up scalar during each iteration
bool _runup_complete = false; // flag for determining if runup is complete
- // speed_ramp - ramps speed towards target, result put in _control_out
- void speed_ramp(int16_t rotor_target);
+ // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
+ void update_rotor_ramp(float rotor_ramp_input);
- // update_speed_estimate - function to estimate speed
- void update_speed_estimate();
+ // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
+ void update_rotor_runup();
// write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000
void write_rsc(int16_t servo_out);
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
index 241d851a70..7d79bd095e 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
@@ -195,7 +195,7 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm)
bool AP_MotorsHeli_Single::allow_arming() const
{
// returns false if main rotor speed is not zero
- if (_main_rotor.get_estimated_speed() > 0) {
+ if (_main_rotor.get_rotor_speed() > 0) {
return false;
}
@@ -209,40 +209,39 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed)
{
_main_rotor.set_desired_speed(desired_speed);
- if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
+ if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_desired_speed(_direct_drive_tailspeed);
} else {
_tail_rotor.set_desired_speed(0);
}
}
-
-// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
+// recalc_scalers - recalculates various scalers used.
void AP_MotorsHeli_Single::recalc_scalers()
{
-
+ _main_rotor.set_control_mode(_rsc_mode);
_main_rotor.set_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
_main_rotor.set_critical_speed(_rsc_critical);
_main_rotor.set_idle_speed(_rsc_idle);
_main_rotor.recalc_scalers();
- if (_rsc_mode != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
- _tail_rotor.set_ramp_time(0);
- _tail_rotor.set_runup_time(0);
- _tail_rotor.set_critical_speed(0);
- _tail_rotor.set_idle_speed(0);
- } else {
+ if (_rsc_mode == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
+ _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT);
_tail_rotor.set_ramp_time(_rsc_ramp_time);
_tail_rotor.set_runup_time(_rsc_runup_time);
_tail_rotor.set_critical_speed(_rsc_critical);
_tail_rotor.set_idle_speed(_rsc_idle);
+ } else {
+ _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_DISABLED);
+ _tail_rotor.set_ramp_time(0);
+ _tail_rotor.set_runup_time(0);
+ _tail_rotor.set_critical_speed(0);
+ _tail_rotor.set_idle_speed(0);
}
-
_tail_rotor.recalc_scalers();
}
-
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsHeli_Single::get_motor_mask()
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h
index 57d1daa3aa..645ddecc7a 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h
@@ -87,16 +87,16 @@ public:
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(int16_t desired_speed);
- // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
- int16_t get_estimated_rotor_speed() const { return _main_rotor.get_estimated_speed(); }
+ // get_main_rotor_speed - gets estimated or measured main rotor speed
+ int16_t get_main_rotor_speed() const { return _main_rotor.get_rotor_speed(); }
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); }
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
- bool rotor_speed_above_critical() const { return _main_rotor.get_estimated_speed() > _main_rotor.get_critical_speed(); }
+ bool rotor_speed_above_critical() const { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
- // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
+ // recalc_scalers - recalculates various scalers used.
void recalc_scalers();
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)