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_TemperatureSensor_config.h"
      17             : 
      18             : #if AP_TEMPERATURE_SENSOR_ANALOG_ENABLED
      19             : 
      20             : #include "AP_TemperatureSensor_Analog.h"
      21             : 
      22             : 
      23             : extern const AP_HAL::HAL &hal;
      24             : 
      25             : const AP_Param::GroupInfo AP_TemperatureSensor_Analog::var_info[] = {
      26             : 
      27             :     // @Param: PIN
      28             :     // @DisplayName: Temperature sensor analog voltage sensing pin
      29             :     // @Description: Sets the analog input pin that should be used for temprature monitoring. Values for some autopilots are given as examples. Search wiki for "Analog pins".
      30             :     // @Values: -1:Disabled, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 5:Navigator, 13:Pixhawk2_PM2/CubeOrange_PM2, 14:CubeOrange, 16:Durandal, 100:PX4-v1
      31             :     // @Range: -1 127
      32             :     // @User: Standard
      33             :     AP_GROUPINFO("PIN", 1, AP_TemperatureSensor_Analog, _pin, -1),
      34             : 
      35             :     // @Param: A0
      36             :     // @DisplayName: Temperature sensor analog 0th polynomial coefficient
      37             :     // @Description: a0 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5
      38             :     AP_GROUPINFO("A0", 2, AP_TemperatureSensor_Analog, _a[0], 0),
      39             : 
      40             :     // @Param: A1
      41             :     // @DisplayName: Temperature sensor analog 1st polynomial coefficient
      42             :     // @Description: a1 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5
      43             :     AP_GROUPINFO("A1", 3, AP_TemperatureSensor_Analog, _a[1], 0),
      44             : 
      45             :     // @Param: A2
      46             :     // @DisplayName: Temperature sensor analog 2nd polynomial coefficient
      47             :     // @Description: a2 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5
      48             :     AP_GROUPINFO("A2", 4, AP_TemperatureSensor_Analog, _a[2], 0),
      49             : 
      50             :     // @Param: A3
      51             :     // @DisplayName: Temperature sensor analog 3rd polynomial coefficient
      52             :     // @Description: a3 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5
      53             :     AP_GROUPINFO("A3", 5, AP_TemperatureSensor_Analog, _a[3], 0),
      54             : 
      55             :     // @Param: A4
      56             :     // @DisplayName: Temperature sensor analog 4th polynomial coefficient
      57             :     // @Description: a4 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5
      58             :     AP_GROUPINFO("A4", 6, AP_TemperatureSensor_Analog, _a[4], 0),
      59             : 
      60             :     // @Param: A5
      61             :     // @DisplayName: Temperature sensor analog 5th polynomial coefficient
      62             :     // @Description: a5 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5
      63             :     AP_GROUPINFO("A5", 7, AP_TemperatureSensor_Analog, _a[5], 0),
      64             : 
      65             :     // CHECK/UPDATE INDEX TABLE IN AP_TemperatureSensor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
      66             : 
      67             :     AP_GROUPEND
      68             : };
      69             : 
      70           0 : AP_TemperatureSensor_Analog::AP_TemperatureSensor_Analog(AP_TemperatureSensor &front,
      71             :                                                          AP_TemperatureSensor::TemperatureSensor_State &state,
      72           0 :                                                          AP_TemperatureSensor_Params &params) :
      73           0 :     AP_TemperatureSensor_Backend(front, state, params)
      74             : {
      75           0 :     AP_Param::setup_object_defaults(this, var_info);
      76           0 :     _state.var_info = var_info;
      77           0 :     _analog_source = hal.analogin->channel(_pin);
      78           0 : }
      79             : 
      80             : // Update function called at 5Hz
      81           0 : void AP_TemperatureSensor_Analog::update()
      82             : {
      83           0 :     if ((_analog_source == nullptr) || !_analog_source->set_pin(_pin)) {
      84             :         // Invalid pln
      85           0 :         return;
      86             :     }
      87             : 
      88             :     // Use ratiometric voltage, measured voltage is relative to supply
      89           0 :     const float voltage = _analog_source->voltage_average_ratiometric();
      90             : 
      91             :     // Evaluate polynomial
      92             :     // temperature (deg) = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5
      93           0 :     float temp = 0.0;
      94           0 :     float poly = 1.0;
      95           0 :     for (uint8_t i = 0; i < ARRAY_SIZE(_a); i++) {
      96           0 :         temp += _a[i] * poly;
      97           0 :         poly *= voltage;
      98             :     }
      99             : 
     100             :     // update state
     101           0 :     set_temperature(temp);
     102             : }
     103             : 
     104             : #endif // AP_TEMPERATURE_SENSOR_ANALOG_ENABLED
     105             :