mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-04 23:18:29 -04:00
AP_Motors:clean up whitespace in heli motors class
This commit is contained in:
parent
ea90217e09
commit
77f0fdf112
@ -231,7 +231,7 @@ void AP_MotorsHeli::output()
|
||||
} else {
|
||||
output_disarmed();
|
||||
}
|
||||
|
||||
|
||||
output_to_motors();
|
||||
|
||||
};
|
||||
|
@ -30,7 +30,7 @@
|
||||
// flybar types
|
||||
#define AP_MOTORS_HELI_NOFLYBAR 0
|
||||
|
||||
// rsc function output channels.
|
||||
// rsc function output channels.
|
||||
#define AP_MOTORS_HELI_RSC CH_8
|
||||
|
||||
class AP_HeliControls;
|
||||
@ -86,7 +86,7 @@ public:
|
||||
|
||||
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1)
|
||||
float get_rsc_setpoint() const { return _main_rotor._rsc_setpoint.get() * 0.01f; }
|
||||
|
||||
|
||||
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
|
||||
virtual void set_desired_rotor_speed(float desired_speed) = 0;
|
||||
|
||||
@ -101,10 +101,10 @@ public:
|
||||
|
||||
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
||||
virtual bool rotor_speed_above_critical() const = 0;
|
||||
|
||||
|
||||
//get rotor governor output
|
||||
virtual float get_governor_output() const = 0;
|
||||
|
||||
|
||||
//get engine throttle output
|
||||
virtual float get_control_output() const = 0;
|
||||
|
||||
@ -147,7 +147,7 @@ public:
|
||||
|
||||
// set land complete flag
|
||||
void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
|
||||
|
||||
|
||||
// enum for heli optional features
|
||||
enum class HeliOption {
|
||||
USE_LEAKY_I = (1<<0), // 1
|
||||
@ -155,7 +155,7 @@ public:
|
||||
|
||||
// use leaking integrator management scheme
|
||||
bool using_leaky_integrator() const { return heli_option(HeliOption::USE_LEAKY_I); }
|
||||
|
||||
|
||||
// var_info for holding Parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
|
||||
|
||||
// @Param: SW_H3_PHANG
|
||||
// @DisplayName: Swash 1 H3 Generic Phase Angle Comp
|
||||
// @Description: Only for H3 swashplate. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
@ -179,7 +179,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
|
||||
|
||||
// @Param: SW2_H3_PHANG
|
||||
// @DisplayName: Swash 2 H3 Generic Phase Angle Comp
|
||||
// @Description: Only for H3 swashplate. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
|
@ -64,10 +64,10 @@ public:
|
||||
|
||||
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
||||
bool rotor_speed_above_critical() const override { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
|
||||
|
||||
|
||||
// get_governor_output
|
||||
float get_governor_output() const override { return _main_rotor.get_governor_output(); }
|
||||
|
||||
|
||||
// get_control_output
|
||||
float get_control_output() const override { return _main_rotor.get_control_output(); }
|
||||
|
||||
|
@ -46,10 +46,10 @@ public:
|
||||
|
||||
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
||||
bool rotor_speed_above_critical() const override { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
|
||||
|
||||
|
||||
// get_governor_output
|
||||
float get_governor_output() const override { return _main_rotor.get_governor_output(); }
|
||||
|
||||
|
||||
// get_control_output
|
||||
float get_control_output() const override { return _main_rotor.get_control_output(); }
|
||||
|
||||
|
@ -133,7 +133,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = {
|
||||
// @Increment: 10
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("GOV_RPM", 13, AP_MotorsHeli_RSC, _governor_rpm, AP_MOTORS_HELI_RSC_GOVERNOR_RPM_DEFAULT),
|
||||
|
||||
|
||||
// Indices 14 thru 17 have been re-assigned and should not be used in the future
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
@ -155,7 +155,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = {
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("GOV_TORQUE", 19, AP_MotorsHeli_RSC, _governor_torque, AP_MOTORS_HELI_RSC_GOVERNOR_TORQUE_DEFAULT),
|
||||
|
||||
|
||||
// @Param: GOV_COMP
|
||||
// @DisplayName: Governor Torque Compensator
|
||||
// @Description: Adjusts the autothrottle governor torque compensator that determines how fast the governor will adjust the base torque reference to compensate for changes in density altitude. If Rrpm is low or high by more than 2-5 rpm, increase this setting by 1% at a time until the governor speed matches your Rrpm setting. Setting the compensator too high can result in surging and throttle "hunting". Do not make large adjustments at one time
|
||||
@ -182,8 +182,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_RSC::var_info[] = {
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("GOV_FF", 22, AP_MotorsHeli_RSC, _governor_ff, AP_MOTORS_HELI_RSC_GOVERNOR_FF_DEFAULT),
|
||||
|
||||
// @Param: GOV_CLDWN
|
||||
|
||||
// @Param: GOV_CLDWN
|
||||
// @DisplayName: AutoThrottle Cooldown Time
|
||||
// @Description: Will provide a fast idle for engine cooldown by raising the Ground Idle speed setting by 50% for the number of seconds the timer is set for. Can be set up to 120 seconds. A setting of zero disables the fast idle. This feature will only apply after the governor and autothrottle have been engaged (throttle switch on and rotor rpm at least 100% of normal speed). Upon initial startup of the engine after arming, normal Ground Idle is used. It will provide a 50% bump to Ground Idle speed for practice autorotation to ensure the engine doesn't quit. It will provide a 50% bump to Ground Idle speed to cool down a hot engine upon landing. At any time during fast idle, disarming will shut the engine down
|
||||
// @Range: 0 120
|
||||
@ -201,7 +201,7 @@ void AP_MotorsHeli_RSC::init_servo()
|
||||
// setup RSC on specified channel by default
|
||||
SRV_Channels::set_aux_channel_default(_aux_fn, _default_channel);
|
||||
|
||||
// set servo range
|
||||
// set servo range
|
||||
SRV_Channels::set_range(SRV_Channels::get_motor_function(_aux_fn), 1000);
|
||||
|
||||
}
|
||||
@ -228,7 +228,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
|
||||
if (!rpm->get_rpm(0, _rotor_rpm)) {
|
||||
_rotor_rpm = -1;
|
||||
}
|
||||
|
||||
|
||||
float dt;
|
||||
uint64_t now = AP_HAL::micros64();
|
||||
float last_control_output = _control_output;
|
||||
@ -248,7 +248,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
|
||||
|
||||
// control output forced to zero
|
||||
_control_output = 0.0f;
|
||||
|
||||
|
||||
// governor is forced to disengage status and ensure governor outputs are reset
|
||||
_autothrottle = false;
|
||||
_governor_engage = false;
|
||||
@ -297,7 +297,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
|
||||
_control_output = get_idle_output() + (_rotor_ramp_output * (throttlecurve - get_idle_output()));
|
||||
} else if (_control_mode == ROTOR_CONTROL_MODE_AUTOTHROTTLE) {
|
||||
autothrottle_run();
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
@ -431,7 +431,7 @@ float AP_MotorsHeli_RSC::calculate_throttlecurve(float collective_in)
|
||||
void AP_MotorsHeli_RSC::autothrottle_run()
|
||||
{
|
||||
float throttlecurve = calculate_throttlecurve(_collective_in);
|
||||
|
||||
|
||||
// autothrottle main power loop with governor
|
||||
if (_governor_engage && !_governor_fault) {
|
||||
// calculate droop - difference between actual and desired speed
|
||||
@ -462,7 +462,7 @@ void AP_MotorsHeli_RSC::autothrottle_run()
|
||||
}
|
||||
} else {
|
||||
_governor_fault_count = 0.0f; // reset fault count if the fault doesn't persist
|
||||
}
|
||||
}
|
||||
} else if (!_governor_engage && !_governor_fault) {
|
||||
// torque rise limiter accelerates rotor to the reference speed
|
||||
// this limits the max torque rise the governor could call for from the main power loop
|
||||
|
@ -95,7 +95,7 @@ public:
|
||||
|
||||
// get_rotor_speed - estimated rotor speed when no governor or rpm sensor is used
|
||||
float get_rotor_speed() const;
|
||||
|
||||
|
||||
// get_governor_output
|
||||
float get_governor_output() const { return _governor_output; }
|
||||
|
||||
@ -116,7 +116,7 @@ public:
|
||||
|
||||
// set_collective. collective for throttle curve calculation
|
||||
void set_collective(float collective) { _collective_in = collective; }
|
||||
|
||||
|
||||
// calculate autothrottle output
|
||||
void autothrottle_run();
|
||||
|
||||
|
@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
|
||||
|
||||
// @Param: SW_H3_PHANG
|
||||
// @DisplayName: H3 Generic Phase Angle Comp
|
||||
// @Description: Only for H3 swashplate. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
@ -422,7 +422,7 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
|
||||
if (_heliflags.inverted_flight) {
|
||||
coll_in = 1 - coll_in;
|
||||
}
|
||||
|
||||
|
||||
// rescale roll_out and pitch_out into the min and max ranges to provide linear motion
|
||||
// across the input range instead of stopping when the input hits the constrain value
|
||||
// these calculations are based on an assumption of the user specified cyclic_max
|
||||
|
@ -9,7 +9,7 @@
|
||||
#include "AP_MotorsHeli_RSC.h"
|
||||
#include "AP_MotorsHeli_Swash.h"
|
||||
|
||||
// rsc and extgyro function output channels.
|
||||
// rsc and extgyro function output channels.
|
||||
#define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7
|
||||
#define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7
|
||||
|
||||
@ -69,10 +69,10 @@ public:
|
||||
|
||||
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
||||
bool rotor_speed_above_critical() const override { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
|
||||
|
||||
|
||||
// get_governor_output
|
||||
float get_governor_output() const override { return _main_rotor.get_governor_output(); }
|
||||
|
||||
|
||||
// get_control_output
|
||||
float get_control_output() const override{ return _main_rotor.get_control_output(); }
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user