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             : #pragma once
      16             : 
      17             : #include "AP_WindVane_config.h"
      18             : 
      19             : #if AP_WINDVANE_ENABLED
      20             : 
      21             : #include <AP_Param/AP_Param.h>
      22             : #include <Filter/Filter.h>
      23             : #include <GCS_MAVLink/GCS_MAVLink.h>
      24             : 
      25             : #ifndef WINDVANE_DEFAULT_PIN
      26             : #define WINDVANE_DEFAULT_PIN -1                     // default wind vane sensor analog pin
      27             : #endif
      28             : #ifndef WINDSPEED_DEFAULT_SPEED_PIN
      29             : #define WINDSPEED_DEFAULT_SPEED_PIN -1              // default pin for reading speed from ModernDevice rev p wind sensor
      30             : #endif
      31             : #ifndef WINDSPEED_DEFAULT_TEMP_PIN
      32             : #define WINDSPEED_DEFAULT_TEMP_PIN -1               // default pin for reading temperature from ModernDevice rev p wind sensor
      33             : #endif
      34             : #define WINDSPEED_DEFAULT_VOLT_OFFSET 1.346f        // default voltage offset between speed and temp pins from ModernDevice rev p wind sensor
      35             : 
      36             : class AP_WindVane_Backend;
      37             : 
      38             : class AP_WindVane
      39             : {
      40             :     friend class AP_WindVane_Backend;
      41             :     friend class AP_WindVane_Home;
      42             :     friend class AP_WindVane_Analog;
      43             :     friend class AP_WindVane_SITL;
      44             :     friend class AP_WindVane_ModernDevice;
      45             :     friend class AP_WindVane_Airspeed;
      46             :     friend class AP_WindVane_RPM;
      47             :     friend class AP_WindVane_NMEA;
      48             : 
      49             : public:
      50             :     AP_WindVane();
      51             : 
      52             :     /* Do not allow copies */
      53             :     CLASS_NO_COPY(AP_WindVane);
      54             : 
      55             :     static AP_WindVane *get_singleton();
      56             : 
      57             :     // return true if wind vane is enabled
      58             :     bool enabled() const;
      59             : 
      60             :     // return true if wind speed is enabled
      61             :     bool wind_speed_enabled() const;
      62             : 
      63             :     // Initialize the Wind Vane object and prepare it for use
      64             :     void init(const class AP_SerialManager& serial_manager);
      65             : 
      66             :     // update wind vane
      67             :     void update();
      68             : 
      69             :     // get the apparent wind direction in body-frame in radians, 0 = head to wind
      70       22563 :     float get_apparent_wind_direction_rad() const { return _direction_apparent; }
      71             : 
      72             :     // get the true wind direction in radians, 0 = wind coming from north
      73       24567 :     float get_true_wind_direction_rad() const { return _direction_true; }
      74             : 
      75             :     // Return apparent wind speed
      76        4854 :     float get_apparent_wind_speed() const { return _speed_apparent; }
      77             : 
      78             :     // Return true wind speed
      79        4854 :     float get_true_wind_speed() const { return _speed_true; }
      80             : 
      81             :     // Return the apparent wind angle used to determin the current tack
      82       20419 :     float get_tack_threshold_wind_dir_rad() const { return _direction_tack; }
      83             : 
      84             :     // enum defining current tack
      85             :     enum Sailboat_Tack {
      86             :         TACK_PORT,
      87             :         TACK_STARBOARD
      88             :     };
      89             : 
      90             :     // return the current tack
      91       23447 :     Sailboat_Tack get_current_tack() const { return _current_tack; }
      92             : 
      93             :     // record home heading for use as wind direction if no sensor is fitted
      94             :     void record_home_heading();
      95             : 
      96             :     // start calibration routine
      97             :     bool start_direction_calibration();
      98             :     bool start_speed_calibration();
      99             : 
     100             :     // send mavlink wind message
     101             :     void send_wind(mavlink_channel_t chan) const;
     102             : 
     103             :     // parameter block
     104             :     static const struct AP_Param::GroupInfo var_info[];
     105             : 
     106             : private:
     107             : 
     108             :     // parameters
     109             :     AP_Int8 _direction_type;                        // type of windvane being used
     110             :     AP_Int8 _dir_analog_pin;                        // analog pin connected to wind vane direction sensor
     111             :     AP_Float _dir_analog_volt_min;                  // minimum voltage read by windvane
     112             :     AP_Float _dir_analog_volt_max;                  // maximum voltage read by windvane
     113             :     AP_Float _dir_analog_bearing_offset;            // angle offset when windvane is indicating a headwind, ie 0 degress relative to vehicle
     114             :     AP_Float _dir_analog_deadzone;                  // analog pot deadzone in degrees
     115             :     AP_Float _dir_filt_hz;                          // vane Low pass filter frequency
     116             :     AP_Int8 _calibration;                           // enter calibration
     117             :     AP_Float _dir_speed_cutoff;                     // vane cutoff wind speed
     118             :     AP_Int8 _speed_sensor_type;                     // wind speed sensor type
     119             :     AP_Int8 _speed_sensor_speed_pin;                // speed sensor analog pin for reading speed
     120             :     AP_Float _speed_sensor_temp_pin;                // speed sensor analog pin for reading temp, -1 if disable
     121             :     AP_Float _speed_sensor_voltage_offset;          // analog speed zero wind voltage offset
     122             :     AP_Float _speed_filt_hz;                        // speed sensor low pass filter frequency
     123             :     AP_Float _true_filt_hz;                         // true wind speed and direction low pass filter frequency
     124             : 
     125             :     AP_WindVane_Backend *_direction_driver;
     126             :     AP_WindVane_Backend *_speed_driver;
     127             : 
     128             :     // update wind speed sensor
     129             :     void update_apparent_wind_speed();
     130             : 
     131             :     // update apparent wind direction
     132             :     void update_apparent_wind_direction();
     133             : 
     134             :     // calculate true wind speed and direction from apparent wind
     135             :     void update_true_wind_speed_and_direction();
     136             : 
     137             :     // assume true wind has not changed and calculate apparent wind
     138             :     void update_apparent_wind_dir_from_true();
     139             : 
     140             :     // wind direction variables
     141             :     float _direction_apparent_raw;                  // wind's apparent direction in radians (0 = ahead of vehicle) in body frame
     142             :     float _direction_apparent;                      // wind's apparent direction in radians (0 = ahead of vehicle) in body frame - filtered
     143             :     float _direction_true_raw;                      // wind's true direction in radians (0 = North)
     144             :     float _direction_true;                          // wind's true direction in radians (0 = North) - filtered
     145             :     float _direction_tack;                          // filtered apparent wind used to determin the current tack
     146         285 :     LowPassFilterFloat _direction_apparent_sin_filt{2.0f};
     147         285 :     LowPassFilterFloat _direction_apparent_cos_filt{2.0f};
     148         285 :     LowPassFilterFloat _direction_true_sin_filt{2.0f};
     149         285 :     LowPassFilterFloat _direction_true_cos_filt{2.0f};
     150         285 :     LowPassFilterFloat _tack_sin_filt{0.1f};
     151         285 :     LowPassFilterFloat _tack_cos_filt{0.1f};
     152             : 
     153             :     // wind speed variables
     154             :     float _speed_apparent_raw;                      // wind's apparent speed in m/s
     155             :     float _speed_apparent;                          // wind's apparent speed in m/s - filtered
     156             :     float _speed_true_raw;                          // wind's true estimated speed in m/s
     157             :     float _speed_true;                              // wind's true estimated speed in m/s - filtered
     158         285 :     LowPassFilterFloat _speed_apparent_filt{2.0f};
     159         285 :     LowPassFilterFloat _speed_true_filt{2.0f};
     160             : 
     161             :     // current tack
     162             :     Sailboat_Tack _current_tack;
     163             : 
     164             :     // heading in radians recorded when vehicle was armed
     165             :     float _home_heading;
     166             : 
     167             :     enum WindVaneType {
     168             :         WINDVANE_NONE           = 0,
     169             : #if AP_WINDVANE_HOME_ENABLED
     170             :         WINDVANE_HOME_HEADING   = 1,
     171             :         WINDVANE_PWM_PIN        = 2,
     172             : #endif
     173             : #if AP_WINDVANE_ANALOG_ENABLED
     174             :         WINDVANE_ANALOG_PIN     = 3,
     175             : #endif
     176             : #if AP_WINDVANE_NMEA_ENABLED
     177             :         WINDVANE_NMEA           = 4,
     178             : #endif
     179             : #if AP_WINDVANE_SIM_ENABLED
     180             :         WINDVANE_SITL_TRUE      = 10,
     181             :         WINDVANE_SITL_APPARENT  = 11,
     182             : #endif
     183             :     };
     184             : 
     185             :     enum Speed_type {
     186             :         WINDSPEED_NONE               = 0,
     187             : #if AP_WINDVANE_AIRSPEED_ENABLED
     188             :         WINDSPEED_AIRSPEED           = 1,
     189             : #endif
     190             :         WINDVANE_WIND_SENSOR_REV_P   = 2,
     191             : #if AP_WINDVANE_RPM_ENABLED
     192             :         WINDSPEED_RPM                = 3,
     193             : #endif
     194             : #if AP_WINDVANE_NMEA_ENABLED
     195             :         WINDSPEED_NMEA               = 4,
     196             : #endif
     197             : #if AP_WINDVANE_SIM_ENABLED
     198             :         WINDSPEED_SITL_TRUE          = 10,
     199             :         WINDSPEED_SITL_APPARENT      = 11,
     200             : #endif
     201             :     };
     202             : 
     203             :     static AP_WindVane *_singleton;
     204             : };
     205             : 
     206             : namespace AP {
     207             :     AP_WindVane *windvane();
     208             : };
     209             : 
     210             : #endif  // AP_WINDVANE_ENABLED