Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_WindVane_config.h"
17 :
18 : #if AP_WINDVANE_ENABLED
19 :
20 : #include "AP_WindVane.h"
21 :
22 : #include "AP_WindVane_Home.h"
23 : #include "AP_WindVane_Analog.h"
24 : #include "AP_WindVane_ModernDevice.h"
25 : #include "AP_WindVane_Airspeed.h"
26 : #include "AP_WindVane_RPM.h"
27 : #include "AP_WindVane_SITL.h"
28 : #include "AP_WindVane_NMEA.h"
29 :
30 : #include <GCS_MAVLink/GCS.h>
31 : #include <AP_AHRS/AP_AHRS.h>
32 : #include <AP_HAL/AP_HAL.h>
33 : #include <AP_Logger/AP_Logger.h>
34 : #include <AP_SerialManager/AP_SerialManager.h>
35 :
36 : extern const AP_HAL::HAL& hal;
37 :
38 : const AP_Param::GroupInfo AP_WindVane::var_info[] = {
39 :
40 : // @Param: TYPE
41 : // @DisplayName: Wind Vane Type
42 : // @Description: Wind Vane type
43 : // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog,4:NMEA,10:SITL true,11:SITL apparent
44 : // @User: Standard
45 : // @RebootRequired: True
46 : AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE),
47 :
48 : // 2 was RC_IN_NO
49 :
50 : // @Param: DIR_PIN
51 : // @DisplayName: Wind vane analog voltage pin for direction
52 : // @Description: Analog input pin to read as wind vane direction
53 : // @Values: 11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,103:Pixhawk SBUS
54 : // @Range: -1 127
55 : // @User: Standard
56 : AP_GROUPINFO("DIR_PIN", 3, AP_WindVane, _dir_analog_pin, WINDVANE_DEFAULT_PIN),
57 :
58 : // @Param: DIR_V_MIN
59 : // @DisplayName: Wind vane voltage minimum
60 : // @Description: Minimum voltage supplied by analog wind vane. When using pin 103, the maximum value of the parameter is 3.3V.
61 : // @Units: V
62 : // @Increment: 0.01
63 : // @Range: 0 5.0
64 : // @User: Standard
65 : AP_GROUPINFO("DIR_V_MIN", 4, AP_WindVane, _dir_analog_volt_min, 0.0f),
66 :
67 : // @Param: DIR_V_MAX
68 : // @DisplayName: Wind vane voltage maximum
69 : // @Description: Maximum voltage supplied by analog wind vane. When using pin 103, the maximum value of the parameter is 3.3V.
70 : // @Units: V
71 : // @Increment: 0.01
72 : // @Range: 0 5.0
73 : // @User: Standard
74 : AP_GROUPINFO("DIR_V_MAX", 5, AP_WindVane, _dir_analog_volt_max, 3.3f),
75 :
76 : // @Param: DIR_OFS
77 : // @DisplayName: Wind vane headwind offset
78 : // @Description: Angle offset when analog windvane is indicating a headwind, ie 0 degress relative to vehicle
79 : // @Units: deg
80 : // @Increment: 1
81 : // @Range: 0 360
82 : // @User: Standard
83 : AP_GROUPINFO("DIR_OFS", 6, AP_WindVane, _dir_analog_bearing_offset, 0.0f),
84 :
85 : // @Param: DIR_FILT
86 : // @DisplayName: apparent Wind vane direction low pass filter frequency
87 : // @Description: apparent Wind vane direction low pass filter frequency, a value of -1 disables filter
88 : // @Units: Hz
89 : // @User: Standard
90 : AP_GROUPINFO("DIR_FILT", 7, AP_WindVane, _dir_filt_hz, 0.5f),
91 :
92 : // @Param: CAL
93 : // @DisplayName: Wind vane calibration start
94 : // @Description: Start wind vane calibration by setting this to 1 or 2
95 : // @Values: 0:None, 1:Calibrate direction, 2:Calibrate speed
96 : // @User: Standard
97 : AP_GROUPINFO("CAL", 8, AP_WindVane, _calibration, 0),
98 :
99 : // @Param: DIR_DZ
100 : // @DisplayName: Wind vane deadzone when using analog sensor
101 : // @Description: Wind vane deadzone when using analog sensor
102 : // @Units: deg
103 : // @Increment: 1
104 : // @Range: 0 360
105 : // @User: Standard
106 : AP_GROUPINFO("DIR_DZ", 9, AP_WindVane, _dir_analog_deadzone, 0),
107 :
108 : // @Param: SPEED_MIN
109 : // @DisplayName: Wind vane cut off wind speed
110 : // @Description: Wind vane direction will be ignored when apparent wind speeds are below this value (if wind speed sensor is present). If the apparent wind is consistently below this value the vane will not work
111 : // @Units: m/s
112 : // @Increment: 0.1
113 : // @Range: 0 5
114 : // @User: Standard
115 : AP_GROUPINFO("SPEED_MIN", 10, AP_WindVane, _dir_speed_cutoff, 0),
116 :
117 : // @Param: SPEED_TYPE
118 : // @DisplayName: Wind speed sensor Type
119 : // @Description: Wind speed sensor type
120 : // @Values: 0:None,1:Airspeed library,2:Modern Devices Wind Sensor,3:RPM library,4:NMEA,10:SITL true,11:SITL apparent
121 : // @User: Standard
122 : // @RebootRequired: True
123 : AP_GROUPINFO("SPEED_TYPE", 11, AP_WindVane, _speed_sensor_type, 0),
124 :
125 : // @Param: SPEED_PIN
126 : // @DisplayName: Wind vane speed sensor analog pin
127 : // @Description: Wind speed analog speed input pin for Modern Devices Wind Sensor rev. p
128 : // @Values: 11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,103:Pixhawk SBUS
129 : // @Range: -1 127
130 : // @User: Standard
131 : AP_GROUPINFO("SPEED_PIN", 12, AP_WindVane, _speed_sensor_speed_pin, WINDSPEED_DEFAULT_SPEED_PIN),
132 :
133 : // @Param: TEMP_PIN
134 : // @DisplayName: Wind vane speed sensor analog temp pin
135 : // @Description: Wind speed sensor analog temp input pin for Modern Devices Wind Sensor rev. p, set to -1 to diasble temp readings
136 : // @Values: 11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,103:Pixhawk SBUS
137 : // @Range: -1 127
138 : // @User: Standard
139 : AP_GROUPINFO("TEMP_PIN", 13, AP_WindVane, _speed_sensor_temp_pin, WINDSPEED_DEFAULT_TEMP_PIN),
140 :
141 : // @Param: SPEED_OFS
142 : // @DisplayName: Wind speed sensor analog voltage offset
143 : // @Description: Wind sensor analog voltage offset at zero wind speed
144 : // @Units: V
145 : // @Increment: 0.01
146 : // @Range: 0 3.3
147 : // @User: Standard
148 : AP_GROUPINFO("SPEED_OFS", 14, AP_WindVane, _speed_sensor_voltage_offset, WINDSPEED_DEFAULT_VOLT_OFFSET),
149 :
150 : // @Param: SPEED_FILT
151 : // @DisplayName: apparent wind speed low pass filter frequency
152 : // @Description: apparent Wind speed low pass filter frequency, a value of -1 disables filter
153 : // @Units: Hz
154 : // @User: Standard
155 : AP_GROUPINFO("SPEED_FILT", 15, AP_WindVane, _speed_filt_hz, 0.5f),
156 :
157 : // @Param: TRUE_FILT
158 : // @DisplayName: True speed and direction low pass filter frequency
159 : // @Description: True speed and direction low pass filter frequency, a value of -1 disables filter
160 : // @Units: Hz
161 : // @User: Standard
162 : AP_GROUPINFO("TRUE_FILT", 16, AP_WindVane, _true_filt_hz, 0.05f),
163 :
164 : AP_GROUPEND
165 : };
166 :
167 : // constructor
168 285 : AP_WindVane::AP_WindVane()
169 : {
170 285 : AP_Param::setup_object_defaults(this, var_info);
171 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
172 285 : if (_singleton) {
173 0 : AP_HAL::panic("Too many Wind Vane sensors");
174 : }
175 : #endif
176 285 : _singleton = this;
177 285 : }
178 :
179 : /*
180 : * Get the AP_WindVane singleton
181 : */
182 0 : AP_WindVane *AP_WindVane::get_singleton()
183 : {
184 0 : return _singleton;
185 : }
186 :
187 : // return true if wind vane is enabled
188 461508 : bool AP_WindVane::enabled() const
189 : {
190 461508 : return _direction_type != WINDVANE_NONE;
191 : }
192 :
193 : // return true if wind speed is enabled
194 0 : bool AP_WindVane::wind_speed_enabled() const
195 : {
196 0 : return (_speed_sensor_type != WINDSPEED_NONE);
197 : }
198 :
199 : // Initialize the Wind Vane object and prepare it for use
200 285 : void AP_WindVane::init(const AP_SerialManager& serial_manager)
201 : {
202 : // don't construct twice
203 285 : if (_direction_driver != nullptr || _speed_driver != nullptr ) {
204 0 : return;
205 : }
206 :
207 : // wind direction
208 285 : switch (_direction_type) {
209 281 : case WindVaneType::WINDVANE_NONE:
210 : // WindVane disabled
211 281 : return;
212 : #if AP_WINDVANE_HOME_ENABLED
213 0 : case WindVaneType::WINDVANE_HOME_HEADING:
214 : case WindVaneType::WINDVANE_PWM_PIN:
215 0 : _direction_driver = NEW_NOTHROW AP_WindVane_Home(*this);
216 0 : break;
217 : #endif
218 : #if AP_WINDVANE_ANALOG_ENABLED
219 0 : case WindVaneType::WINDVANE_ANALOG_PIN:
220 0 : _direction_driver = NEW_NOTHROW AP_WindVane_Analog(*this);
221 0 : break;
222 : #endif
223 : #if AP_WINDVANE_SIM_ENABLED
224 4 : case WindVaneType::WINDVANE_SITL_TRUE:
225 : case WindVaneType::WINDVANE_SITL_APPARENT:
226 4 : _direction_driver = NEW_NOTHROW AP_WindVane_SITL(*this);
227 4 : break;
228 : #endif
229 : #if AP_WINDVANE_NMEA_ENABLED
230 0 : case WindVaneType::WINDVANE_NMEA:
231 0 : _direction_driver = NEW_NOTHROW AP_WindVane_NMEA(*this);
232 0 : _direction_driver->init(serial_manager);
233 0 : break;
234 : #endif
235 : }
236 :
237 : // wind speed
238 4 : switch (_speed_sensor_type) {
239 0 : case Speed_type::WINDSPEED_NONE:
240 0 : break;
241 : #if AP_WINDVANE_AIRSPEED_ENABLED
242 0 : case Speed_type::WINDSPEED_AIRSPEED:
243 0 : _speed_driver = NEW_NOTHROW AP_WindVane_Airspeed(*this);
244 0 : break;
245 : #endif
246 : #if AP_WINDVANE_MODERNDEVICE_ENABLED
247 0 : case Speed_type::WINDVANE_WIND_SENSOR_REV_P:
248 0 : _speed_driver = NEW_NOTHROW AP_WindVane_ModernDevice(*this);
249 0 : break;
250 : #endif
251 : #if AP_WINDVANE_SIM_ENABLED
252 4 : case Speed_type::WINDSPEED_SITL_TRUE:
253 : case Speed_type::WINDSPEED_SITL_APPARENT:
254 : // single driver does both speed and direction
255 4 : if (_direction_type != _speed_sensor_type) {
256 0 : _speed_driver = NEW_NOTHROW AP_WindVane_SITL(*this);
257 : } else {
258 4 : _speed_driver = _direction_driver;
259 : }
260 4 : break;
261 : #endif // AP_WINDVANE_SIM_ENABLED
262 : #if AP_WINDVANE_NMEA_ENABLED
263 0 : case Speed_type::WINDSPEED_NMEA:
264 : // single driver does both speed and direction
265 0 : if (_direction_type != WindVaneType::WINDVANE_NMEA) {
266 0 : _speed_driver = NEW_NOTHROW AP_WindVane_NMEA(*this);
267 0 : _speed_driver->init(serial_manager);
268 : } else {
269 0 : _speed_driver = _direction_driver;
270 : }
271 0 : break;
272 : #endif // AP_WINDVANE_NMEA_ENABLED
273 : #if AP_WINDVANE_RPM_ENABLED
274 0 : case Speed_type::WINDSPEED_RPM:
275 0 : _speed_driver = NEW_NOTHROW AP_WindVane_RPM(*this);
276 0 : break;
277 : #endif
278 : }
279 : }
280 :
281 : // update wind vane, expected to be called at 20hz
282 314458 : void AP_WindVane::update()
283 : {
284 314458 : const bool have_speed = _speed_driver != nullptr;
285 314458 : const bool have_direction = _direction_driver != nullptr;
286 :
287 : // exit immediately if not enabled
288 314458 : if (!enabled() || (!have_speed && !have_direction)) {
289 302173 : return;
290 : }
291 :
292 : // calibrate if booted and disarmed
293 12285 : if (!hal.util->get_soft_armed()) {
294 3495 : if (_calibration == 1 && have_direction) {
295 0 : _direction_driver->calibrate();
296 3495 : } else if (_calibration == 2 && have_speed) {
297 0 : _speed_driver->calibrate();
298 3495 : } else if (_calibration != 0) {
299 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: driver not found");
300 0 : _calibration.set_and_save(0);
301 : }
302 8790 : } else if (_calibration != 0) {
303 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: disarm for cal");
304 0 : _calibration.set_and_save(0);
305 : }
306 :
307 : // read apparent wind speed
308 12285 : if (have_speed) {
309 12285 : _speed_driver->update_speed();
310 : }
311 :
312 : // read apparent wind direction
313 12285 : if (have_direction) {
314 12285 : _direction_driver->update_direction();
315 : }
316 :
317 12285 : if (have_speed && have_direction) {
318 12285 : if (_speed_apparent >= _dir_speed_cutoff) {
319 : // calculate true wind speed and direction from apparent wind
320 12285 : update_true_wind_speed_and_direction();
321 : } else {
322 : // assume true wind has not changed, calculate the apparent wind direction and speed
323 0 : update_apparent_wind_dir_from_true();
324 : }
325 : } else {
326 : // only have direction, can't do true wind calcs, set true direction to apparent + heading
327 0 : _direction_true_raw = wrap_PI(_direction_apparent_raw + AP::ahrs().get_yaw_rad());
328 0 : _speed_true_raw = 0.0f;
329 : }
330 :
331 : /*
332 : Apply filters
333 : https://en.wikipedia.org/wiki/Mean_of_circular_quantities
334 : */
335 : float filtered_sin;
336 : float filtered_cos;
337 :
338 : // apparent speed
339 12285 : if (is_positive(_speed_filt_hz)) {
340 12285 : _speed_apparent_filt.set_cutoff_frequency(_speed_filt_hz);
341 12285 : _speed_apparent = _speed_apparent_filt.apply(_speed_apparent_raw,0.05f);
342 : } else {
343 0 : _speed_apparent = _speed_apparent_raw;
344 : }
345 :
346 : // apparent direction
347 12285 : if (is_positive(_dir_filt_hz)) {
348 12285 : _direction_apparent_sin_filt.set_cutoff_frequency(_dir_filt_hz);
349 12285 : _direction_apparent_cos_filt.set_cutoff_frequency(_dir_filt_hz);
350 12285 : filtered_sin = _direction_apparent_sin_filt.apply(sinf(_direction_apparent_raw),0.05f);
351 12285 : filtered_cos = _direction_apparent_cos_filt.apply(cosf(_direction_apparent_raw),0.05f);
352 12285 : _direction_apparent = atan2f(filtered_sin, filtered_cos);
353 : } else {
354 0 : _direction_apparent = _direction_apparent_raw;
355 : }
356 :
357 : // apparent direction for tack threshold, hard coded freq
358 12285 : filtered_sin = _tack_sin_filt.apply(sinf(_direction_apparent_raw),0.05f);
359 12285 : filtered_cos = _tack_cos_filt.apply(cosf(_direction_apparent_raw),0.05f);
360 12285 : _direction_tack = atan2f(filtered_sin, filtered_cos);
361 12285 : _current_tack = is_negative(_direction_tack) ? Sailboat_Tack::TACK_PORT : Sailboat_Tack::TACK_STARBOARD;
362 :
363 : // true wind direction and speed, both at the same freq
364 12285 : if (is_positive(_true_filt_hz)) {
365 12285 : _speed_true_filt.set_cutoff_frequency(_true_filt_hz);
366 12285 : _direction_true_sin_filt.set_cutoff_frequency(_true_filt_hz);
367 12285 : _direction_true_cos_filt.set_cutoff_frequency(_true_filt_hz);
368 :
369 12285 : _speed_true = _speed_true_filt.apply(_speed_true_raw,0.05f);
370 12285 : filtered_sin = _direction_true_sin_filt.apply(sinf(_direction_true_raw),0.05f);
371 12285 : filtered_cos = _direction_true_cos_filt.apply(cosf(_direction_true_raw),0.05f);
372 12285 : _direction_true = atan2f(filtered_sin, filtered_cos);
373 : } else {
374 0 : _speed_true = _speed_true_raw;
375 0 : _direction_true = _direction_true_raw;
376 : }
377 :
378 : #if HAL_LOGGING_ENABLED
379 : // @LoggerMessage: WIND
380 : // @Description: Windvane readings
381 : // @Field: TimeUS: Time since system startup
382 : // @Field: DrRaw: raw apparent wind direction direct from sensor, in body-frame
383 : // @Field: DrApp: Apparent wind direction, in body-frame
384 : // @Field: DrTru: True wind direction
385 : // @Field: SpdRaw: raw wind speed direct from sensor
386 : // @Field: SpdApp: Apparent wind Speed
387 : // @Field: SpdTru: True wind speed
388 24570 : AP::logger().WriteStreaming("WIND", "TimeUS,DrRaw,DrApp,DrTru,SpdRaw,SpdApp,SpdTru",
389 : "sddhnnn", "F000000", "Qffffff",
390 : AP_HAL::micros64(),
391 12285 : degrees(_direction_apparent_raw),
392 12285 : degrees(_direction_apparent),
393 12285 : degrees(_direction_true),
394 12285 : _speed_apparent_raw,
395 12285 : _speed_apparent,
396 12285 : _speed_true);
397 : #endif
398 :
399 : }
400 :
401 113 : void AP_WindVane::record_home_heading()
402 : {
403 113 : _home_heading = AP::ahrs().get_yaw_rad();
404 113 : }
405 :
406 : // to start direction calibration from mavlink or other
407 0 : bool AP_WindVane::start_direction_calibration()
408 : {
409 0 : if (enabled() && (_calibration == 0)) {
410 0 : _calibration.set(1);
411 0 : return true;
412 : }
413 0 : return false;
414 : }
415 :
416 : // to start speed calibration from mavlink or other
417 0 : bool AP_WindVane::start_speed_calibration()
418 : {
419 0 : if (enabled() && (_calibration == 0)) {
420 0 : _calibration.set(2);
421 0 : return true;
422 : }
423 0 : return false;
424 : }
425 :
426 : // send mavlink wind message
427 124899 : void AP_WindVane::send_wind(mavlink_channel_t chan) const
428 : {
429 : // exit immediately if not enabled
430 124899 : if (!enabled()) {
431 120045 : return;
432 : }
433 :
434 : // send wind
435 4854 : mavlink_msg_wind_send(
436 : chan,
437 : wrap_360(degrees(get_true_wind_direction_rad())),
438 : get_true_wind_speed(),
439 : 0);
440 :
441 : // send apparent wind using named floats
442 : // TODO: create a dedicated MAVLink message
443 4854 : gcs().send_named_float("AppWndSpd", get_apparent_wind_speed());
444 4854 : gcs().send_named_float("AppWndDir", degrees(get_apparent_wind_direction_rad()));
445 :
446 : }
447 :
448 : // calculate true wind speed and direction from apparent wind
449 : // https://en.wikipedia.org/wiki/Apparent_wind
450 12285 : void AP_WindVane::update_true_wind_speed_and_direction()
451 : {
452 : // if no vehicle speed, can't do calcs
453 12285 : Vector3f veh_velocity;
454 12285 : if (!AP::ahrs().get_velocity_NED(veh_velocity)) {
455 : // if no vehicle speed use apparent speed and direction directly
456 327 : _direction_true_raw = _direction_apparent_raw;
457 327 : _speed_true_raw = _speed_apparent;
458 327 : return;
459 : }
460 :
461 : // convert apparent wind speed and direction to 2D vector in same frame as vehicle velocity
462 11958 : const float wind_dir_180 = _direction_apparent_raw + AP::ahrs().get_yaw_rad() + radians(180);
463 11958 : const Vector2f wind_apparent_vec(cosf(wind_dir_180) * _speed_apparent, sinf(wind_dir_180) * _speed_apparent);
464 :
465 : // add vehicle velocity
466 11958 : Vector2f wind_true_vec = Vector2f(wind_apparent_vec.x + veh_velocity.x, wind_apparent_vec.y + veh_velocity.y);
467 :
468 : // calculate true speed and direction
469 11958 : _direction_true_raw = wrap_PI(atan2f(wind_true_vec.y, wind_true_vec.x) - radians(180));
470 11958 : _speed_true_raw = wind_true_vec.length();
471 : }
472 :
473 : // apparent wind is low, can't trust sensors, assume true wind has not changed and calculate apparent wind
474 0 : void AP_WindVane::update_apparent_wind_dir_from_true()
475 : {
476 : // if no vehicle speed, can't do calcs
477 0 : Vector3f veh_velocity;
478 0 : if (!AP::ahrs().get_velocity_NED(veh_velocity)) {
479 0 : return;
480 : }
481 :
482 : // convert true wind speed and direction to 2D vector in same frame as vehicle velocity
483 0 : const float wind_dir_180 = _direction_true + radians(180);
484 0 : const Vector2f wind_true_vec(cosf(wind_dir_180) * _speed_true, sinf(wind_dir_180) * _speed_true);
485 :
486 : // add vehicle velocity
487 0 : Vector2f wind_apparent_vec = Vector2f(wind_true_vec.x - veh_velocity.x, wind_true_vec.y - veh_velocity.y);
488 :
489 : // calculate apartment speed and direction
490 0 : _direction_apparent_raw = wrap_PI(atan2f(wind_apparent_vec.y, wind_apparent_vec.x) - radians(180) - AP::ahrs().get_yaw_rad());
491 0 : _speed_apparent_raw = wind_apparent_vec.length();
492 : }
493 :
494 : AP_WindVane *AP_WindVane::_singleton = nullptr;
495 :
496 : namespace AP {
497 0 : AP_WindVane *windvane()
498 : {
499 0 : return AP_WindVane::get_singleton();
500 : }
501 : };
502 :
503 : #endif // AP_WINDVANE_ENABLED
|
