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_MODERNDEVICE_ENABLED
      19             : 
      20             : #include "AP_WindVane_ModernDevice.h"
      21             : // read wind speed from Modern Device rev p wind sensor
      22             : // https://moderndevice.com/news/calibrating-rev-p-wind-sensor-new-regression/
      23             : 
      24             : #include <AP_HAL/AP_HAL.h>
      25             : #include <GCS_MAVLink/GCS.h>
      26             : 
      27             : extern const AP_HAL::HAL& hal;
      28             : 
      29             : // constructor
      30           0 : AP_WindVane_ModernDevice::AP_WindVane_ModernDevice(AP_WindVane &frontend) :
      31           0 :     AP_WindVane_Backend(frontend)
      32             : {
      33           0 :     _speed_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
      34           0 :     _temp_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
      35           0 : }
      36             : 
      37           0 : void AP_WindVane_ModernDevice::update_speed()
      38             : {
      39           0 :     float analog_voltage = 0.0f;
      40             : 
      41             :     // only read temp pin if defined, sensor will do OK assuming constant temp
      42           0 :     float temp_ambient = 28.0f; // equations were generated at this temp in above data sheet
      43           0 :     if (is_positive(_frontend._speed_sensor_temp_pin.get())) {
      44           0 :         if (!_temp_analog_source->set_pin(_frontend._speed_sensor_temp_pin.get())) {
      45             :             // pin invalid, don't have health monitoring to report yet
      46           0 :             return;
      47             :         }
      48           0 :         analog_voltage = _temp_analog_source->voltage_average();
      49           0 :         temp_ambient = (analog_voltage - 0.4f) / 0.0195f; // deg C
      50             :         // constrain to reasonable range to avoid deviating from calibration too much and potential divide by zero
      51           0 :         temp_ambient = constrain_float(temp_ambient, 10.0f, 40.0f);
      52             :     }
      53             : 
      54           0 :     if (!_speed_analog_source->set_pin(_frontend._speed_sensor_speed_pin.get())) {
      55             :         // pin invalid, don't have health monitoring to report yet
      56           0 :         return;
      57             :     }
      58           0 :     _current_analog_voltage = _speed_analog_source->voltage_average();
      59             : 
      60             :     // apply voltage offset and make sure not negative
      61             :     // by default the voltage offset is the number provide by the manufacturer
      62           0 :     analog_voltage = _current_analog_voltage - _frontend._speed_sensor_voltage_offset;
      63           0 :     if (is_negative(analog_voltage)) {
      64           0 :         analog_voltage = 0.0f;
      65             :     }
      66             : 
      67             :     // simplified equation from data sheet, converted from mph to m/s
      68           0 :     _frontend._speed_apparent_raw  = 24.254896f * powf((analog_voltage / powf(temp_ambient, 0.115157f)), 3.009364f);
      69             : }
      70             : 
      71           0 : void AP_WindVane_ModernDevice::calibrate()
      72             : {
      73           0 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: rev P. zero wind voltage offset set to %.1f",double(_current_analog_voltage));
      74           0 :     _frontend._speed_sensor_voltage_offset.set_and_save(_current_analog_voltage);
      75           0 :     _frontend._calibration.set_and_save(0);
      76           0 : }
      77             : 
      78             : #endif  // AP_WINDVANE_MODERNDEVICE_ENABLED