Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: Tradheli- fix inverted mode collective handling

Browse Source
This commit is contained in:
bnsgeyer 2024-02-29 23:09:14 -05:00 committed by Randy Mackay
parent 52bffc4b4d
commit d103eebf91
2 changed files with 31 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -539,12 +539,27 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
_throttle_in = throttle_in;
update_althold_lean_angle_max(throttle_in);
if (_inverted_flight) {
throttle_in = 1.0 - throttle_in;
if (_transition_count > 0) {
_transition_count -= 1;
} else {
_transition_count = 0;
}
float throttle_out = 0.0f;
if (_transition_count > 0) {
if ((_ahrs.roll_sensor >= -3000 && _ahrs.roll_sensor <= 3000) || _ahrs.roll_sensor >= 15000 || _ahrs.roll_sensor <= -15000) {
throttle_out = (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid()) / cosf(radians(_ahrs.roll_sensor * 0.01f)) + ((AP_MotorsHeli&)_motors).get_coll_mid();
} else if ((_ahrs.roll_sensor > 3000 && _ahrs.roll_sensor < 15000) || (_ahrs.roll_sensor > -15000 && _ahrs.roll_sensor < -3000)) {
float scale_factor = cosf(radians(_ahrs.roll_sensor * 0.01f)) / cosf(radians(30.0f));
throttle_out = scale_factor * (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid())/ cosf(radians(30.0f)) + ((AP_MotorsHeli&)_motors).get_coll_mid();
}
} else if (_inverted_flight) {
throttle_out = 1.0f - throttle_in;
} else {
throttle_out = throttle_in;
}
_motors.set_throttle_filter_cutoff(filter_cutoff);
_motors.set_throttle(throttle_in);
_motors.set_throttle(throttle_out);
// Clear angle_boost for logging purposes
_angle_boost = 0.0f;
}
@ -575,3 +590,12 @@ void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate)
_pid_rate_yaw.set_notch_sample_rate(sample_rate);
#endif
}
// enable/disable inverted flight
void AC_AttitudeControl_Heli::set_inverted_flight(bool inverted)
{
if (_inverted_flight != inverted) {
_transition_count = AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME * AP::scheduler().get_filtered_loop_rate_hz();
}
_inverted_flight = inverted;
}

6
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -28,6 +28,7 @@
#define AC_ATTITUDE_HELI_RATE_Y_FF_FILTER 20.0f
#define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT 300
#define AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD ToRad(30.0f)
#define AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME 3.0f
class AC_AttitudeControl_Heli : public AC_AttitudeControl {
public:
@ -82,9 +83,7 @@ public:
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;
// enable/disable inverted flight
void set_inverted_flight(bool inverted) override {
_inverted_flight = inverted;
}
void set_inverted_flight(bool inverted) override;
// set the PID notch sample rates
void set_notch_sample_rate(float sample_rate) override;
@ -103,6 +102,7 @@ private:
// true in inverted flight mode
bool _inverted_flight;
uint16_t _transition_count;
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors();