Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Tradheli: fix yaw attitude error corrections while rotors turning on the ground #28690

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
+106 −42
Open

Tradheli: fix yaw attitude error corrections while rotors turning on the ground #28690

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
626927f
AP_Motors: support for making use of heading correction on ground sel…
bnsgeyer Nov 20, 2024
a0df9fe
Copter: support for making use of heading error correction selectable
bnsgeyer Nov 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 9 additions & 6 deletions ArduCopter/mode_acro_heli.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,26 +45,29 @@ void ModeAcro_Heli::run()
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->reset_target_and_rate(false);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changing this from false to true here will remove all counter talk on the tail rotor. If you can be still spinning down in this phase of flight this will let the tail move more than it would do now.

Low risk but something to consider.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@lthall What do you mean by counter talk? I think the issue here is that when disarmed, there won't be any target reset that would cause an issue with trying to fly disarmed. I know this sounds crazy but I believe that this was intentional by my predecessor to allow users to fly in acro if the aircraft disarms unexpectedly, like due to code bug. I think that this should true so that the attitude errors are reset. However I may make this change in a separate PR to make this more visible as an intentional change.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@Ferruccio1984 What do you think of having the attitude error correction (pitch and roll) removed for the disarmed state in acro. This means that when you move the pitch and roll stick that the swashplate will not react while disarmed which is what you probably saw in your flight today. I think it is a good idea because it would keep users from moving the swashplate while disarmed and then being surprised on spool up if the swash was not centered. I think the idea of ever having to fly in a disarmed state is very low risk.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I personally like that we can do "control surface" checks when on the ground. Making sure that all of the servos and swash are moving and in the correct direction is a good pre-flight check.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@MattKear i think my comment was not completely accurate. In acro currently, if you move the sticks, the commanded attitude is tracked and you could end up having 90 deg commanded while on the ground and disarmed. With ths new implementation, you should still see movement of rhe swash because you are commanding rates and it will always center itself when you center the stick (zero rates) where it does not do that currently. So you can stick check controls on the ground while disarmed. Again this is only in acro. Stabilize should not change except for yaw and that should react to rate requests also.

attitude_control->reset_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// If aircraft is landed, set target heading to current and reset the integrator
// IF aircraft is landed and not using leaky integrator
// OR initializing targets on arming and using leaky integrator,
// THEN set target heading to current and reset the integrator
// Otherwise motors could be at ground idle for practice autorotation
if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
attitude_control->reset_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_DOWN:
if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_yaw_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}

if (!motors->has_flybar()){
Expand Down
11 changes: 8 additions & 3 deletions ArduCopter/mode_althold.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,15 +50,20 @@ void ModeAltHold::run()

case AltHoldModeState::MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
Expand Down
34 changes: 23 additions & 11 deletions ArduCopter/mode_drift.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,26 +91,38 @@ void ModeDrift::run()
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
break;

case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms_smoothly();
break;

case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
// For helicopters, IF aircraft is landed and not using leaky integrator
// OR initializing targets on arming and using leaky integrator,
// THEN set target heading to current and reset the integrator
// Otherwise motors could be at ground idle for practice autorotation
// If a multirotor then it is always done
if (!copter.is_tradheli() || (motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;

case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
// clear landing flag above zero throttle
if (!copter.is_tradheli()) {
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
} else {
if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_yaw_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
}
}
break;
}

Expand Down
9 changes: 7 additions & 2 deletions ArduCopter/mode_flowhold.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -288,10 +288,15 @@ void ModeFlowHold::run()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
Expand Down
11 changes: 9 additions & 2 deletions ArduCopter/mode_loiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,10 +132,17 @@ void ModeLoiter::run()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why wouldn't we want to reset_rate on landed ground idle?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This provides some damping to the tail if it gets pushed by a gust or something but will stay close to zero as long as we keep the integrator zero.

attitude_control->reset_rate_controller_I_terms_smoothly();
loiter_nav->init_target();
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a fairly significant change in behaviour by removing the fall through. We would at least want to keep the loiter_nav->init_target()?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yep, we need to add the loiter_nav->init_target();

I think we can move this to the bottom:

attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@lthall I think I need to move all of the lines in the pretakeoff state up to the landed ground idle since I removed the fall through.


case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
loiter_nav->init_target();
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
Expand Down
15 changes: 12 additions & 3 deletions ArduCopter/mode_poshold.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ void ModePosHold::run()

case AltHoldModeState::MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
Expand All @@ -116,13 +116,22 @@ void ModePosHold::run()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
attitude_control->reset_yaw_target_and_rate();
init_wind_comp_estimate();
FALLTHROUGH;

// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero

Expand Down
11 changes: 8 additions & 3 deletions ArduCopter/mode_sport.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,15 +76,20 @@ void ModeSport::run()

case AltHoldModeState::MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
Expand Down
15 changes: 9 additions & 6 deletions ArduCopter/mode_stabilize_heli.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,26 +52,29 @@ void ModeStabilize_Heli::run()
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// If aircraft is landed, set target heading to current and reset the integrator
// IF aircraft is landed and not using leaky integrator
// OR initializing targets on arming and using leaky integrator,
// THEN set target heading to current and reset the integrator
// Otherwise motors could be at ground idle for practice autorotation
if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_DOWN:
if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}

// call attitude controller
Expand Down
11 changes: 9 additions & 2 deletions ArduCopter/mode_zigzag.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -356,10 +356,17 @@ void ModeZigZag::manual_control()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
loiter_nav->init_target();
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
loiter_nav->init_target();
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_Motors/AP_MotorsHeli.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,9 +108,9 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @Param: OPTIONS
// @DisplayName: Heli_Options
// @Description: Bitmask of heli options. Bit 0 changes how the pitch, roll, and yaw axis integrator term is managed for low speed and takeoff/landing. In AC 4.0 and earlier, scheme uses a leaky integrator for ground speeds less than 5 m/s and won't let the steady state integrator build above ILMI. The integrator is allowed to build to the ILMI value when it is landed. The other integrator management scheme bases integrator limiting on takeoff and landing. Whenever the aircraft is landed the integrator is set to zero. When the aicraft is airborne, the integrator is only limited by IMAX.
// @Bitmask: 0:Use Leaky I
// @Bitmask: 0:Use Leaky I, 1:Heading Error Correction On-Ground
// @User: Standard
AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I),
AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I + (uint8_t)HeliOption::USE_HDG_CORRECTION),

// @Param: COL_ANG_MIN
// @DisplayName: Collective Blade Pitch Angle Minimum
Expand Down
8 changes: 6 additions & 2 deletions libraries/AP_Motors/AP_MotorsHeli.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,11 +138,15 @@ class AP_MotorsHeli : public AP_Motors {

// enum for heli optional features
enum class HeliOption {
USE_LEAKY_I = (1<<0), // 1
USE_LEAKY_I = (1<<0),
USE_HDG_CORRECTION = (1<<1),
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I do not want to feature creep this PR, so I am just saying this as a recommendation for future work. I think it is weird that we ask AP_Motors whether we should be using leaky I and now this heading correction. I understand that it is a hang over because most of the heli specific code lives in motors, but in the future I would like to move the heli option param out of motors and up to the vehicle(copter) level. This adds to my point below as to why I would rather we didn't make it a virtual function.

};

// use leaking integrator management scheme
bool using_leaky_integrator() const { return heli_option(HeliOption::USE_LEAKY_I); }
bool using_leaky_integrator() const override { return heli_option(HeliOption::USE_LEAKY_I); }

// use heading error correction
bool using_hdg_error_correction() const override { return heli_option(HeliOption::USE_HDG_CORRECTION); }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need this as an option? What is the use case that users will be trying to drive yaw whilst on the ground?


// Run arming checks
bool arming_checks(size_t buflen, char *buffer) const override;
Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_Motors/AP_Motors_Class.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,6 +258,10 @@ class AP_Motors {
// This function required for tradheli. Tradheli initializes targets when going from unarmed to armed state.
// This function is overriden in motors_heli class. Always true for multicopters.
virtual bool init_targets_on_arming() const { return true; }
// use leaking integrator management scheme is always false for multirotors
virtual bool using_leaky_integrator() const { return false; }
// use heading error correction which is always true for multirotors
virtual bool using_hdg_error_correction() const { return true; }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I do not think we should not be making these virtuals in AP_Motors_Class. We are taking something that is a heli-only option and making visible in the multi-copter code. Instead we should keep it in AP_MotorsHeli and use defines for the option, as we do else where in the code.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We have been trying to remove the hash defines in the shared code and instead make it flexible enough to cover all the hovering aircraft.

The aspirational goal is to not have any Heli specific code in the shared code base and instead have optional behaviour that is defined by the vehicle.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok, that is fair enough. I can certainly get on board with that!
If that is the case, I would still like to see the heli_options param move up to copter level. I think it is weird that we ask motors about leaky I and heading error correction, both of which effect the rate/attitude controllers which are a "layer" above motors.


// returns true if the configured PWM type is digital and should have fixed endpoints
bool is_digital_pwm_type() const;
Expand Down
Loading