Line data Source code
1 : /*
2 : * Aircraft Weathervane options common to vtol plane and copters
3 : */
4 : #include <AP_Vehicle/AP_Vehicle_Type.h>
5 : #include "AC_WeatherVane.h"
6 : #include <GCS_MAVLink/GCS.h>
7 : #include <AP_AHRS/AP_AHRS.h>
8 :
9 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
10 : #define WVANE_PARAM_ENABLED 1
11 : #define WVANE_PARAM_SPD_MAX_DEFAULT 0
12 : #define WVANE_PARAM_VELZ_MAX_DEFAULT 0
13 : #define WVANE_PARAM_GAIN_DEFAULT 0
14 : #else
15 : #define WVANE_PARAM_ENABLED 0
16 : #define WVANE_PARAM_SPD_MAX_DEFAULT 2
17 : #define WVANE_PARAM_VELZ_MAX_DEFAULT 1
18 : #define WVANE_PARAM_GAIN_DEFAULT 1
19 : #endif
20 :
21 :
22 : const AP_Param::GroupInfo AC_WeatherVane::var_info[] = {
23 :
24 : // @Param: ENABLE
25 : // @DisplayName: Enable
26 : // @Description: Enable weather vaning. When active, the aircraft will automatically yaw into wind when in a VTOL position controlled mode. Pilot yaw commands override the weathervaning action.
27 : // @Values: -1:Only use during takeoffs or landing see weathervane takeoff and land override parameters,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
28 : // @User: Standard
29 : AP_GROUPINFO_FLAGS("ENABLE", 1, AC_WeatherVane, _direction, WVANE_PARAM_ENABLED, AP_PARAM_FLAG_ENABLE),
30 :
31 : // @Param: GAIN
32 : // @DisplayName: Weathervaning gain
33 : // @Description: This converts the target roll/pitch angle of the aircraft into the correcting (into wind) yaw rate. e.g. Gain = 2, roll = 30 deg, pitch = 0 deg, yaw rate = 60 deg/s.
34 : // @Range: 0.5 4
35 : // @Increment: 0.1
36 : // @User: Standard
37 : AP_GROUPINFO("GAIN", 2, AC_WeatherVane, _gain, WVANE_PARAM_GAIN_DEFAULT),
38 :
39 : // @Param: ANG_MIN
40 : // @DisplayName: Weathervaning min angle
41 : // @Description: The minimum target roll/pitch angle before active weathervaning will start. This provides a dead zone that is particularly useful for poorly trimmed quadplanes.
42 : // @Units: deg
43 : // @Range: 0 10
44 : // @Increment: 0.1
45 : // @User: Standard
46 : AP_GROUPINFO("ANG_MIN", 3, AC_WeatherVane, _min_dz_ang_deg, 1.0),
47 :
48 : // @Param: HGT_MIN
49 : // @DisplayName: Weathervaning min height
50 : // @Description: Above this height weathervaning is permitted. If a range finder is fitted or if terrain is enabled, this parameter sets height AGL. Otherwise, this parameter sets height above home. Set zero to ignore minimum height requirement to activate weathervaning.
51 : // @Description{Plane}: Above this height weathervaning is permitted. If RNGFND_LANDING is enabled or terrain is enabled then this parameter sets height AGL. Otherwise this parameter sets height above home. Set zero to ignore minimum height requirement to activate weathervaning
52 : // @Units: m
53 : // @Range: 0 50
54 : // @Increment: 1
55 : // @User: Standard
56 : AP_GROUPINFO("HGT_MIN", 4, AC_WeatherVane, _min_height, 0.0),
57 :
58 : // @Param: SPD_MAX
59 : // @DisplayName: Weathervaning max ground speed
60 : // @Description: Below this ground speed weathervaning is permitted. Set to 0 to ignore this condition when checking if vehicle should weathervane.
61 : // @Units: m/s
62 : // @Range: 0 50
63 : // @Increment: 0.1
64 : // @User: Standard
65 : AP_GROUPINFO("SPD_MAX", 5, AC_WeatherVane, _max_vel_xy, WVANE_PARAM_SPD_MAX_DEFAULT),
66 :
67 : // @Param: VELZ_MAX
68 : // @DisplayName: Weathervaning max vertical speed
69 : // @Description: The maximum climb or descent speed that the vehicle will still attempt to weathervane. Set to 0 to ignore this condition to get the aircraft to weathervane at any climb/descent rate. This is particularly useful for aircraft with low disc loading that struggle with yaw control in decent.
70 : // @Units: m/s
71 : // @Range: 0 5
72 : // @Increment: 0.1
73 : // @User: Standard
74 : AP_GROUPINFO("VELZ_MAX", 6, AC_WeatherVane, _max_vel_z, WVANE_PARAM_VELZ_MAX_DEFAULT),
75 :
76 : // @Param: TAKEOFF
77 : // @DisplayName: Takeoff override
78 : // @Description: Override the weather vaning behaviour when in takeoffs
79 : // @Values: -1:No override,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
80 : // @User: Standard
81 : AP_GROUPINFO("TAKEOFF", 7, AC_WeatherVane, _takeoff_direction, -1),
82 :
83 : // @Param: LAND
84 : // @DisplayName: Landing override
85 : // @Description: Override the weather vaning behaviour when in landing
86 : // @Values: -1:No override,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
87 : // @User: Standard
88 : AP_GROUPINFO("LAND", 8, AC_WeatherVane, _landing_direction, -1),
89 :
90 : // @Param: OPTIONS
91 : // @DisplayName: Weathervaning options
92 : // @Description: Options impacting weathervaning behaviour
93 : // @Bitmask: 0:Use pitch when nose or tail-in for faster weathervaning
94 : // @User: Standard
95 : AP_GROUPINFO("OPTIONS", 9, AC_WeatherVane, _options, 0),
96 :
97 : AP_GROUPEND
98 : };
99 :
100 :
101 : // Constructor
102 1202 : AC_WeatherVane::AC_WeatherVane(void)
103 : {
104 1202 : AP_Param::setup_object_defaults(this, var_info);
105 1202 : }
106 :
107 1709661 : bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing)
108 : {
109 1709661 : Direction dir = (Direction)_direction.get();
110 1709661 : if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0) || !is_positive(_gain)) {
111 : // parameter disabled, or 0 gain
112 : // disabled temporarily
113 : // dont't override pilot
114 1616610 : reset();
115 1616610 : return false;
116 : }
117 :
118 : // override direction when in takeoff for landing
119 93051 : if (is_takeoff && (_takeoff_direction >= 0)) {
120 0 : dir = (Direction)_takeoff_direction.get();
121 : }
122 93051 : if (is_landing && (_landing_direction >= 0)) {
123 0 : dir = (Direction)_landing_direction.get();
124 : }
125 93051 : if (dir == Direction::OFF || (dir == Direction::TAKEOFF_OR_LAND_ONLY)) {
126 : // Disabled for takeoff or landing
127 : // Disabled if in flight and dir = -1
128 0 : reset();
129 0 : return false;
130 : }
131 :
132 : // Check if we are above the minimum height to weather vane
133 93051 : if (is_positive(_min_height) && (hgt <= _min_height)) {
134 0 : reset();
135 0 : return false;
136 : }
137 :
138 : // Check if we meet the velocity thresholds to allow weathervaning
139 93051 : if (is_positive(_max_vel_xy) || is_positive(_max_vel_z)) {
140 36519 : Vector3f vel_ned;
141 73038 : if (!AP::ahrs().get_velocity_NED(vel_ned) || // need speed estimate
142 140860 : (is_positive(_max_vel_xy) && (vel_ned.xy().length_squared() > (_max_vel_xy*_max_vel_xy))) || // check xy speed
143 67822 : (is_positive(_max_vel_z) && (fabsf(vel_ned.z) > _max_vel_z))) { // check z speed
144 8791 : reset();
145 8791 : return false;
146 : }
147 : }
148 :
149 84260 : const uint32_t now = AP_HAL::millis();
150 84260 : if (now - last_check_ms > 250) {
151 : // not run this function or reset recently
152 8 : reset();
153 : }
154 84260 : last_check_ms = now;
155 :
156 : /*
157 : Use a 2 second buffer to ensure weathervaning occurs once the vehicle has
158 : clearly achieved an acceptable condition.
159 : */
160 84260 : if (first_activate_ms == 0) {
161 15 : first_activate_ms = now;
162 : }
163 84260 : if (now - first_activate_ms < 2000) {
164 9216 : return false;
165 : }
166 :
167 75044 : const float deadzone_cdeg = _min_dz_ang_deg*100.0;
168 75044 : float output = 0.0;
169 75044 : const char* dir_string = "";
170 :
171 : // should we enable pitch input for nose-in and tail-in?
172 75044 : const bool pitch_enable = (uint8_t(_options.get()) & uint8_t(Options::PITCH_ENABLE)) != 0;
173 :
174 75044 : switch (dir) {
175 0 : case Direction::OFF:
176 : case Direction::TAKEOFF_OR_LAND_ONLY:
177 0 : reset();
178 0 : return false;
179 :
180 66135 : case Direction::NOSE_IN:
181 66135 : if (pitch_enable && is_positive(pitch_cdeg - deadzone_cdeg)) {
182 0 : output = fabsf(roll_cdeg) + (pitch_cdeg - deadzone_cdeg);
183 : } else {
184 66135 : output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
185 : }
186 66135 : if (is_negative(roll_cdeg)) {
187 30849 : output *= -1.0;
188 : }
189 66135 : dir_string = "nose in";
190 66135 : break;
191 :
192 0 : case Direction::NOSE_OR_TAIL_IN:
193 0 : output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
194 0 : if (is_negative(roll_cdeg) != is_positive(pitch_cdeg)) {
195 0 : output *= -1.0;
196 : }
197 0 : dir_string = "nose or tail in";
198 0 : break;
199 :
200 8909 : case Direction::SIDE_IN:
201 8909 : output = MAX(fabsf(pitch_cdeg) - deadzone_cdeg, 0.0);
202 8909 : if (is_positive(pitch_cdeg) != is_positive(roll_cdeg)) {
203 3088 : output *= -1.0;
204 : }
205 8909 : dir_string = "side in";
206 8909 : break;
207 :
208 0 : case Direction::TAIL_IN:
209 0 : if (pitch_enable && is_negative(pitch_cdeg + deadzone_cdeg)) {
210 0 : output = fabsf(roll_cdeg) - (pitch_cdeg + deadzone_cdeg);
211 : } else {
212 0 : output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
213 : }
214 0 : if (is_positive(roll_cdeg)) {
215 0 : output *= -1.0;
216 : }
217 0 : dir_string = "tail in";
218 0 : break;
219 : }
220 :
221 75044 : if (active_msg_dir != dir) {
222 13 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Weathervane Active: %s", dir_string);
223 : (void)dir_string; // in case GCS is disabled
224 13 : active_msg_dir = dir;
225 : }
226 :
227 : // Slew output and apply gain
228 75044 : last_output = 0.98 * last_output + 0.02 * output * _gain;
229 75044 : yaw_output = last_output;
230 75044 : return true;
231 : }
232 :
233 : // Reset the weathervane controller
234 1993316 : void AC_WeatherVane::reset(void)
235 : {
236 1993316 : last_output = 0;
237 1993316 : active_msg_dir = Direction::OFF;
238 1993316 : first_activate_ms = 0;
239 1993316 : last_check_ms = AP_HAL::millis();
240 1993316 : }
241 :
|
