diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 59e65dc343e73..9e7b962680822 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -963,6 +963,18 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const return g2.flight_options & flight_option; } +#if AP_SCRIPTING_ENABLED +// this implements the virtual function defined in AP_Vehicle for Plane to set the desired airspeed in m/s +bool Plane::set_desired_airspeed(float airspeed_new) +{ + if (control_mode->can_override_throttle()) { + plane.new_airspeed_cm = constrain_float(airspeed_new, aparm.airspeed_min, aparm.airspeed_max) * 100.0f; + return true; + } + return false; +} +#endif + #if AC_PRECLAND_ENABLED void Plane::precland_update(void) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 613cc25a9b248..ff12e27e5d1ab 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1045,6 +1045,9 @@ class Plane : public AP_Vehicle { void update_flight_stage(); void set_flight_stage(AP_FixedWing::FlightStage fs); bool flight_option_enabled(FlightOptions flight_option) const; +#if AP_SCRIPTING_ENABLED + bool set_desired_airspeed(float airspeed_new) override; // different from set_desired_speed() in rover and copter +#endif // navigation.cpp void loiter_angle_reset(void); diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index cfe306813b31c..64a12691ad920 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -118,6 +118,10 @@ class Mode // true if the mode sets the vehicle destination, which controls // whether control input is ignored with STICK_MIXING=0 virtual bool does_auto_throttle() const { return false; } + + // flags if the throttle can be overriden by setting new_airspeed_cm + // eventually it should return does_auto_throttle() but for now this is not correct + virtual bool can_override_throttle() const { return false; } // true if the mode supports autotuning (via switch for modes other // that AUTOTUNE itself @@ -225,7 +229,9 @@ class ModeAuto : public Mode bool does_auto_navigation() const override; bool does_auto_throttle() const override; - + + bool can_override_throttle() const override { return does_auto_throttle(); } + bool mode_allows_autotuning() const override { return true; } bool is_landing() const override; @@ -301,6 +307,8 @@ class ModeGuided : public Mode bool does_auto_throttle() const override { return true; } + bool can_override_throttle() const override { return does_auto_throttle(); } + // handle a guided target request from GCS bool handle_guided_request(Location target_loc) override; diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 7171b241c5296..29122d5ddf346 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2427,6 +2427,11 @@ function vehicle:nav_scripting_enable(param1) end ---@return boolean function vehicle:set_desired_speed(param1) end +-- Set autopilot desired airspeed (Plane) +---@param airspeed_new number -- new airspeed in m/s +---@return boolean -- true if successful +function vehicle:set_desired_airspeed(airspeed_new) end + -- desc ---@param param1 number ---@param param2 number diff --git a/libraries/AP_Scripting/examples/plane-set-airspeed.lua b/libraries/AP_Scripting/examples/plane-set-airspeed.lua new file mode 100644 index 0000000000000..6c8a0caea6aee --- /dev/null +++ b/libraries/AP_Scripting/examples/plane-set-airspeed.lua @@ -0,0 +1,46 @@ +--[[ + demonstrate setting the airspeed for a plane in fixed wing mode +--]] +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local AUX_FN = 300 +local last_sw = -1 + +--[[ + If the user changes the position of the scripting switch choose either + LOW: AIRSPEED_MIN + MID: AIRSPEED_CRUISE + HIGH: AIRSPEED_MAX +--]] +local function set_desired_speed() + local sw_current = rc:get_aux_cached(AUX_FN) + if not sw_current then + -- ignore untill the user flips the switch + return + end + if sw_current ~= last_sw then + last_sw = sw_current + if sw_current == 0 then + new_airspeed = Parameter("AIRSPEED_MIN"):get() + gcs:send_text(MAV_SEVERITY.INFO, string.format("Speed set to AIRSPEED_MIN %f", new_airspeed)) + elseif sw_current == 1 then + new_airspeed = Parameter("AIRSPEED_CRUISE"):get() + gcs:send_text(MAV_SEVERITY.INFO, string.format("Speed set to AIRSPEED_CRUISE %f", new_airspeed)) + else + new_airspeed = Parameter("AIRSPEED_MAX"):get() + gcs:send_text(MAV_SEVERITY.INFO, string.format("Speed set to AIRSPEED_MAX %f", new_airspeed)) + end + if not vehicle:set_desired_airspeed(new_airspeed) then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("Failed to set airspeed")) + end + end +end + +local function update() + if arming:is_armed() then + set_desired_speed() + end + return update, 1000 +end + +return update, 1000 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 40910361296c5..5da3ed779b188 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -339,6 +339,7 @@ singleton AP_Vehicle method set_target_throttle_rate_rpy void float -100 100 flo singleton AP_Vehicle method set_rudder_offset void float'skip_check boolean singleton AP_Vehicle method set_desired_turn_rate_and_speed boolean float'skip_check float'skip_check singleton AP_Vehicle method set_desired_speed boolean float'skip_check +singleton AP_Vehicle method set_desired_airspeed boolean float'skip_check singleton AP_Vehicle method nav_scripting_enable boolean uint8_t'skip_check singleton AP_Vehicle method set_velocity_match boolean Vector2f singleton AP_Vehicle method set_land_descent_rate boolean float'skip_check diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 8b22c9395d73d..0697819478ab7 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -184,6 +184,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool set_target_velocity_NED(const Vector3f& vel_ned) { return false; } virtual bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; } virtual bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) { return false; } + virtual bool set_guided_radius_and_direction(float radius, bool direction_is_ccw) { return false; } // command throttle percentage and roll, pitch, yaw target // rates. For use with scripting controllers @@ -209,6 +210,9 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // set auto mode speed in meters/sec (for use by scripting with Copter/Rover) virtual bool set_desired_speed(float speed) { return false; } + // set auto mode airspeed in meters/sec (for use by scripting with Plane) + virtual bool set_desired_airspeed(float airspeed_new) { return false; } + // support for NAV_SCRIPT_TIME mission command virtual bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) { return false; } virtual void nav_script_time_done(uint16_t id) {}