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.h"
      17             : 
      18             : #if AP_TEMPERATURE_SENSOR_ENABLED
      19             : #include "AP_TemperatureSensor_Backend.h"
      20             : 
      21             : #include <AP_Logger/AP_Logger.h>
      22             : #include <AP_BattMonitor/AP_BattMonitor.h>
      23             : #include <AP_Servo_Telem/AP_Servo_Telem.h>
      24             : #include <AP_RangeFinder/AP_RangeFinder.h>
      25             : 
      26             : /*
      27             :   All backends use the same parameter table and set of indices. Therefore, two
      28             :   backends must not use the same index. The list of used indices and
      29             :   corresponding backends is below.
      30             : 
      31             :     1:   AP_TemperatureSensor_DroneCAN.cpp, note there is a clash with analog, but due to different param types we get away with it
      32             :     1-7: AP_TemperatureSensor_Analog.cpp
      33             :     8-9: AP_TemperatureSensor_MAX31865.cpp
      34             : 
      35             :   Usage does not need to be contiguous. The maximum possible index is 63.
      36             : */
      37             : 
      38             : /*
      39             :     base class constructor.
      40             :     This incorporates initialisation as well.
      41             : */
      42          89 : AP_TemperatureSensor_Backend::AP_TemperatureSensor_Backend(AP_TemperatureSensor &front,
      43             :                                                             AP_TemperatureSensor::TemperatureSensor_State &state,
      44          89 :                                                             AP_TemperatureSensor_Params &params):
      45          89 :     _front(front),
      46          89 :     _state(state),
      47          89 :     _params(params)
      48             : {
      49          89 : }
      50             : 
      51             : // returns true if a temperature has been recently updated
      52       52012 : bool AP_TemperatureSensor_Backend::healthy(void) const
      53             : {
      54       52012 :     return (_state.last_time_ms > 0) && (AP_HAL::millis() - _state.last_time_ms < 5000);
      55             : }
      56             : 
      57             : #if HAL_LOGGING_ENABLED
      58         193 : void AP_TemperatureSensor_Backend::Log_Write_TEMP() const
      59             : {
      60             :     // @LoggerMessage: TEMP
      61             :     // @Description: Temperature Sensor Data
      62             :     // @Field: TimeUS: Time since system startup
      63             :     // @Field: Instance: temperature sensor instance
      64             :     // @Field: Temp: temperature
      65         386 :     AP::logger().Write("TEMP",
      66             :             "TimeUS,"     "Instance,"       "Temp" , // labels
      67             :             "s"               "#"           "O"    , // units
      68             :             "F"               "-"           "0"    , // multipliers
      69             :             "Q"               "B"           "f"    , // types
      70         193 :      AP_HAL::micros64(), _state.instance, _state.temperature);
      71         193 : }
      72             : #endif
      73             : 
      74      109699 : void AP_TemperatureSensor_Backend::set_temperature(const float temperature)
      75             : {
      76             :     {
      77      109699 :         WITH_SEMAPHORE(_sem);
      78      109699 :         _state.temperature = temperature;
      79      109699 :         _state.last_time_ms = AP_HAL::millis();
      80      109699 :     }
      81             : 
      82      109699 :     update_external_libraries(temperature);
      83      109699 : }
      84             : 
      85      109699 : void AP_TemperatureSensor_Backend::update_external_libraries(const float temperature)
      86             : {
      87             : #if HAL_WITH_ESC_TELEM
      88             :     AP_ESC_Telem_Backend::TelemetryData t;
      89             : #endif
      90             : #if AP_SERVO_TELEM_ENABLED
      91             :     AP_Servo_Telem *servo_telem;
      92             :     AP_Servo_Telem::TelemetryData servo_telem_data;
      93             : #endif
      94             : 
      95      109699 :     switch ((AP_TemperatureSensor_Params::Source)_params.source.get()) {
      96             : #if HAL_WITH_ESC_TELEM
      97           0 :         case AP_TemperatureSensor_Params::Source::ESC:
      98           0 :             t.temperature_cdeg = temperature * 100;
      99           0 :             update_telem_data(_params.source_id-1, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL);
     100           0 :             break;
     101             : 
     102           0 :         case AP_TemperatureSensor_Params::Source::Motor:
     103           0 :             t.motor_temp_cdeg = temperature * 100;
     104           0 :             update_telem_data(_params.source_id-1, t, AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL);
     105           0 :             break;
     106             : #endif
     107             : 
     108             : #if AP_BATTERY_ENABLED
     109           0 :         case AP_TemperatureSensor_Params::Source::Battery_Index:
     110           0 :             AP::battery().set_temperature(temperature, _params.source_id-1);
     111           0 :             break;
     112           0 :         case AP_TemperatureSensor_Params::Source::Battery_ID_SerialNumber:
     113           0 :             AP::battery().set_temperature_by_serial_number(temperature, _params.source_id);
     114           0 :             break;
     115             : #endif
     116           0 :         case AP_TemperatureSensor_Params::Source::DroneCAN:
     117             :             // Label only, used by AP_Periph
     118           0 :             break;
     119             : 
     120             : #if AP_SERVO_TELEM_ENABLED
     121           0 :         case AP_TemperatureSensor_Params::Source::Servo_Motor:
     122           0 :             servo_telem = AP_Servo_Telem::get_singleton();
     123           0 :             if (servo_telem == nullptr) {
     124           0 :                 break;
     125             :             }
     126           0 :             servo_telem_data.motor_temperature_cdeg = temperature * 100;
     127           0 :             servo_telem_data.present_types = AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP;
     128           0 :             servo_telem->update_telem_data(_params.source_id-1, servo_telem_data);
     129           0 :             break;
     130             : 
     131           0 :         case AP_TemperatureSensor_Params::Source::Servo_PCB:
     132           0 :             servo_telem = AP_Servo_Telem::get_singleton();
     133           0 :             if (servo_telem == nullptr) {
     134           0 :                 break;
     135             :             }
     136           0 :             servo_telem_data.pcb_temperature_cdeg = temperature * 100;
     137           0 :             servo_telem_data.present_types = AP_Servo_Telem::TelemetryData::Types::PCB_TEMP;
     138           0 :             servo_telem->update_telem_data(_params.source_id-1, servo_telem_data);
     139           0 :             break;
     140             : #endif // AP_SERVO_TELEM_ENABLED
     141             : 
     142             : #if AP_RANGEFINDER_ENABLED
     143         409 :         case AP_TemperatureSensor_Params::Source::Rangefinder: {
     144         409 :             RangeFinder *rangefinder = AP::rangefinder();
     145         409 :             if (rangefinder == nullptr) {
     146           0 :                 break;
     147             :             }
     148         409 :             rangefinder->set_temperature_C(_params.source_id-1, temperature);
     149         409 :             break;
     150             :         }
     151             : #endif
     152             : 
     153      109290 :         case AP_TemperatureSensor_Params::Source::None:
     154             :         case AP_TemperatureSensor_Params::Source::Pitot_tube:
     155             :         default:
     156      109290 :             break;
     157             :     }
     158             : 
     159      109699 : }
     160             : 
     161             : #endif // AP_TEMPERATURE_SENSOR_ENABLED