diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 8ed782ef035a8..3daccd0c29d38 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 d924ef91071bd..ae073014d6484 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -521,6 +521,18 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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 2d92fba413e6f..c503eb1db6711 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 bbd9d304f0934..4dc377a54f2a2 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 diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index e24e997b80eb4..0a24d06786eaa 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -406,8 +406,10 @@ bool AP_Mission::replace_cmd(uint16_t index, const Mission_Command& cmd) /// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command bool AP_Mission::is_nav_cmd(const Mission_Command& cmd) { - // NAV commands all have ids below MAV_CMD_NAV_LAST except NAV_SET_YAW_SPEED - return (cmd.id <= MAV_CMD_NAV_LAST || cmd.id == MAV_CMD_NAV_SET_YAW_SPEED); + // NAV commands all have ids below MAV_CMD_NAV_LAST, plus some exceptions + return (cmd.id <= MAV_CMD_NAV_LAST || + cmd.id == MAV_CMD_NAV_SET_YAW_SPEED || + cmd.id == MAV_CMD_NAV_SCRIPT_TIME); } /// get_next_nav_cmd - gets next "navigation" command found at or after start_index @@ -1158,6 +1160,13 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.scripting.p3 = packet.param4; break; + case MAV_CMD_NAV_SCRIPT_TIME: + cmd.content.nav_script_time.command = packet.param1; + cmd.content.nav_script_time.timeout_s = packet.param2; + cmd.content.nav_script_time.arg1 = packet.param3; + cmd.content.nav_script_time.arg2 = packet.param4; + break; + default: // unrecognised command return MAV_MISSION_UNSUPPORTED; @@ -1610,6 +1619,13 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c packet.param4 = cmd.content.scripting.p3; break; + case MAV_CMD_NAV_SCRIPT_TIME: + packet.param1 = cmd.content.nav_script_time.command; + packet.param2 = cmd.content.nav_script_time.timeout_s; + packet.param3 = cmd.content.nav_script_time.arg1; + packet.param4 = cmd.content.nav_script_time.arg2; + break; + default: // unrecognised command return false; @@ -2328,6 +2344,8 @@ const char *AP_Mission::Mission_Command::type() const return "Jump"; case MAV_CMD_DO_GO_AROUND: return "Go Around"; + case MAV_CMD_NAV_SCRIPT_TIME: + return "NavScriptTime"; default: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 2575b5e11b7d0..04f2bf16aaacc 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -221,6 +221,14 @@ class AP_Mission float p3; }; + // Scripting NAV command (with verify) + struct PACKED nav_script_time_Command { + uint8_t command; + uint8_t timeout_s; + float arg1; + float arg2; + }; + union Content { // jump structure Jump_Command jump; @@ -291,6 +299,9 @@ class AP_Mission // do scripting scripting_Command scripting; + // nav scripting + nav_script_time_Command nav_script_time; + // location Location location{}; // Waypoint location }; diff --git a/libraries/AP_Scripting/examples/plane_aerobatics.lua b/libraries/AP_Scripting/examples/plane_aerobatics.lua new file mode 100644 index 0000000000000..d73a8a98c9c58 --- /dev/null +++ b/libraries/AP_Scripting/examples/plane_aerobatics.lua @@ -0,0 +1,169 @@ +-- perform simple aerobatic manoeuvres in AUTO mode + +local running = false + +local roll_stage = 0 + +local ROLL_TCONST = param:get('RLL2SRV_TCONST') * 0.5 +local PITCH_TCONST = param:get('PTCH2SRV_TCONST') * 0.5 +local TRIM_THROTTLE = param:get('TRIM_THROTTLE') * 1.0 + +local scr_user1_param = Parameter() +local scr_user2_param = Parameter() +local scr_user3_param = Parameter() +assert(scr_user1_param:init('SCR_USER1'), 'could not find SCR_USER1 parameter') +assert(scr_user2_param:init('SCR_USER2'), 'could not find SCR_USER2 parameter') +assert(scr_user3_param:init('SCR_USER3'), 'could not find SCR_USER3 parameter') + +local last_roll_err = 0.0 +local last_id = 0 + +-- find our rudder channel +local RUDDER_CHAN = SRV_Channels:find_channel(21) +local RUDDER_TRIM = param:get("SERVO" .. RUDDER_CHAN + 1 .. "_TRIM") +local RUDDER_REVERSED = param:get("SERVO" .. RUDDER_CHAN + 1 .. "_REVERSED") +local RUDDER_MIN = param:get("SERVO" .. RUDDER_CHAN + 1 .. "_MIN") +local RUDDER_MAX = param:get("SERVO" .. RUDDER_CHAN + 1 .. "_MAX") +local RUDDER_THROW = (RUDDER_MAX - RUDDER_MIN) * 0.5 + +-- constrain a value between limits +function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +-- a controller to target a zero roll angle, coping with inverted flight +-- output is a body frame roll rate, with convergence over time tconst in seconds +function roll_zero_controller(tconst) + local roll_deg = math.deg(ahrs:get_roll()) + local pitch_deg = math.deg(ahrs:get_pitch()) + local roll_err = 0.0 + if math.abs(pitch_deg) > 85 then + -- close to 90 we retain the last roll rate + roll_err = last_roll_err + elseif roll_deg > 90 then + roll_err = 180 - roll_deg + elseif roll_deg < -90 then + roll_err = (-180) - roll_deg + else + roll_err = -roll_deg + end + last_roll_err = roll_err + return roll_err / tconst +end + +-- a controller to target a zero pitch angle +-- output is a body frame pitch rate, with convergence over time tconst in seconds +function pitch_controller(target_pitch_deg, tconst) + local roll_deg = math.deg(ahrs:get_roll()) + local pitch_deg = math.deg(ahrs:get_pitch()) + local pitch_rate = (target_pitch_deg - pitch_deg) * math.cos(math.rad(roll_deg)) / tconst + RUDDER_GAIN = scr_user1_param:get() + local rudder = pitch_deg * RUDDER_GAIN * math.sin(math.rad(roll_deg)) / tconst + return pitch_rate, rudder +end + +-- a controller for throttle to account for pitch +function throttle_controller(tconst) + local pitch_rad = ahrs:get_pitch() + local thr_ff = scr_user3_param:get() + local throttle = TRIM_THROTTLE + math.sin(pitch_rad) * thr_ff + return constrain(throttle, 0.0, 100.0) +end + +function do_axial_roll(arg1, arg2) + -- constant roll rate axial roll + if not running then + running = true + roll_stage = 0 + gcs:send_text(0, string.format("Starting roll")) + end + local roll_rate = arg1 + local throttle = arg2 + local pitch_deg = math.deg(ahrs:get_pitch()) + local roll_deg = math.deg(ahrs:get_roll()) + if roll_stage == 0 then + if roll_deg > 45 then + roll_stage = 1 + end + elseif roll_stage == 1 then + if roll_deg > -5 and roll_deg < 5 then + running = false + -- we're done + gcs:send_text(0, string.format("Finished roll r=%.1f p=%.1f", roll_deg, pitch_deg)) + vehicle:nav_script_time_done(last_id) + roll_stage = 2 + return + end + end + if roll_stage < 2 then + target_pitch = scr_user2_param:get() + pitch_rate, rudder = pitch_controller(target_pitch, PITCH_TCONST) + vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) + if RUDDER_REVERSED == 1 then + rudder = -rudder + end + local rudder_out = math.floor(RUDDER_TRIM + RUDDER_THROW * rudder) + rudder_out = constrain(rudder_out, RUDDER_MIN, RUDDER_MAX) + SRV_Channels:set_output_pwm_chan_timeout(RUDDER_CHAN, rudder_out, 50) + end +end + +local loop_stage = 0 + +function do_loop(arg1, arg2) + -- do one loop with controllable pitch rate and throttle + if not running then + running = true + loop_stage = 0 + gcs:send_text(0, string.format("Starting loop")) + end + local pitch_rate = arg1 + local throttle = throttle_controller() + local pitch_deg = math.deg(ahrs:get_pitch()) + local roll_deg = math.deg(ahrs:get_roll()) + if loop_stage == 0 then + if pitch_deg > 60 then + loop_stage = 1 + end + elseif loop_stage == 1 then + if math.abs(roll_deg) < 90 and pitch_deg > -5 and pitch_deg < 5 then + running = false + -- we're done + gcs:send_text(0, string.format("Finished loop p=%.1f", pitch_deg)) + vehicle:nav_script_time_done(last_id) + loop_stage = 2 + return + end + end + if loop_stage < 2 then + local roll_rate = roll_zero_controller(ROLL_TCONST) + vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) + end +end + +function update() + id, cmd, arg1, arg2 = vehicle:nav_script_time() + if id then + if id ~= last_id then + -- we've started a new command + running = false + last_id = id + end + if cmd == 1 then + do_axial_roll(arg1, arg2) + elseif cmd == 2 then + do_loop(arg1, arg2) + end + else + running = false + end + return update, 10 +end + +return update() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index b11c264c6dec1..41857894687e5 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -218,6 +218,9 @@ singleton AP_Vehicle method get_wp_distance_m boolean float'Null singleton AP_Vehicle method get_wp_bearing_deg boolean float'Null singleton AP_Vehicle method get_wp_crosstrack_error_m boolean float'Null singleton AP_Vehicle method get_pan_tilt_norm boolean float'Null float'Null +singleton AP_Vehicle method nav_script_time boolean uint16_t'Null uint8_t'Null float'Null float'Null +singleton AP_Vehicle method nav_script_time_done void uint16_t'skip_check +singleton AP_Vehicle method set_target_throttle_rate_rpy boolean float -100 100 float'skip_check float'skip_check float'skip_check include AP_SerialLED/AP_SerialLED.h singleton AP_SerialLED alias serialLED diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 22a3c7a6246da..bb071980107e2 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -201,6 +201,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { 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; } + // command throttle percentage and roll, pitch, yaw target + // rates. For use with scripting controllers + virtual bool set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) { return false; } + // get target location (for use by scripting) virtual bool get_target_location(Location& target_loc) { return false; } @@ -210,6 +214,11 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // set steering and throttle (-1 to +1) (for use by scripting with Rover) virtual bool set_steering_and_throttle(float steering, float throttle) { return false; } + + // support for NAV_SCRIPT_TIME mission command + virtual bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) { return false; } + virtual void nav_script_time_done(uint16_t id) {} + #endif // ENABLE_SCRIPTING diff --git a/modules/mavlink b/modules/mavlink index 5de39eb56f855..6bcbd853aeb9a 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 5de39eb56f8559343a079e5a44259745e01e1227 +Subproject commit 6bcbd853aeb9a40c4a61ea26d9876b969cb366a2