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

Plane: cleanup abort of NAV_SCRIPT

Browse Source 
use the same enable flag for tricks and auto NAV_SCRIPT_TIME and
ensure we disable if the script stops controlling
This commit is contained in:
Andrew Tridgell 2022-10-27 09:50:10 +11:00
parent 06edc9a139
commit 500ac9b99c
7 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -537,7 +537,7 @@ void Plane::update_alt()
update_flight_stage();
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
if (nav_scripting_active()) {
// don't call TECS while we are in a trick
return;
}

6
ArduPlane/Attitude.cpp
View File

@ -508,8 +508,7 @@ void Plane::stabilize()
if (control_mode == &mode_training) {
stabilize_training(speed_scaler);
#if AP_SCRIPTING_ENABLED
} else if ((control_mode == &mode_auto &&
mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) || (nav_scripting.enabled && nav_scripting.current_ms > 0)) {
} else if (nav_scripting_active()) {
// scripting is in control of roll and pitch rates and throttle
const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler);
const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
@ -519,9 +518,6 @@ void Plane::stabilize()
const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false);
steering_control.rudder = rudder;
}
if (AP_HAL::millis() - nav_scripting.current_ms > 50) { //set_target_throttle_rate_rpy has not been called from script in last 50ms
nav_scripting.current_ms = 0;
}
#endif
} else if (control_mode == &mode_acro) {
stabilize_acro(speed_scaler);

4
ArduPlane/Plane.h
View File

@ -518,9 +518,7 @@ private:
float throttle_pct;
uint32_t start_ms;
uint32_t current_ms;
bool done;
} nav_scripting;
#endif
struct {
@ -1127,7 +1125,7 @@ private:
#if AP_SCRIPTING_ENABLED
// support for NAV_SCRIPT_TIME mission command
bool nav_scripting_active(void) const;
bool nav_scripting_active(void);
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
void nav_script_time_done(uint16_t id) override;

26
ArduPlane/commands_logic.cpp
View File

@ -1160,10 +1160,10 @@ float Plane::get_wp_radius() const
*/
void Plane::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
{
nav_scripting.done = false;
nav_scripting.enabled = true;
nav_scripting.id++;
nav_scripting.start_ms = AP_HAL::millis();
nav_scripting.current_ms = 0;
nav_scripting.current_ms = nav_scripting.start_ms;
// start with current roll rate, pitch rate and throttle
nav_scripting.roll_rate_dps = plane.rollController.get_pid_info().target;
@ -1181,18 +1181,25 @@ bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd)
const uint32_t now = AP_HAL::millis();
if (now - nav_scripting.start_ms > cmd.content.nav_script_time.timeout_s*1000U) {
gcs().send_text(MAV_SEVERITY_INFO, "NavScriptTime timed out");
nav_scripting.done = true;
nav_scripting.enabled = false;
}
}
return nav_scripting.done;
return !nav_scripting.enabled;
}
// check if we are in a NAV_SCRIPT_* command
bool Plane::nav_scripting_active(void) const
bool Plane::nav_scripting_active(void)
{
return !nav_scripting.done &&
control_mode == &mode_auto &&
mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME;
if (AP_HAL::millis() - nav_scripting.current_ms > 200) {
// set_target_throttle_rate_rpy has not been called from script in last 200ms
nav_scripting.enabled = false;
nav_scripting.current_ms = 0;
}
if (control_mode == &mode_auto &&
mission.get_current_nav_cmd().id != MAV_CMD_NAV_SCRIPT_TIME) {
nav_scripting.enabled = false;
}
return nav_scripting.enabled;
}
// support for NAV_SCRIPTING mission command
@ -1216,7 +1223,7 @@ bool Plane::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2
void Plane::nav_script_time_done(uint16_t id)
{
if (id == nav_scripting.id) {
nav_scripting.done = true;
nav_scripting.enabled = false;
}
}
@ -1228,6 +1235,7 @@ void Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps
nav_scripting.yaw_rate_dps = constrain_float(yaw_rate_dps, -g.acro_yaw_rate, g.acro_yaw_rate);
nav_scripting.throttle_pct = constrain_float(throttle_pct, aparm.throttle_min, aparm.throttle_max);
nav_scripting.current_ms = AP_HAL::millis();
nav_scripting.enabled = true;
}
// enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands

4
ArduPlane/mode_cruise.cpp
View File

@ -29,7 +29,7 @@ void ModeCruise::update()
}
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
if (plane.nav_scripting_active()) {
// while a trick is running unlock heading and zero altitude offset
locked_heading = false;
lock_timer_ms = 0;
@ -53,7 +53,7 @@ void ModeCruise::update()
void ModeCruise::navigate()
{
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
if (plane.nav_scripting_active()) {
// don't try to navigate while running trick
return;
}

4
ArduPlane/mode_loiter.cpp
View File

@ -29,7 +29,7 @@ void ModeLoiter::update()
}
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
if (plane.nav_scripting_active()) {
// while a trick is running we reset altitude
plane.set_target_altitude_current();
plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE);
@ -100,7 +100,7 @@ void ModeLoiter::navigate()
}
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting.enabled) {
if (plane.nav_scripting_active()) {
// don't try to navigate while running trick
return;
}

2
ArduPlane/servos.cpp
View File

@ -562,7 +562,7 @@ void Plane::set_servos_controlled(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
}
#if AP_SCRIPTING_ENABLED
} else if (plane.nav_scripting.current_ms > 0 && nav_scripting.enabled) {
} else if (nav_scripting_active()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
#endif
} else if (control_mode == &mode_stabilize ||