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_SIM_ENABLED
      19             : 
      20             : #include "AP_WindVane_SITL.h"
      21             : 
      22             : #include <SITL/SITL.h>
      23             : #include <AP_AHRS/AP_AHRS.h>
      24             : 
      25       12285 : void AP_WindVane_SITL::update_direction()
      26             : {
      27       12285 :     if (_frontend._direction_type == _frontend.WindVaneType::WINDVANE_SITL_TRUE) {
      28             :         // read in the true wind direction and calculate the apparent
      29             : 
      30             :         // temporarily store true speed and direction for easy access
      31       12285 :         const float wind_speed = AP::sitl()->wind_speed_active;
      32       12285 :         const float wind_dir_rad = radians(AP::sitl()->wind_direction_active);
      33             : 
      34             :         // Note than the SITL wind direction is defined as the direction the wind is traveling to
      35             :         // This is accounted for in these calculations
      36             : 
      37             :         // convert true wind speed and direction into a 2D vector
      38       12285 :         Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
      39             : 
      40             :         // add vehicle speed to get apparent wind vector
      41       12285 :         wind_vector_ef.x += AP::sitl()->state.speedN;
      42       12285 :         wind_vector_ef.y += AP::sitl()->state.speedE;
      43             : 
      44       12285 :         _frontend._direction_apparent_raw =  wrap_PI(atan2f(wind_vector_ef.y, wind_vector_ef.x) - AP::ahrs().get_yaw_rad());
      45             : 
      46             :     } else { // WINDVANE_SITL_APARRENT
      47             :         // directly read the body frame apparent wind set by physics backend
      48           0 :         _frontend._direction_apparent_raw =  wrap_PI(AP::sitl()->get_apparent_wind_dir());
      49             :     }
      50             : 
      51       12285 : }
      52             : 
      53       12285 : void AP_WindVane_SITL::update_speed()
      54             : {
      55       12285 :     if (_frontend._speed_sensor_type == _frontend.Speed_type::WINDSPEED_SITL_TRUE) {
      56             :         // read in the true wind direction and calculate the apparent
      57             : 
      58             :         // temporarily store true speed and direction for easy access
      59       12285 :         const float wind_speed = AP::sitl()->wind_speed_active;
      60       12285 :         const float wind_dir_rad = radians(AP::sitl()->wind_direction_active);
      61             : 
      62             :         // convert true wind speed and direction into a 2D vector
      63       12285 :         Vector2f wind_vector_ef(cosf(wind_dir_rad) * wind_speed, sinf(wind_dir_rad) * wind_speed);
      64             : 
      65             :         // add vehicle speed to get apparent wind vector
      66       12285 :         wind_vector_ef.x += AP::sitl()->state.speedN;
      67       12285 :         wind_vector_ef.y += AP::sitl()->state.speedE;
      68             : 
      69       12285 :         _frontend._speed_apparent_raw = wind_vector_ef.length();
      70             : 
      71             :     } else { // WINDSPEED_SITL_APARRENT
      72             :         // directly read the apparent wind from as set by physics backend
      73           0 :         _frontend._speed_apparent_raw = AP::sitl()->get_apparent_wind_spd();
      74             :     }
      75       12285 : }
      76             : #endif  // AP_WINDVANE_SIM_ENABLED