aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: implement set_rudder_offset()

Browse Source
This commit is contained in:
Andrew Tridgell 2022-12-23 08:21:59 +11:00
parent 84944bbf53
commit 7735614634
3 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/Attitude.cpp
View File

@ -604,7 +604,12 @@ void Plane::stabilize()
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
if (yawController.rate_control_enabled()) { if (yawController.rate_control_enabled()) {
const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); float rudder = nav_scripting.rudder_offset_pct * 45;
if (nav_scripting.run_yaw_rate_controller) {
rudder += yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false);
} else {
yawController.reset_I();
}
steering_control.rudder = rudder; steering_control.rudder = rudder;
} }
#endif #endif

3
ArduPlane/Plane.h
View File

@ -522,6 +522,8 @@ private:
float throttle_pct; float throttle_pct;
uint32_t start_ms; uint32_t start_ms;
uint32_t current_ms; uint32_t current_ms;
float rudder_offset_pct;
bool run_yaw_rate_controller;
} nav_scripting; } nav_scripting;
#endif #endif
@ -1140,6 +1142,7 @@ private:
// command throttle percentage and roll, pitch, yaw target // command throttle percentage and roll, pitch, yaw target
// rates. For use with scripting controllers // rates. For use with scripting controllers
void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override; void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override;
void set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller) override;
bool nav_scripting_enable(uint8_t mode) override; bool nav_scripting_enable(uint8_t mode) override;
#endif #endif

11
ArduPlane/commands_logic.cpp
View File

@ -1184,6 +1184,8 @@ bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd)
if (now - nav_scripting.start_ms > cmd.content.nav_script_time.timeout_s*1000U) { if (now - nav_scripting.start_ms > cmd.content.nav_script_time.timeout_s*1000U) {
gcs().send_text(MAV_SEVERITY_INFO, "NavScriptTime timed out"); gcs().send_text(MAV_SEVERITY_INFO, "NavScriptTime timed out");
nav_scripting.enabled = false; nav_scripting.enabled = false;
nav_scripting.rudder_offset_pct = 0;
nav_scripting.run_yaw_rate_controller = true;
} }
} }
return !nav_scripting.enabled; return !nav_scripting.enabled;
@ -1196,6 +1198,8 @@ bool Plane::nav_scripting_active(void)
// set_target_throttle_rate_rpy has not been called from script in last 1000ms // set_target_throttle_rate_rpy has not been called from script in last 1000ms
nav_scripting.enabled = false; nav_scripting.enabled = false;
nav_scripting.current_ms = 0; nav_scripting.current_ms = 0;
nav_scripting.rudder_offset_pct = 0;
nav_scripting.run_yaw_rate_controller = true;
gcs().send_text(MAV_SEVERITY_INFO, "NavScript time out"); gcs().send_text(MAV_SEVERITY_INFO, "NavScript time out");
} }
if (control_mode == &mode_auto && if (control_mode == &mode_auto &&
@ -1240,6 +1244,13 @@ void Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps
nav_scripting.current_ms = AP_HAL::millis(); nav_scripting.current_ms = AP_HAL::millis();
} }
// support for rudder offset override in aerobatic scripting
void Plane::set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller)
{
nav_scripting.rudder_offset_pct = rudder_pct;
nav_scripting.run_yaw_rate_controller = run_yaw_rate_controller;
}
// enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands // enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands
bool Plane::nav_scripting_enable(uint8_t mode) bool Plane::nav_scripting_enable(uint8_t mode)
{ {