mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: Heli: remove inverted flight state
This commit is contained in:
parent
73760ea393
commit
24c843dc26
|
@ -75,9 +75,6 @@ public:
|
||||||
// set_collective_for_landing - limits collective from going too low if we know we are landed
|
// 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; }
|
void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
|
||||||
|
|
||||||
// set_inverted_flight - enables/disables inverted flight
|
|
||||||
void set_inverted_flight(bool inverted) { _heliflags.inverted_flight = inverted; }
|
|
||||||
|
|
||||||
// get_rsc_mode - gets the current rotor speed control method
|
// get_rsc_mode - gets the current rotor speed control method
|
||||||
uint8_t get_rsc_mode() const { return _main_rotor.get_control_mode(); }
|
uint8_t get_rsc_mode() const { return _main_rotor.get_control_mode(); }
|
||||||
|
|
||||||
|
@ -247,7 +244,6 @@ protected:
|
||||||
struct heliflags_type {
|
struct heliflags_type {
|
||||||
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum
|
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum
|
||||||
uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up
|
uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up
|
||||||
uint8_t inverted_flight : 1; // true for inverted flight
|
|
||||||
uint8_t init_targets_on_arming : 1; // 0 if targets were initialized, 1 if targets were not initialized after arming
|
uint8_t init_targets_on_arming : 1; // 0 if targets were initialized, 1 if targets were not initialized after arming
|
||||||
uint8_t save_rsc_mode : 1; // used to determine the rsc mode needs to be saved while disarmed
|
uint8_t save_rsc_mode : 1; // used to determine the rsc mode needs to be saved while disarmed
|
||||||
uint8_t in_autorotation : 1; // true if aircraft is in autorotation
|
uint8_t in_autorotation : 1; // true if aircraft is in autorotation
|
||||||
|
|
|
@ -415,10 +415,6 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_heliflags.inverted_flight) {
|
|
||||||
collective_in = 1 - collective_in;
|
|
||||||
}
|
|
||||||
|
|
||||||
// constrain collective input
|
// constrain collective input
|
||||||
float collective_out = collective_in;
|
float collective_out = collective_in;
|
||||||
if (collective_out <= 0.0f) {
|
if (collective_out <= 0.0f) {
|
||||||
|
|
|
@ -207,10 +207,6 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
|
||||||
|
|
||||||
float collective_range = (_collective_max - _collective_min) * 0.001f;
|
float collective_range = (_collective_max - _collective_min) * 0.001f;
|
||||||
|
|
||||||
if (_heliflags.inverted_flight) {
|
|
||||||
collective_out = 1.0f - collective_out;
|
|
||||||
}
|
|
||||||
|
|
||||||
// feed power estimate into main rotor controller
|
// feed power estimate into main rotor controller
|
||||||
_main_rotor.set_collective(fabsf(collective_out));
|
_main_rotor.set_collective(fabsf(collective_out));
|
||||||
|
|
||||||
|
|
|
@ -382,10 +382,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
|
||||||
limit.throttle_lower = false;
|
limit.throttle_lower = false;
|
||||||
limit.throttle_upper = false;
|
limit.throttle_upper = false;
|
||||||
|
|
||||||
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
|
// 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
|
// 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
|
// these calculations are based on an assumption of the user specified cyclic_max
|
||||||
|
|
Loading…
Reference in New Issue