diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 4c24723e8d1f5..4589d778a9b61 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -381,6 +381,18 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false); return true; } + +bool Copter::set_thrust(float thrust) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + Quaternion q{0,0,0,0}; + mode_guided.set_angle(q, Vector3f{}, thrust, true); + return true; +} #endif #if MODE_CIRCLE_ENABLED == ENABLED diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c8614c4fafb53..b00aa155191d3 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -665,6 +665,7 @@ class Copter : public AP_Vehicle { bool set_target_velocity_NED(const Vector3f& vel_ned) override; 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 relative_yaw) override; 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) override; + bool set_thrust(float thrust) override; #endif #if MODE_CIRCLE_ENABLED == ENABLED bool get_circle_radius(float &radius_m) override; diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 4a70c50c22220..12808e2807867 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -850,6 +850,19 @@ bool Plane::set_land_descent_rate(float descent_rate) return false; } +// VTOL thrust override from scripting in GUIDED mode +bool Plane::set_thrust(float thrust) +{ +#if HAL_QUADPLANE_ENABLED + if (quadplane.available() && control_mode == &plane.mode_guided) { + quadplane.thrust_override.start_ms = AP_HAL::millis(); + quadplane.thrust_override.thrust = thrust; + return true; + } +#endif + return false; +} + #endif // AP_SCRIPTING_ENABLED // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 39bae83e5530d..d006e0abaac16 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -297,6 +297,9 @@ class Plane : public AP_Vehicle { Mode *control_mode = &mode_initializing; Mode *previous_mode = &mode_initializing; + // were we in VTOL control when we left the previous mode? + bool previous_in_vtol; + // time of last mode change uint32_t last_mode_change_ms; @@ -951,6 +954,9 @@ class Plane : public AP_Vehicle { // 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); + + // override thrust from scripting + bool set_thrust(float thrust) override; #endif // commands.cpp diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 2581a2a2d674a..1663d0875d768 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -199,8 +199,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: GUIDED_MODE // @DisplayName: Enable VTOL in GUIDED mode - // @Description: This enables use of VTOL in guided mode. When enabled the aircraft will switch to VTOL flight when the guided destination is reached and hover at the destination. - // @Values: 0:Disabled,1:Enabled + // @Description: This enables use of VTOL in guided mode. When enabled the aircraft will switch to VTOL flight when the guided destination is reached and hhold position while hovering at the destination. If set to 2 then the vehicle will continue hovering if GUIDED mode is entered from a VTOL mode and is hovering. For QRTL and AUTO mode landings it will continue hovering if in the VTOL control part of the landing sequence. + // @Values: 0:Disabled,1:Enabled,2:VTOLIfInVTOL // @User: Standard AP_GROUPINFO("GUIDED_MODE", 40, QuadPlane, guided_mode, 0), @@ -1005,6 +1005,20 @@ void QuadPlane::run_z_controller(void) last_pidz_init_ms = now; } last_pidz_active_ms = now; + +#if AP_SCRIPTING_ENABLED + if (thrust_override.start_ms != 0) { + // require updates every 50ms at least for thrust control + if (now - thrust_override.start_ms > 50) { + thrust_override.start_ms = 0; + pos_control->init_z_controller(); + } else { + attitude_control->set_throttle_out(thrust_override.thrust, true, 0); + return; + } + } +#endif // AP_SCRIPTING_ENABLED + pos_control->update_z_controller(); } @@ -3735,7 +3749,11 @@ bool QuadPlane::guided_mode_enabled(void) // loiter turns is a fixed wing only operation return false; } - return guided_mode != 0; + if (guided_mode == GUIDED_MODE::G_IF_Q_MODE) { + // if previously in a Q mode then do VTOL guided + return plane.previous_in_vtol; + } + return guided_mode != GUIDED_MODE::G_OFF; } /* diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 887f3bc9cf4ce..e94a3736dfb5c 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -363,8 +363,14 @@ class QuadPlane QRTL_ALWAYS, }; + enum class GUIDED_MODE : uint8_t { + G_OFF = 0, + G_ON = 1, + G_IF_Q_MODE = 2 + }; + // control if a VTOL GUIDED will be used - AP_Int8 guided_mode; + AP_Enum guided_mode; // control ESC throttle calibration AP_Int8 esc_calibration; @@ -598,6 +604,13 @@ class QuadPlane // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set bool delay_arming; +#if AP_SCRIPTING_ENABLED + struct { + uint32_t start_ms; + float thrust; + } thrust_override; +#endif + /* return true if current mission item is a vtol takeoff */ diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index e0ae140fd31f4..f6043b78f5e62 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -278,6 +278,13 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason) Mode &old_previous_mode = *previous_mode; Mode &old_mode = *control_mode; +#if HAL_QUADPLANE_ENABLED + if (quadplane.available()) { + // remember if we were in a VTOL mode for GUIDED + previous_in_vtol = quadplane.in_vtol_mode() || quadplane.in_vtol_takeoff(); + } +#endif + // update control_mode assuming success // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode previous_mode = control_mode; diff --git a/libraries/AP_Scripting/applets/VTOL-expo.lua b/libraries/AP_Scripting/applets/VTOL-expo.lua new file mode 100644 index 0000000000000..3417ac65d4527 --- /dev/null +++ b/libraries/AP_Scripting/applets/VTOL-expo.lua @@ -0,0 +1,562 @@ +--[[ + estimation of MOT_THST_EXPO for VTOL thrust linearisation + + should be used in copter or quadplane GUIDED mode +--]] + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local PARAM_TABLE_KEY = 11 +local PARAM_TABLE_PREFIX = "EXPO_" + +local UPDATE_RATE_HZ = 200 + +local GRAVITY_MSS = 9.80665 + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +-- setup expo specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 12), 'could not add param table') + +--[[ + // @Param: EXPO_ENABLE + // @DisplayName: Expo enable + // @Description: Enable expo tuning system + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local EXPO_ENABLE = bind_add_param('ENABLE', 1, 0) + +--[[ + // @Param: EXPO_RC_FUNC + // @DisplayName: Expo RC function + // @Description: RCn_OPTION number to use to control tuning stop/start/save + // @User: Standard +--]] +local EXPO_RC_FUNC = bind_add_param('RC_FUNC', 2, 300) + +--[[ + // @Param: EXPO_PULSE_TIME + // @DisplayName: Expo pulse time + // @Description: Pulse length for expo measurement + // @Range: 0.5 4 + // @Units: s + // @User: Standard +--]] +local EXPO_PULSE_TIME = bind_add_param('PULSE_TIME', 3, 1.5) + +--[[ + // @Param: EXPO_PULSE_THST + // @DisplayName: Expo pulse thrust amplitude + // @Description: Pulse thrust amplitude for expo measurement. This is a proportion of hover throttle. A value of 0.5 means the pulse will go from 0.5*hover_throttle to 1.5*hover_throttle + // @Range: 0.1 0.6 + // @User: Standard +--]] +local EXPO_PULSE_THST = bind_add_param('PULSE_THST', 4, 0.25) + +--[[ + // @Param: EXPO_LIMIT_MIN + // @DisplayName: Expo min value + // @Description: Minimum value we will tune to + // @Range: -1 1 + // @User: Standard +--]] +local EXPO_LIMIT_MIN = bind_add_param('LIMIT_MIN', 5, 0.0) + +--[[ + // @Param: EXPO_LIMIT_MAX + // @DisplayName: Expo max value + // @Description: Maximum value we will tune to + // @Range: -1 1 + // @User: Standard +--]] +local EXPO_LIMIT_MAX = bind_add_param('LIMIT_MAX', 6, 0.9) + +--[[ + // @Param: EXPO_PULSE_SLEW + // @DisplayName: Expo pulse thrust slew time + // @Description: The time over which the thrust slews to the target thrust + // @Range: 0.0 0.5 + // @Units: s + // @User: Standard +--]] +local EXPO_PULSE_SLEW = bind_add_param('PULSE_SLEW', 7, 0.05) + +--[[ + // @Param: EXPO_ACC_FLTHZ + // @DisplayName: Expo acceleration filtering + // @Description: The cutoff frequency for accel filtering + // @Range: 1 20 + // @Units: Hz + // @User: Standard +--]] +local EXPO_ACC_FLTHZ = bind_add_param('ACC_FLTHZ', 8, 3) + +--[[ + // @Param: EXPO_THR_FLTHZ + // @DisplayName: Expo throttle filtering + // @Description: The cutoff frequency for throttle filtering + // @Range: 0.5 10 + // @Units: Hz + // @User: Standard +--]] +local EXPO_THR_FLTHZ = bind_add_param('THR_FLTHZ', 9, 2) + +--[[ + // @Param: EXPO_CONV_THR + // @DisplayName: Expo convergence threshold + // @Description: The acceleration threshold for convergence + // @Range: 0.01 0.3 + // @Units: m/s/s + // @User: Standard +--]] +local EXPO_CONV_THR = bind_add_param('CONV_THR', 10, 0.15) + +-- get time in seconds since boot +function get_time() + return millis():tofloat() * 0.001 +end + +local is_quadplane = false + +-- we must be in GUIDED mode +local MODE_COPTER_GUIDED = 4 +local MODE_PLANE_GUIDED = 15 + +local MOT_THST_EXPO +local SPIN_MIN +local SPIN_MAX +local GUID_TIMEOUT +local expected_mode = MODE_COPTER_GUIDED + +if param:get("Q_M_THST_EXPO") then + is_quadplane = true + expected_mode = MODE_PLANE_GUIDED + MOT_THST_EXPO = Parameter('Q_M_THST_EXPO') + SPIN_MIN = Parameter('Q_M_SPIN_MIN') + SPIN_MAX = Parameter('Q_M_SPIN_MAX') +else + MOT_THST_EXPO = Parameter('MOT_THST_EXPO') + SPIN_MIN = Parameter('MOT_SPIN_MIN') + SPIN_MAX = Parameter('MOT_SPIN_MAX') +end + +INS_HNTCH_ENABLE = Parameter('INS_HNTCH_ENABLE') +INS_HNTCH_MODE = Parameter('INS_HNTCH_MODE') +INS_HNTCH_REF = Parameter('INS_HNTCH_REF') +INS_HNTC2_ENABLE = Parameter('INS_HNTC2_ENABLE') +INS_HNTC2_MODE = Parameter('INS_HNTC2_MODE') +INS_HNTC2_REF = Parameter('INS_HNTC2_REF') + +if not is_quadplane then + GUID_TIMEOUT = Parameter('GUID_TIMEOUT') +end + +local PHASE = { NONE=0, PULSE=1, WAIT=2 } +local SWITCH = { STOP=0, RUN=1, SAVE=2 } + +local test_phase = PHASE.NONE +local phase_start_time = nil +local phase_start_throttle = nil +local accel_min = nil +local accel_max = nil +local last_warning = 0 +local saved_pos = nil +local saved_yaw_deg = nil +local unchanged_count = 0 +local pulse_count = 0 +local filtered_throttle = 0 +local filtered_accel = 0 +local saved_params = {} + +local UNCHANGED_LIMIT = 5 + +-- this controls how quickly we converge +local SCALING_RATIO = 0.3 + +-- set vehicle thrust as 0 to 1 +function set_thrust(thrust) + vehicle:set_thrust(thrust) +end + +-- get earth frame z accel, offsetting for gravity +function get_accel_ofs_z() + return ahrs:get_accel_ef():z() + GRAVITY_MSS +end + +-- constrain a value between limits +local function constrain(v, vmin, vmax) + return math.max(math.min(v, vmax), vmin) +end + +--[[ + change a parameter, remembering the old value for reversion +--]] +local function change_param(param, value) + if saved_params[param] == nil then + -- save old value + saved_params[param] = param:get() + end + param:set(value) +end + +-- save parameter changed +function save_params() + for p, _ in pairs(saved_params) do + p:set_and_save(p:get()) + end + if saved_params[MOT_THST_EXPO] ~= nil then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Expo: saved at %.2f", MOT_THST_EXPO:get())) + end + if saved_params[INS_HNTCH_REF] ~= nil then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Expo: saved INS_HNTCH_REF at %.2f", INS_HNTCH_REF:get())) + end + if saved_params[INS_HNTC2_REF] ~= nil then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Expo: saved INS_HNTC2_REF at %.2f", INS_HNTC2_REF:get())) + end + -- clear saved parameters + saved_params = {} +end + +-- get the thrust delta we will apply in a pulse +function get_thrust_delta(throttle) + -- don't let us get over 90% throttle or below 2% throttle + local pulse_thrust = constrain(EXPO_PULSE_THST:get(),0.1,0.6) + return math.min(pulse_thrust*throttle, (1.0 - throttle)-0.1) +end + +local function lowpass_filter(filtered, value, dt, cutoff_hz) + local rc = 1.0/(math.pi*2*math.max(cutoff_hz, 0.01)) + local alpha = 1.0 - dt/(dt+rc) + return (1.0 - alpha) * value + alpha * filtered +end + +--[[ + get the thrust curve we use for testing expo. + The curve is designed to give us maximum acceleration at zero + vertical velocity, which means minimum drag. It is a double slew + limited square wave + returns thrust and a boolean to indicate if the pulse is complete +--]] +local function get_thrust_curve(t) + local pulse_time = EXPO_PULSE_TIME:get() + local pulse_slew = EXPO_PULSE_SLEW:get() + if pulse_slew*5 > pulse_time then + pulse_slew = pulse_time * 0.2 + end + local pulse_hold = (pulse_time - 4*pulse_slew) * 0.5 + if t < pulse_slew then + return (t / pulse_slew), false + end + if t < pulse_slew + pulse_hold then + return 1.0, false + end + if t < 3*pulse_slew + pulse_hold then + local t1 = t - (pulse_slew + pulse_hold) + return 1.0 - 2*t1/(pulse_slew*2), false + end + if t < 4*pulse_slew + 2.5*pulse_hold then + return -1.0, false + end + local t2 = t - (pulse_slew*4 + pulse_hold*2.5) + return -1.0 + t2 / pulse_slew, t2 >= pulse_slew +end + +--[[ + forward thrust curve +--]] +local function apply_thrust_curve(thrust, expo) + expo = constrain(expo, -1.0, 1.0) + if math.abs(expo) < 0.01 then + -- zero expo means linear, avoid floating point exception for small values + return thrust + end + local throttle_ratio = ((expo - 1.0) + math.sqrt((1.0 - expo) * (1.0 - expo) + 4.0 * expo * thrust)) / (2.0 * expo) + return constrain(throttle_ratio, 0.0, 1.0) +end + +--[[ + inverse thrust curve +--]] +local function remove_thrust_curve(throttle, expo) + -- apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0 + expo = constrain(expo, -1.0, 1.0) + if math.abs(expo) < 0.01 then + -- zero expo means linear, avoid floating point exception for small values + return throttle + end + local thrust = (throttle * (2.0 * expo)) - (expo - 1.0) + thrust = (thrust * thrust) - ((1.0 - expo) * (1.0 - expo)) + thrust = thrust / (4.0 * expo) + return constrain(thrust, 0.0, 1.0) +end + +--[[ + converts desired thrust to linearized actuator output in a range of 0~1 +--]] +local function thrust_to_actuator(thrust_in, expo) + thrust_in = constrain(thrust_in, 0.0, 1.0) + return SPIN_MIN:get() + (SPIN_MAX:get() - SPIN_MIN:get()) * apply_thrust_curve(thrust_in, expo) +end + +--[[ + converts actuator to thrust +--]] +local function actuator_to_thrust(actuator, expo) + actuator = (actuator - SPIN_MIN:get()) / (SPIN_MAX:get() - SPIN_MIN:get()) + return constrain(remove_thrust_curve(actuator, expo), 0.0, 1.0) +end + +--[[ + adjust refernce for expo in throttle notch +--]] +local function adjust_reference(ref, old_expo, new_expo) + local actuator = thrust_to_actuator(ref, old_expo) + return actuator_to_thrust(actuator, new_expo) +end + +--[[ + adjust throttle based notch filter references +--]] +local function adjust_notch_filters(old_expo, new_expo) + if INS_HNTCH_ENABLE:get() == 1 and INS_HNTCH_MODE:get() == 1 then + local old_ref = INS_HNTCH_REF:get() + change_param(INS_HNTCH_REF, adjust_reference(old_ref, old_expo, new_expo)) + end + if INS_HNTC2_ENABLE:get() == 1 and INS_HNTC2_MODE:get() == 1 then + local old_ref = INS_HNTC2_REF:get() + change_param(INS_HNTC2_REF, adjust_reference(old_ref, old_expo, new_expo)) + end +end + +--[[ + adjust the expo for the given thrust change and accel_min, accel_max +--]] +local function adjust_expo(thrust_change) + if pulse_count % 2 == 1 then + -- only adjust on even pulses + return + end + if accel_max == nil or accel_min == nil then + gcs:send_text(MAV_SEVERITY.INFO, string.format("Expo: did not reach vel target")) + return + end + local expected_accel = thrust_change * GRAVITY_MSS + --[[ if we get acceleration deviations higher than expected then + the expo is too low. If we get deviations lower than expected + then expo is too high + --]] + local accel_err = (accel_max - accel_min) * 0.5 - expected_accel + local scale = 1 + constrain(accel_err,-1,1)*SCALING_RATIO + local old_expo = MOT_THST_EXPO:get() + if math.abs(old_expo) < 0.03 then + if old_expo < 0 then + old_expo = -0.03 + else + old_expo = 0.03 + end + end + local new_expo = constrain(old_expo * scale, EXPO_LIMIT_MIN:get(), EXPO_LIMIT_MAX:get()) + gcs:send_text(MAV_SEVERITY.INFO, string.format("Expo: %.2f accel=(%.2f,%.2f) err=%.3f", new_expo, accel_min, accel_max, accel_err)) + change_param(MOT_THST_EXPO, new_expo) + + -- possibly adjust notch filters + adjust_notch_filters(old_expo, new_expo) + + accel_min = nil + accel_max = nil + if math.abs(accel_err) <= EXPO_CONV_THR:get() then + unchanged_count = (unchanged_count + 1) + if unchanged_count >= UNCHANGED_LIMIT then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Expo: finished tune %.2f", MOT_THST_EXPO:get())) + end + else + unchanged_count = 0 + end +end + +-- revert any expo change +function revert() + for p, value in pairs(saved_params) do + p:set(value) + end + if saved_params[MOT_THST_EXPO] ~= nil then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("Expo: reverted to %.2f", MOT_THST_EXPO:get())) + end + saved_params = {} + saved_pos = nil + saved_yaw_deg = nil + accel_min = nil + accel_max = nil + unchanged_count = 0 + pulse_count = 0 +end + +-- main update function +function update() + if EXPO_ENABLE:get() == 0 then + return + end + + if is_quadplane then + --[[ force Q_GUIDED_MODE==2 when using the EXPO script to ensure + users are not surprised by a fixed wing transition when + entering guided mode from QLOITER or another Q mode + --]] + param:set("Q_GUIDED_MODE", 2) + end + + local sw_pos = rc:get_aux_cached(EXPO_RC_FUNC:get()) + if not sw_pos then + return + end + + local now = get_time() + + if sw_pos == SWITCH.RUN and (not arming:is_armed() or not vehicle:get_likely_flying()) and get_time() > last_warning + 5 then + if now - last_warning > 3 then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("Expo: Must be flying to tune")) + last_warning = get_time() + end + revert() + return + end + if sw_pos == SWITCH.RUN and vehicle:get_mode() ~= expected_mode then + if now - last_warning > 3 then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("Expo: Must be in GUIDED mode to tune")) + last_warning = get_time() + end + revert() + return + end + if not arming:is_armed() or not vehicle:get_likely_flying() then + -- abort, revert parameters + revert() + return + end + + -- we are armed and flying, update filtered throttle and accel + filtered_throttle = lowpass_filter(filtered_throttle, motors:get_throttle(), 1.0/UPDATE_RATE_HZ, EXPO_THR_FLTHZ:get()) + filtered_accel = lowpass_filter(filtered_accel, get_accel_ofs_z(), 1.0/UPDATE_RATE_HZ, EXPO_ACC_FLTHZ:get()) + + if sw_pos == SWITCH.STOP then + -- not tuning + revert() + return + end + + if sw_pos == SWITCH.SAVE then + -- save all params + if saved_params[MOT_THST_EXPO] ~= nil then + save_params() + revert() + end + end + if sw_pos ~= SWITCH.RUN then + return + end + + if unchanged_count >= UNCHANGED_LIMIT then + -- we've finished the tune + if not is_quadplane and saved_pos ~= nil then + vehicle:set_target_pos_NED(saved_pos, true, saved_yaw_deg, false, 0, false, false) + end + return + end + + if saved_pos == nil then + -- we are just starting tuning, get current values + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Expo: starting tune with expo=%.2f", MOT_THST_EXPO:get())) + saved_pos = ahrs:get_relative_position_NED_origin() + saved_yaw_deg = math.deg(ahrs:get_yaw()) + phase_start_time = now + + -- initialise filtered throttle and accel + filtered_throttle = motors:get_throttle() + filtered_accel = get_accel_ofs_z() + end + + local pos_err = (ahrs:get_relative_position_NED_origin() - saved_pos):length() + local vel_err = ahrs:get_velocity_NED():length() + local accel = filtered_accel + + logger.write('VEXP','t,Accel,AccelF,PErr,VErr,Phase,PC,Thr,ThrF,Expo', 'fffffBIfff', + now-phase_start_time, + get_accel_ofs_z(), accel, pos_err, vel_err, test_phase, pulse_count, + motors:get_throttle(), filtered_throttle, + MOT_THST_EXPO:get()) + + local quiescent = (vel_err < 0.2 and pos_err < 0.2 and math.abs(filtered_accel) < 0.1) + + if test_phase == PHASE.NONE and not quiescent then + if not is_quadplane then + vehicle:set_target_pos_NED(saved_pos, true, saved_yaw_deg, false, 0, false, false) + end + end + + if test_phase == PHASE.NONE and quiescent then + phase_start_time = now + phase_start_throttle = filtered_throttle + if not is_quadplane then + GUID_TIMEOUT:set(0.1) + end + set_thrust(phase_start_throttle) + test_phase = PHASE.PULSE + end + + if test_phase == PHASE.PULSE then + local thrust_delta = get_thrust_delta(phase_start_throttle) + local t = now - phase_start_time + local direction = (pulse_count % 2) * 2 - 1 + local thrust_curve, finished = get_thrust_curve(t) + if finished then + set_thrust(phase_start_throttle) + test_phase = PHASE.WAIT + phase_start_time = now + pulse_count = pulse_count + 1 + adjust_expo(get_thrust_delta(phase_start_throttle)/phase_start_throttle) + return + end + set_thrust(phase_start_throttle + thrust_delta * direction * thrust_curve) + + if now - phase_start_time >= EXPO_PULSE_TIME:get()*0.5 then + local velD = ahrs:get_velocity_NED():z() + if accel_min == nil and direction == -1 and velD < 0 then + accel_min = accel + end + if accel_max == nil and direction == 1 and velD > 0 then + accel_max = accel + end + end + end + + if test_phase == PHASE.WAIT then + if not is_quadplane then + vehicle:set_target_pos_NED(saved_pos, true, saved_yaw_deg, false, 0, false, false) + end + if quiescent then + test_phase = PHASE.NONE + end + end + +end + +gcs:send_text(MAV_SEVERITY.INFO, "Loaded VTOL-expo tuning") + +-- wrapper around update(). This calls update() at 100Hz, +function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(MAV_SEVERITY.EMERGENCY, "Internal Error: " .. err) + revert() + return + end + return protected_wrapper, 1000/UPDATE_RATE_HZ +end + +-- start running update loop +return protected_wrapper() diff --git a/libraries/AP_Scripting/applets/VTOL-expo.md b/libraries/AP_Scripting/applets/VTOL-expo.md new file mode 100644 index 0000000000000..08ab8490e96ee --- /dev/null +++ b/libraries/AP_Scripting/applets/VTOL-expo.md @@ -0,0 +1,91 @@ +# VTOL Expo Tune + +This script implements a system for estimating the VTOL thrust expo +(MOT_THST_EXPO on copter, Q_M_THST_EXPO on quadplanes). + +The script is designed to be used in GUIDED mode. + +# Parameters + +The following parameters are used: + +## EXPO_ENABLE + +this must be set to 1 to enable the script + +## EXPO_RC_FUNC + +The RCz_OPTIONS scripting function binding to be used for this script. +Default RCz_OPTIONS binding is 300 (scripting1). + +## EXPO_PULSE_TIME + +This sets the length of the throttle pulses. The default of 1.5 +seconds should be good for most vehicles, but some larger vehicles may +need a longer value. + +## EXPO_PULSE_SLEW + +This sets the slew time of the throttle pulses. The default of 0.05 +seconds should be good for most vehicles, but some larger vehicles may +need a longer value. + +## EXPO_PULSE_THST + +This sets the amplitude of the throttle pulses as a proportion of +hover throttle. The default of 0.25 should be good for most vehicles. + +## EXPO_CONV_THR + +This sets the convergence threshold. When the acceleration error is +below this level for 5 consecutive pulse pairs the tuning is complete. + +## EXPO_LIMIT_MIN + +This sets the minimum expo value that the script will use. + +## EXPO_LIMIT_MAX + +This sets the maximum expo value that the script will use. + +# Operation + +You need to be in GUIDED mode with the vehicle at least 10 meters off +the ground. Have the VTOL-expo.lua script loaded and have a 3 position +switch setup with RCn_OPTION for that switch set to EXPO_RC_FUNC +(default to 300). + +When you are ready to tune move the switch to the middle position. The +vehicle will pulse down the throttle for half of EXPO_PULSE_TIME +seconds followed up a pulse up of the throttle for the same time. The +vehicle will then settle for a few seconds before repeating. After +each pulse the expo will be adjusted based on the measured +acceleration. + +When the tune is finished it will stop pulsing. You can then save the +tune by moving the switch to the top position. + +At any point before you have saved you can move the switch to the +bottom position and the original expo will be restored and the tune +will stop. You can also stop the tune by changing to any other flight +mode. A flight mode change before saving will also revert any expo +change. + +When the acceleration error has been less than EXPO_CONV_THR for 5 +pulse pairs the tune is finished and the final value will be reported +and the tuning will stop. + +You should tune in low wind conditions, and ensure the vehicle is not +oscillating at all before running the tune. If the vehicle oscillates +in roll, pitch or yaw while running the expo tune then the resulting +expo may be incorrect (it will get a value that is too high). You +should re-tune to prevent oscillation before re-running the expo tune. + +# Note on using in quadplanes + +In quadplanes the parameter Q_GUIDED_MODE will be overridden to a +value of 2 while running this script with EXPO_ENABLE=1. This ensures +that if you enter GUIDED mode from a VTOL mode it will not transition +to forward flight. A value of 2 for Q_GUIDED_MODE is new for the 4.5.x +plane release. + diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 6e64f74c7dd99..9b999a85ba7f0 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2015,6 +2015,11 @@ function vehicle:set_target_posvel_NED(target_pos, target_vel) end ---@return boolean function vehicle:set_target_pos_NED(target_pos, use_yaw, yaw_deg, use_yaw_rate, yaw_rate_degs, yaw_relative, terrain_alt) end +-- desc +---@param thrust number +---@return boolean +function vehicle:set_thrust(thrust) end + -- desc ---@param current_target Location_ud -- current target, from get_target_location() ---@param new_target Location_ud -- new target @@ -2746,6 +2751,10 @@ function ahrs:get_hagl() end ---@return Vector3f_ud function ahrs:get_accel() end +-- desc +---@return Vector3f_ud +function ahrs:get_accel_ef() end + -- desc ---@return Vector3f_ud function ahrs:get_gyro() end diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 5611d8d46a258..04c2269783909 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -34,6 +34,7 @@ singleton AP_AHRS method get_location alias get_position singleton AP_AHRS method get_home Location singleton AP_AHRS method get_gyro Vector3f singleton AP_AHRS method get_accel Vector3f +singleton AP_AHRS method get_accel_ef Vector3f singleton AP_AHRS method get_hagl boolean float'Null singleton AP_AHRS method wind_estimate Vector3f singleton AP_AHRS method groundspeed_vector Vector2f @@ -259,6 +260,7 @@ singleton AP_Vehicle method set_target_posvelaccel_NED boolean Vector3f Vector3f singleton AP_Vehicle method set_target_velaccel_NED boolean Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean singleton AP_Vehicle method set_target_velocity_NED boolean Vector3f singleton AP_Vehicle method set_target_angle_and_climbrate boolean float -180 180 float -90 90 float -360 360 float'skip_check boolean float'skip_check +singleton AP_Vehicle method set_thrust boolean float'skip_check singleton AP_Vehicle method get_circle_radius boolean float'Null singleton AP_Vehicle method set_circle_rate boolean float'skip_check singleton AP_Vehicle method set_steering_and_throttle boolean float -1 1 float -1 1 diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 5dc44475f7d29..711c873a9c62f 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -160,6 +160,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_thrust(float thrust) { return false; } // command throttle percentage and roll, pitch, yaw target // rates. For use with scripting controllers diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index e145d0d8239f4..826a750d148b5 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -18,6 +18,8 @@ #include "SIM_Motor.h" #include +#include +#include "SITL.h" using namespace SITL; @@ -212,9 +214,27 @@ float Motor::pwm_to_command(float pwm) const /* calculate thrust given a command value */ -float Motor::calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) const +float Motor::calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) { - float velocity_out = voltage_scale * max_outflow_velocity * sqrtf((1-mot_expo)*command + mot_expo*sq(command)); + /* + allow override of the expo + */ + float expo = mot_expo; + +#if AP_SIM_ENABLED + const auto *sitl = AP::sitl(); + if (sitl && !is_zero(sitl->vtol_motor_expo.get())) { + expo = sitl->vtol_motor_expo.get(); + } + + if (sitl && is_positive(sitl->vtol_motor_tc.get())) { + const float alpha = calc_lowpass_alpha_dt(1.0 / sitl->loop_rate_hz, 1.0 / sitl->vtol_motor_tc.get()); + filtered_command += (command - filtered_command) * alpha; + command = filtered_command; + } +#endif // AP_SIM_ENABLED + + float velocity_out = voltage_scale * max_outflow_velocity * sqrtf((1-expo)*command + expo*sq(command)); float ret = 0.5 * air_density * effective_prop_area * (sq(velocity_out) - sq(velocity_in)); #if 0 if (command > 0) { diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h index 39745b6843a91..6c779d0a6103d 100644 --- a/libraries/SITL/SIM_Motor.h +++ b/libraries/SITL/SIM_Motor.h @@ -120,7 +120,7 @@ class Motor { } // calculate thrust of motor - float calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) const; + float calc_thrust(float command, float air_density, float velocity_in, float voltage_scale); private: float mot_pwm_min; @@ -137,6 +137,7 @@ class Motor { float true_prop_area; float momentum_drag_coefficient; float diagonal_size; + float filtered_command; float last_command; uint64_t last_calc_us; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 3b5cf1fc3b322..fda96763d930d 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -379,6 +379,21 @@ const AP_Param::GroupInfo SIM::var_info3[] = { AP_SUBGROUPINFO(airspeed[1], "ARSPD2_", 51, SIM, SIM::AirspeedParm), #endif + // @Param: VTOL_EXPO + // @DisplayName: VTOL motor expo + // @Description: If this is non-zero then it overrides the VTOL motor thrust expo + // @Range: -1 1 + // @User: Advanced + AP_GROUPINFO("VTOL_EXPO", 52, SIM, vtol_motor_expo, 0.0), + + // @Param: VTOL_TC + // @DisplayName: VTOL motor time constant + // @Description: If non-zero then this gives a time constant for modelling motor response time + // @Range: 0 2 + // @Units: s + // @User: Advanced + AP_GROUPINFO("VTOL_TC", 53, SIM, vtol_motor_tc, 0.0), + #ifdef SFML_JOYSTICK AP_SUBGROUPEXTENSION("", 63, SIM, var_sfml_joystick), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 1596f9f929017..615cba891c8e0 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -347,6 +347,12 @@ class SIM { // what servos are motors AP_Int32 vibe_motor_mask; + + // VTOL motor expo, used if non-zero + AP_Float vtol_motor_expo; + + // VTOL motor response time constant + AP_Float vtol_motor_tc; // minimum throttle for addition of ins noise AP_Float ins_noise_throttle_min;