AC_AttitudeControl: Heli: invert throttle in inverted flight, move state down to heli

This commit is contained in:
Iampete1 2024-02-29 23:30:54 +00:00 committed by Randy Mackay
parent 24c843dc26
commit 576ee75669
3 changed files with 8 additions and 3 deletions

View File

@ -566,9 +566,6 @@ protected:
void control_monitor_filter_pid(float value, float &rms_P); void control_monitor_filter_pid(float value, float &rms_P);
void control_monitor_update(void); void control_monitor_update(void);
// true in inverted flight mode
bool _inverted_flight;
public: public:
// log a CTRL message // log a CTRL message
void control_monitor_log(void) const; void control_monitor_log(void) const;

View File

@ -538,6 +538,11 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
{ {
_throttle_in = throttle_in; _throttle_in = throttle_in;
update_althold_lean_angle_max(throttle_in); update_althold_lean_angle_max(throttle_in);
if (_inverted_flight) {
throttle_in = 1.0 - throttle_in;
}
_motors.set_throttle_filter_cutoff(filter_cutoff); _motors.set_throttle_filter_cutoff(filter_cutoff);
_motors.set_throttle(throttle_in); _motors.set_throttle(throttle_in);
// Clear angle_boost for logging purposes // Clear angle_boost for logging purposes

View File

@ -101,6 +101,9 @@ private:
uint8_t tail_passthrough : 1; // 1 if we should pass through pilots yaw input to tail uint8_t tail_passthrough : 1; // 1 if we should pass through pilots yaw input to tail
} _flags_heli; } _flags_heli;
// true in inverted flight mode
bool _inverted_flight;
// Integrate vehicle rate into _att_error_rot_vec_rad // Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors(); void integrate_bf_rate_error_to_angle_errors();