AP_Scripting: added AEROM_PATH_SCALE

and add some docs on parameters
This commit is contained in:
Andrew Tridgell 2022-11-19 07:15:02 +11:00
parent 9234e5168f
commit b2a38c0c60
2 changed files with 29 additions and 2 deletions

View File

@ -188,3 +188,17 @@ Moving the activation switch to the bottom position cancels any
running trick and stops the trick system. running trick and stops the trick system.
Changing flight modes will also cancel any active trick. Changing flight modes will also cancel any active trick.
## Parameters
There are a number of parameters you can set to adjust the path
tracking. Some of the key parameters are:
- AEROM_ANG_ACCEL : maximum angular acceleration in degrees per second per second. Reduce to give smoother flight, but more lag
- AEROM_ANG_TC : time constant for correcting angular roll errors. Reduce for snappier rolls, but more risk of oscillation
- AEROM_KE_ANG : knifeedge angle. This is the required pitch angle in knifeedge flight to hold height at cruise speed.
- AEROM_ENTRY_RATE : roll rate in degrees per second for entering and exiting a roll change
- AEROM_THR_MIN : minumum throttle percentage for all aerobatic maneuvers
- AEROM_THR_BOOST: minumum throttle percentage for maneuvers marked as throttle boost
- AEROM_YAW_ACCEL: maximum yaw acceleration in degrees per second per second. Lower to soften yaw control
- AEROM_PATH_SCALE: scale factor for all maneuvers. A value above 1.0 will increase the size of the maneuvers. A value below 1.0 will decrease the size. A negative value will mirror the maneuvers, allowing a sequence designed for left-to-right to be flown right-to-left.

View File

@ -35,6 +35,7 @@ AEROM_THR_MIN = bind_add_param('THR_MIN', 17, 0)
AEROM_THR_BOOST = bind_add_param('THR_BOOST', 18, 50) AEROM_THR_BOOST = bind_add_param('THR_BOOST', 18, 50)
AEROM_YAW_ACCEL = bind_add_param('YAW_ACCEL', 19, 1500) AEROM_YAW_ACCEL = bind_add_param('YAW_ACCEL', 19, 1500)
AEROM_LKAHD = bind_add_param('LKAHD', 20, 0.5) AEROM_LKAHD = bind_add_param('LKAHD', 20, 0.5)
AEROM_PATH_SCALE = bind_add_param('PATH_SCALE', 21, 1.0)
-- cope with old param values -- cope with old param values
if AEROM_ANG_ACCEL:get() < 100 and AEROM_ANG_ACCEL:get() > 0 then if AEROM_ANG_ACCEL:get() < 100 and AEROM_ANG_ACCEL:get() > 0 then
@ -1777,6 +1778,19 @@ function rotate_path(path_f, t, orientation, offset)
local speed = path_f.get_speed(t) local speed = path_f.get_speed(t)
local thr_boost = path_f.get_throttle_boost(t) local thr_boost = path_f.get_throttle_boost(t)
local point = quat_earth_to_body(orientation, point) local point = quat_earth_to_body(orientation, point)
local scale = AEROM_PATH_SCALE:get()
point = point:scale(math.abs(scale))
if scale < 0 then
-- we need to mirror the path
point:y(-point:y())
roll_correction = -roll_correction
angle = -angle
-- compensate path orientation for the mirroring
local orient = orientation:inverse()
point = quat_body_to_earth((orient * orient), point)
end
return point+offset, math.rad(angle+roll_correction), speed, thr_boost return point+offset, math.rad(angle+roll_correction), speed, thr_boost
end end
@ -1905,13 +1919,12 @@ function do_path()
local speed = target_groundspeed() local speed = target_groundspeed()
path_var.target_speed = speed path_var.target_speed = speed
path_var.length = path.get_length() path_var.length = path.get_length() * math.abs(AEROM_PATH_SCALE:get())
path_var.total_rate_rads_ef = makeVector3f(0.0, 0.0, 0.0) path_var.total_rate_rads_ef = makeVector3f(0.0, 0.0, 0.0)
--assuming constant velocity --assuming constant velocity
path_var.total_time = path_var.length/speed path_var.total_time = path_var.length/speed
path_var.last_pos = path.get_pos(0) --position at t0
--deliberately only want yaw component, because the maneuver should be performed relative to the earth, not relative to the initial orientation --deliberately only want yaw component, because the maneuver should be performed relative to the earth, not relative to the initial orientation
path_var.initial_ori = Quaternion() path_var.initial_ori = Quaternion()