From 9d03b44cf79adfcdd3031eda2f58d2959ca78fda Mon Sep 17 00:00:00 2001 From: ChristopherOlson Date: Sat, 9 Feb 2019 17:58:42 -0600 Subject: [PATCH] Copter:Heli - governor set_rpm call independent of frame type --- ArduCopter/heli.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 6c5eb7ac59..df030758c4 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -146,7 +146,7 @@ void Copter::heli_update_rotor_speed_targets() } switch (rsc_control_mode) { case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH: - // pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom + // pass through pilot desired rotor speed from the RC if (motors->get_interlock()) { motors->set_desired_rotor_speed(rsc_control_deglitched); } else { @@ -156,9 +156,12 @@ void Copter::heli_update_rotor_speed_targets() case ROTOR_CONTROL_MODE_SPEED_SETPOINT: case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT: case ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT: - // pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode - // other than being used to create a crude estimate of rotor speed + // pass setpoint through as desired rotor speed. Needs work, this is pointless as it is + // not used by closed loop control. Being used as a catch-all for other modes regardless + // of whether or not they actually use it + // set rpm from rotor speed sensor if (motors->get_interlock()) { + motors->set_rpm(rpm_sensor.get_rpm(0)); motors->set_desired_rotor_speed(motors->get_rsc_setpoint()); }else{ motors->set_desired_rotor_speed(0.0f);