mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 05:58:30 -04:00
Copter:Heli - governor set_rpm call independent of frame type
This commit is contained in:
parent
2f7d198196
commit
9d03b44cf7
@ -146,7 +146,7 @@ void Copter::heli_update_rotor_speed_targets()
|
|||||||
}
|
}
|
||||||
switch (rsc_control_mode) {
|
switch (rsc_control_mode) {
|
||||||
case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
|
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()) {
|
if (motors->get_interlock()) {
|
||||||
motors->set_desired_rotor_speed(rsc_control_deglitched);
|
motors->set_desired_rotor_speed(rsc_control_deglitched);
|
||||||
} else {
|
} else {
|
||||||
@ -156,9 +156,12 @@ void Copter::heli_update_rotor_speed_targets()
|
|||||||
case ROTOR_CONTROL_MODE_SPEED_SETPOINT:
|
case ROTOR_CONTROL_MODE_SPEED_SETPOINT:
|
||||||
case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT:
|
case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT:
|
||||||
case ROTOR_CONTROL_MODE_CLOSED_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
|
// pass setpoint through as desired rotor speed. Needs work, this is pointless as it is
|
||||||
// other than being used to create a crude estimate of rotor speed
|
// 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()) {
|
if (motors->get_interlock()) {
|
||||||
|
motors->set_rpm(rpm_sensor.get_rpm(0));
|
||||||
motors->set_desired_rotor_speed(motors->get_rsc_setpoint());
|
motors->set_desired_rotor_speed(motors->get_rsc_setpoint());
|
||||||
}else{
|
}else{
|
||||||
motors->set_desired_rotor_speed(0.0f);
|
motors->set_desired_rotor_speed(0.0f);
|
||||||
|
Loading…
Reference in New Issue
Block a user