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_ANALOG_ENABLED
      19             : 
      20             : #include "AP_WindVane_Analog.h"
      21             : 
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <GCS_MAVLink/GCS.h>
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27             : #define WINDVANE_CALIBRATION_VOLT_DIFF_MIN  1.0f    // calibration routine's min voltage difference required for success
      28             : 
      29             : #include <AP_HAL/AP_HAL.h>
      30             : 
      31             : extern const AP_HAL::HAL& hal;
      32             : 
      33             : // constructor
      34           0 : AP_WindVane_Analog::AP_WindVane_Analog(AP_WindVane &frontend) :
      35           0 :     AP_WindVane_Backend(frontend)
      36             : {
      37           0 :     _dir_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
      38           0 : }
      39             : 
      40           0 : void AP_WindVane_Analog::update_direction()
      41             : {
      42           0 :     if (!_dir_analog_source->set_pin(_frontend._dir_analog_pin)) {
      43             :         // pin invalid, don't have health monitoring to report yet
      44           0 :         return;
      45             :     }
      46           0 :     _current_analog_voltage = _dir_analog_source->voltage_latest();
      47             : 
      48           0 :     const float voltage_ratio = linear_interpolate(0.0f, 1.0f, _current_analog_voltage, _frontend._dir_analog_volt_min, _frontend._dir_analog_volt_max);
      49           0 :     const float direction = (voltage_ratio * radians(360 - _frontend._dir_analog_deadzone)) + radians(_frontend._dir_analog_bearing_offset);
      50             : 
      51           0 :     _frontend._direction_apparent_raw  = wrap_PI(direction);
      52             : }
      53             : 
      54           0 : void AP_WindVane_Analog::calibrate()
      55             : {
      56             : 
      57             :     // start calibration
      58           0 :     if (_cal_start_ms == 0) {
      59           0 :         _cal_start_ms = AP_HAL::millis();
      60           0 :         _cal_volt_max = _current_analog_voltage;
      61           0 :         _cal_volt_min = _current_analog_voltage;
      62           0 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: Calibration started, rotate wind vane");
      63             :     }
      64             : 
      65             :     // record min and max voltage
      66           0 :     _cal_volt_max = MAX(_cal_volt_max, _current_analog_voltage);
      67           0 :     _cal_volt_min = MIN(_cal_volt_min, _current_analog_voltage);
      68             : 
      69             :     // calibrate for 30 seconds
      70           0 :     if ((AP_HAL::millis() - _cal_start_ms) > 30000) {
      71             :         // check for required voltage difference
      72           0 :         const float volt_diff = _cal_volt_max - _cal_volt_min;
      73           0 :         if (volt_diff >= WINDVANE_CALIBRATION_VOLT_DIFF_MIN) {
      74             :             // save min and max voltage
      75           0 :             _frontend._dir_analog_volt_max.set_and_save(_cal_volt_max);
      76           0 :             _frontend._dir_analog_volt_min.set_and_save(_cal_volt_min);
      77           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: Calibration complete (volt min:%.1f max:%1.f)",
      78             :             (double)_cal_volt_min,
      79             :             (double)_cal_volt_max);
      80             :         } else {
      81           0 :              GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: Calibration failed (volt diff %.1f below %.1f)",
      82             :             (double)volt_diff,
      83             :             (double)WINDVANE_CALIBRATION_VOLT_DIFF_MIN);
      84             :         }
      85           0 :         _frontend._calibration.set_and_save(0);
      86           0 :         _cal_start_ms = 0;
      87             :     }
      88           0 : }
      89             : 
      90             : #endif  // AP_WINDVANE_ANALOG_ENABLED