diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/README.md b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/README.md index 160e281b6e..1d029e6422 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/README.md +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/README.md @@ -19,33 +19,35 @@ AUTO missions. Once the trick is completed, the mode that was being used at the track and altitude are reset to the values present when the mode is restored. Tricks in AUTO missions require that they be performed between two waypoints to establish the ground track. -| ID | Name | Arg1 | Arg2 | Arg3 | Arg4 | Turnaround | -| -- | ------------------------ | ------ | ---------- | ------- | ---------- | ---------- | -| 1 | Figure Eight | radius | bank angle | | | No | -| 2 | Loop | radius | bank angle | num loops | | No | -| 3 | Horizontal Rectangle | length | width | radius | bank angle | No | -| 4 | Climbing Circle | radius | height | bank angle | | No | -| 5 | vertical Box | length | height | radius | | No | -| 6 | Immelmann (FastRoll) | radius | | | | Yes | -| 7 | Straight Roll | length | num rolls | | | No | -| 8 | Rolling Circle | radius | num rolls | | | No | -| 9 | Half Cuban Eight | radius | | | | Yes | -| 10 | Half Reverse Cuban Eight | radius | | | | Yes | -| 11 | Cuban Eight | radius | | | | No | -| 12 | Humpty Bump | radius | height | | | Yes | -| 13 | Straight Flight | length | bank angle | | | No | -| 14 | Scale Figure Eight | radius | bank angle | | | No | -| 15 | Immelmann Turn | radius | | | | Yes | -| 16 | Split-S | radius | | | | Yes | -| 17 | Upline-45 | radius | height gain | | | No | -| 18 | Downline-45 | radius | height loss | | | No | -| 19 | Stall Turn | radius | height | direction | | Yes | -| 20 | Procedure Turn | radius | bank angle | step-out | | Yes | -| 21 | Derry Turn | radius | bank angle | | | No | -| 22 | Two Point Roll | length | | | | No | -| 23 | Half Climbing Circle | radius | height | bank angle | | Yes | -| 24 | Crossbox Humpty | radius | height | | | Yes | -| 25 | Laydown Humpty | radius | height | | | Yes | +| ID | Name | Arg1 | Arg2 | Arg3 | Arg4 | Turnaround | +| -- | ------------------------ | ------ | ---------- | ------- | ---------- | ---------- | +| 1 | Figure Eight | radius | bank angle | | | No | +| 2 | Loop | radius | bank angle | num loops | | No | +| 3 | Horizontal Rectangle | length | width | radius | bank angle | No | +| 4 | Climbing Circle | radius | height | bank angle | | No | +| 5 | vertical Box | length | height | radius | | No | +| 6 | Immelmann (FastRoll) | radius | | | | Yes | +| 7 | Straight Roll | length | num rolls | | | No | +| 8 | Rolling Circle | radius | num rolls | | | No | +| 9 | Half Cuban Eight | radius | | | | Yes | +| 10 | Half Reverse Cuban Eight | radius | | | | Yes | +| 11 | Cuban Eight | radius | | | | No | +| 12 | Humpty Bump | radius | height | | | Yes | +| 13 | Straight Flight | length | bank angle | | | No | +| 14 | Scale Figure Eight | radius | bank angle | | | No | +| 15 | Immelmann Turn | radius | | | | Yes | +| 16 | Split-S | radius | | | | Yes | +| 17 | Upline-45 | radius | height gain | | | No | +| 18 | Downline-45 | radius | height loss | | | No | +| 19 | Stall Turn | radius | height | direction | | Yes | +| 20 | Procedure Turn | radius | bank angle | step-out | | Yes | +| 21 | Derry Turn | radius | bank angle | | | No | +| 22 | Two Point Roll | length | | | | No | +| 23 | Half Climbing Circle | radius | height | bank angle | | Yes | +| 24 | Crossbox Humpty | radius | height | | | Yes | +| 25 | Laydown Humpty | radius | height | | | Yes | +| 25 | Barrell Roll | radius | length | num spirals | | No | +| 26 | Straight Hold | length | bank angle | | | No | Note: In the script you will find other (specialised) manouvers which do not appear in the 'command table'. These tend to be specialised manouvers which may expect an inverted entry or diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index f957316de4..6d7b944a9d 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -476,6 +476,13 @@ function quat_body_to_earth(quat, v) return v end +--[[ + copy a quaternion +--]] +function quat_copy(q) + return q:inverse():inverse() +end + --[[ path component that does a straight horizontal line --]] @@ -557,16 +564,6 @@ function path_horizontal_arc(_radius, _angle, _height_gain) function self.get_final_orientation() local q = Quaternion() q:from_axis_angle(makeVector3f(0,0,1), sgn(radius)*math.rad(angle)) - --[[ - we also need to apply the roll correction to the final orientation - --]] - local rc = self.get_roll_correction(1.0) - if rc ~= 0 then - local q2 = Quaternion() - q2:from_axis_angle(makeVector3f(1,0,0), math.rad(-rc)) - q = q * q2 - q:normalize() - end return q end @@ -583,6 +580,42 @@ function path_horizontal_arc(_radius, _angle, _height_gain) return self end +--[[ + path component that does a cylinder for a barrell roll +--]] +function path_cylinder(_radius, _length, _num_spirals) + local self = PathComponent() + self.name = "path_cylinder" + local radius = _radius + local length = _length + local num_spirals = _num_spirals + local gamma = math.atan((length/num_spirals)/(2*math.pi*radius)) + local qrot = Quaternion() + qrot:from_axis_angle(makeVector3f(0,0,1), (0.5*math.pi)-gamma) + + function self.get_pos(t) + local t2ang = t * num_spirals * math.pi * 2 + local v = makeVector3f(length*t, math.abs(radius)*math.sin(t2ang+math.pi), -radius*(1.0 - math.cos(t2ang))) + return quat_earth_to_body(qrot, v) + end + + function self.get_length() + local circumference = 2 * math.pi * math.abs(radius) + local length_per_spiral = length / num_spirals + local helix_length = math.sqrt(length_per_spiral*length_per_spiral + circumference*circumference) + return helix_length * num_spirals + end + + --[[ + roll correction for the rotation caused by the path + --]] + function self.get_roll_correction(t) + return t*360*math.sin(gamma)*num_spirals + end + + return self +end + --[[ a Path has the methods of both RollComponent and PathComponent allowing for a complete description of a subpath @@ -593,7 +626,10 @@ function Path(_path_component, _roll_component) local path_component = _path_component local roll_component = _roll_component function self.get_roll(t, time_s) - return wrap_180(roll_component.get_roll(t, time_s) + path_component.get_roll_correction(t)) + return wrap_180(roll_component.get_roll(t, time_s)) + end + function self.get_roll_correction(t) + return path_component.get_roll_correction(t) end function self.get_pos(t) return path_component.get_pos(t) @@ -628,6 +664,7 @@ function path_composer(_name, _subpaths) local start_orientation = {} local start_pos = {} local start_angle = {} + local start_roll_correction = {} local end_speed = {} local start_speed = {} local total_length = 0 @@ -637,6 +674,7 @@ function path_composer(_name, _subpaths) local orientation = Quaternion() local pos = makeVector3f(0,0,0) local angle = 0 + local roll_correction = 0 local speed = target_groundspeed() local highest_i = 0 local cache_i = -1 @@ -665,9 +703,10 @@ function path_composer(_name, _subpaths) for i = 1, num_sub_paths do -- accumulate orientation, position and angle - start_orientation[i] = orientation - start_pos[i] = pos + start_orientation[i] = quat_copy(orientation) + start_pos[i] = pos:copy() start_angle[i] = angle + start_roll_correction[i] = roll_correction local sp = self.subpath(i) lengths[i] = sp.get_length() @@ -676,9 +715,11 @@ function path_composer(_name, _subpaths) local spos = quat_earth_to_body(orientation, sp.get_pos(1.0)) pos = pos + spos - orientation = orientation * sp.get_final_orientation() + orientation = sp.get_final_orientation() * orientation orientation:normalize() + angle = angle + sp.get_roll(1.0, lengths[i]/speed) + roll_correction = roll_correction + sp.get_roll_correction(1.0) start_speed[i] = speed end_speed[i] = sp.get_speed(1.0) @@ -688,15 +729,15 @@ function path_composer(_name, _subpaths) speed = end_speed[i] if sp.roll_ref ~= nil then - q = Quaternion() + local q = Quaternion() q:from_axis_angle(makeVector3f(1,0,0), math.rad(sp.roll_ref)) - orientation = orientation * q + orientation = q * orientation orientation:normalize() end end -- get our final orientation, including roll - local final_orientation = orientation + local final_orientation = quat_copy(orientation) local q = Quaternion() q:from_axis_angle(makeVector3f(1,0,0), math.rad(wrap_180(angle))) final_orientation = q * final_orientation @@ -753,6 +794,12 @@ function path_composer(_name, _subpaths) return angle + start_angle[i] end + function self.get_roll_correction(t) + local subpath_t, i = self.get_subpath_t(t) + local sp = self.subpath(i) + return sp.get_roll_correction(subpath_t) + start_roll_correction[i] + end + -- return speed for the composed path at time t function self.get_speed(t) local subpath_t, i = self.get_subpath_t(t) @@ -783,7 +830,11 @@ end function make_paths(name, paths) local p = {} for i = 1, #paths do - p[i] = Path(paths[i][1], paths[i][2]) + if paths[i][2] == nil then + p[i] = paths[i][1] + else + p[i] = Path(paths[i][1], paths[i][2]) + end if paths[i].roll_ref then p[i].roll_ref = paths[i].roll_ref end @@ -808,6 +859,12 @@ function half_climbing_circle(radius, height, bank_angle, arg4) }) end +function partial_circle(radius, bank_angle, angle) + return make_paths("partial_circle", { + { path_horizontal_arc(radius, angle, 0), roll_angle_entry_exit(bank_angle) }, + }) +end + function loop(radius, bank_angle, num_loops, arg4) if not num_loops or num_loops <= 0 then num_loops = 1 @@ -949,12 +1006,37 @@ function rolling_circle(radius, num_rolls, arg3, arg4) }) end + +function cylinder(radius, length, num_spirals, arg4) + return make_paths("cylinder", { + { path_cylinder(radius, length, num_spirals), roll_angle(0), thr_boost=true }, + }) +end + +function barrell_roll(radius, length, num_spirals, arg4) + local gamma_deg = math.deg(math.atan((length/num_spirals)/(2*math.pi*radius))) + local speed = target_groundspeed() + local bank = math.deg(math.atan((speed*speed) / (radius * GRAVITY_MSS))) + + return make_paths("barrell_roll", { + { path_horizontal_arc(-radius, 90-gamma_deg, 0), roll_angle_entry_exit(-bank) }, + { path_cylinder(radius, length, num_spirals), roll_angle(0) }, + { path_horizontal_arc(radius, 90-gamma_deg, 0), roll_angle_entry_exit(bank) }, + }) +end + function straight_flight(length, bank_angle, arg3, arg4) return make_paths("straight_flight", { { path_straight(length), roll_angle_entry_exit(bank_angle) }, }) end +function straight_hold(length, bank_angle, arg3, arg4) + return make_paths("straight_hold", { + { path_straight(length), roll_angle_entry(bank_angle) }, + }) +end + function scale_figure_eight(r, bank_angle, arg3, arg4) local rabs = math.abs(r) return make_paths("scale_figure_eight", { @@ -1638,10 +1720,11 @@ function rotate_path(path_f, t, orientation, offset) local t = constrain(t, 0, 1) local point = path_f.get_pos(t) local angle = path_f.get_roll(t) + local roll_correction = path_f.get_roll_correction(t) local speed = path_f.get_speed(t) local thr_boost = path_f.get_throttle_boost(t) local point = quat_earth_to_body(orientation, point) - return point+offset, math.rad(angle), speed, thr_boost + return point+offset, math.rad(angle+roll_correction), speed, thr_boost end --Given vec1, vec2, returns an (rotation axis, angle) tuple that rotates vec1 to be parallel to vec2 @@ -2164,6 +2247,9 @@ command_table[22]= PathFunction(two_point_roll, "Two Point Roll") command_table[23]= PathFunction(half_climbing_circle, "Half Climbing Circle") command_table[24]= PathFunction(crossbox_humpty, "Crossbox Humpty") command_table[25]= PathFunction(laydown_humpty, "Laydown Humpty") +command_table[26] = PathFunction(barrell_roll, "Barrell Roll") +command_table[27]= PathFunction(straight_flight, "Straight Hold") +command_table[28] = PathFunction(partial_circle, "Partial Circle") command_table[200] = PathFunction(test_all_paths, "Test Suite") command_table[201] = PathFunction(nz_clubman, "NZ Clubman") command_table[202] = PathFunction(f3a_p23_l_r, "FAI F3A P23 L to R") @@ -2203,6 +2289,9 @@ load_table["crossbox_humpty"] = crossbox_humpty load_table["laydown_humpty"] = laydown_humpty load_table["straight_align"] = straight_align load_table["figure_eight"] = figure_eight +load_table["barrell_roll"] = barrell_roll +load_table["straight_hold"] = straight_hold +load_table["partial_circle"] = partial_circle --[[ @@ -2250,12 +2339,12 @@ function load_trick(id) local f = load_table[cmd] if f == nil then gcs:send_text(0,string.format("Unknown command '%s' in %s", cmd, fname)) - return - end - paths[#paths+1] = { f, { arg1, arg2, arg3, arg4 }} - if message ~= nil then - paths[#paths].message = message - message = nil + else + paths[#paths+1] = { f, { arg1, arg2, arg3, arg4 }} + if message ~= nil then + paths[#paths].message = message + message = nil + end end end end diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/trick72.txt b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/trick72.txt index 8d21dc9113..b711d0644e 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/trick72.txt +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/trick72.txt @@ -32,7 +32,7 @@ split_s 30 straight_align 0 message: RollingCircle -rolling_circle -50 3 +rolling_circle -50 2 straight_align -50 message: HumptyBump @@ -43,14 +43,11 @@ message: HalfCubanEight half_cuban_eight 25 straight_align 75 -message: Upline45 -upline_45 30 50 +message: BarrellRoll +barrell_roll 30 100 2 -message: Downline45 -downline_45 30 50 - -message: HalfReverseCubanEight -half_reverse_cuban_eight 25 +message: ProcedureTurn +procedure_turn 30 45 30 # get back to the start point straight_align 0