Line data    Source code

       1             : #include "AP_Airspeed_External.h"
       2             : 
       3             : #if AP_AIRSPEED_EXTERNAL_ENABLED
       4             : 
       5           0 : AP_Airspeed_External::AP_Airspeed_External(AP_Airspeed &_frontend, uint8_t _instance) :
       6           0 :     AP_Airspeed_Backend(_frontend, _instance)
       7             : {
       8           0 :     set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,0,_instance,0));
       9           0 : }
      10             : 
      11             : // return the current differential_pressure in Pascal
      12           0 : bool AP_Airspeed_External::get_differential_pressure(float &pressure)
      13             : {
      14           0 :     WITH_SEMAPHORE(sem);
      15           0 :     if (press_count == 0) {
      16           0 :         return false;
      17             :     }
      18           0 :     pressure = sum_pressure/press_count;
      19           0 :     press_count = 0;
      20           0 :     sum_pressure = 0;
      21           0 :     return true;
      22           0 : }
      23             : 
      24             : // get last temperature
      25           0 : bool AP_Airspeed_External::get_temperature(float &temperature)
      26             : {
      27           0 :     WITH_SEMAPHORE(sem);
      28           0 :     if (temperature_count == 0) {
      29           0 :         return false;
      30             :     }
      31           0 :     temperature = sum_temperature/temperature_count;
      32           0 :     temperature_count = 0;
      33           0 :     sum_temperature = 0;
      34           0 :     return true;
      35           0 : }
      36             : 
      37           0 : void AP_Airspeed_External::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt)
      38             : {
      39           0 :     WITH_SEMAPHORE(sem);
      40             : 
      41           0 :     sum_pressure += pkt.differential_pressure;
      42           0 :     press_count++;
      43           0 :     if (press_count > 100) {
      44             :         // prevent overflow
      45           0 :         sum_pressure /= 2;
      46           0 :         press_count /= 2;
      47             :     }
      48             : 
      49           0 :     sum_temperature += pkt.temperature;
      50           0 :     temperature_count++;
      51           0 :     if (temperature_count > 100) {
      52             :         // prevent overflow
      53           0 :         sum_temperature /= 2;
      54           0 :         temperature_count /= 2;
      55             :     }
      56           0 : }
      57             : 
      58             : #endif // AP_AIRSPEED_EXTERNAL_ENABLED