diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 8ed782ef03..3daccd0c29 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -500,6 +500,15 @@ void Plane::stabilize() plane.stabilize_roll(speed_scaler); plane.stabilize_pitch(speed_scaler); } +#endif +#if ENABLE_SCRIPTING + } else if (control_mode == &mode_auto && + mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) { + // 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); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); #endif } else { if (allow_stick_mixing && g.stick_mixing == StickMixing::FBW && control_mode != &mode_stabilize) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d924ef9107..ae073014d6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -521,6 +521,18 @@ private: float terrain_correction; } auto_state; +#if ENABLE_SCRIPTING + // support for scripting nav commands, with verify + struct { + uint16_t id; + float roll_rate_dps; + float pitch_rate_dps; + float throttle_pct; + uint32_t start_ms; + bool done; + } nav_scripting; +#endif + struct { // roll pitch yaw commanded from external controller in centidegrees Vector3l forced_rpy_cd; @@ -932,6 +944,12 @@ private: bool verify_command_callback(const AP_Mission::Mission_Command& cmd); float get_wp_radius() const; +#if ENABLE_SCRIPTING + // nav scripting support + void do_nav_script_time(const AP_Mission::Mission_Command& cmd); + bool verify_nav_script_time(const AP_Mission::Mission_Command& cmd); +#endif + // commands.cpp void set_guided_WP(void); void update_home(); @@ -1119,6 +1137,17 @@ private: float get_throttle_input(bool no_deadzone=false) const; float get_adjusted_throttle_input(bool no_deadzone=false) const; +#ifdef ENABLE_SCRIPTING + // support for NAV_SCRIPT_TIME mission command + bool nav_scripting_active(void) const; + bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override; + void nav_script_time_done(uint16_t id) override; + + // command throttle percentage and roll, pitch, yaw target + // rates. For use with scripting controllers + bool set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override; +#endif + enum Failsafe_Action { Failsafe_Action_None = 0, Failsafe_Action_RTL = 1, diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 2d92fba413..c503eb1db6 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -197,6 +197,12 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) cmd.content.do_engine_control.height_delay_cm*0.01f); break; +#if ENABLE_SCRIPTING + case MAV_CMD_NAV_SCRIPT_TIME: + do_nav_script_time(cmd); + break; +#endif + default: // unable to use the command, allow the vehicle to try the next command return false; @@ -292,6 +298,11 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret case MAV_CMD_CONDITION_DISTANCE: return verify_within_distance(); +#if ENABLE_SCRIPTING + case MAV_CMD_NAV_SCRIPT_TIME: + return verify_nav_script_time(cmd); +#endif + // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_HOME: @@ -1106,3 +1117,78 @@ float Plane::get_wp_radius() const #endif return g.waypoint_radius; } + +#if ENABLE_SCRIPTING +/* + support for scripted navigation, with verify operation for completion + */ +void Plane::do_nav_script_time(const AP_Mission::Mission_Command& cmd) +{ + nav_scripting.done = false; + nav_scripting.id++; + nav_scripting.start_ms = AP_HAL::millis(); + + // start with current roll rate, pitch rate and throttle + nav_scripting.roll_rate_dps = plane.rollController.get_pid_info().target; + nav_scripting.pitch_rate_dps = plane.pitchController.get_pid_info().target; + nav_scripting.throttle_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); +} + +/* + wait for scripting to say that the mission item is complete + */ +bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd) +{ + if (cmd.content.nav_script_time.timeout_s > 0) { + 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; + } + } + return nav_scripting.done; +} + +// check if we are in a NAV_SCRIPT_* command +bool Plane::nav_scripting_active(void) const +{ + return !nav_scripting.done && + control_mode == &mode_auto && + mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME; +} + +// support for NAV_SCRIPTING mission command +bool Plane::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) +{ + if (!nav_scripting_active()) { + // not in NAV_SCRIPT_TIME + return false; + } + const auto &c = mission.get_current_nav_cmd().content.nav_script_time; + id = nav_scripting.id; + cmd = c.command; + arg1 = c.arg1; + arg2 = c.arg2; + return true; +} + +// called when script has completed the command +void Plane::nav_script_time_done(uint16_t id) +{ + if (id == nav_scripting.id) { + nav_scripting.done = true; + } +} + +// support for NAV_SCRIPTING mission command +bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) +{ + if (!nav_scripting_active()) { + return false; + } + nav_scripting.roll_rate_dps = roll_rate_dps; + nav_scripting.pitch_rate_dps = pitch_rate_dps; + nav_scripting.throttle_pct = throttle_pct; + return true; +} +#endif // ENABLE_SCRIPTING diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index bbd9d304f0..4dc377a54f 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -86,6 +86,13 @@ void ModeAuto::update() } else { plane.calc_throttle(); } +#if ENABLE_SCRIPTING + } else if (nav_cmd_id == MAV_CMD_NAV_SCRIPT_TIME) { + // NAV_SCRIPTING has a desired roll and pitch rate and desired throttle + plane.nav_roll_cd = plane.ahrs.roll_sensor; + plane.nav_pitch_cd = plane.ahrs.pitch_sensor; + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); +#endif } else { // we are doing normal AUTO flight, the special cases // are for takeoff and landing