Line data    Source code

       1             : #include "AP_Airspeed_MSP.h"
       2             : 
       3             : #if AP_AIRSPEED_MSP_ENABLED
       4             : 
       5           0 : AP_Airspeed_MSP::AP_Airspeed_MSP(AP_Airspeed &_frontend, uint8_t _instance, uint8_t _msp_instance) :
       6             :     AP_Airspeed_Backend(_frontend, _instance),
       7           0 :     msp_instance(_msp_instance)
       8             : {
       9           0 :     set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_MSP,0,msp_instance,0));
      10           0 : }
      11             : 
      12             : // return the current differential_pressure in Pascal
      13           0 : bool AP_Airspeed_MSP::get_differential_pressure(float &pressure)
      14             : {
      15           0 :     WITH_SEMAPHORE(sem);
      16           0 :     if (press_count == 0) {
      17           0 :         return false;
      18             :     }
      19           0 :     pressure = sum_pressure/press_count;
      20           0 :     press_count = 0;
      21           0 :     sum_pressure = 0;
      22           0 :     return true;
      23           0 : }
      24             : 
      25             : // get last temperature
      26           0 : bool AP_Airspeed_MSP::get_temperature(float &temperature)
      27             : {
      28           0 :     WITH_SEMAPHORE(sem);
      29           0 :     if (temp_count == 0) {
      30           0 :         return false;
      31             :     }
      32           0 :     temperature = sum_temp/temp_count;
      33           0 :     temp_count = 0;
      34           0 :     sum_temp = 0;
      35           0 :     return true;
      36           0 : }
      37             : 
      38           0 : void AP_Airspeed_MSP::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
      39             : {
      40           0 :     if (pkt.instance != msp_instance) {
      41             :         // not for us
      42           0 :         return;
      43             :     }
      44           0 :     WITH_SEMAPHORE(sem);
      45             : 
      46           0 :     sum_pressure += pkt.pressure;
      47           0 :     press_count++;
      48           0 :     if (press_count > 100) {
      49             :         // prevent overflow
      50           0 :         sum_pressure /= 2;
      51           0 :         press_count /= 2;
      52             :     }
      53             : 
      54           0 :     if (pkt.temp == INT16_MIN) {
      55             :         // invalid temperature
      56           0 :         return;
      57             :     }
      58             : 
      59           0 :     sum_temp += pkt.temp*0.01;
      60           0 :     temp_count++;
      61           0 :     if (temp_count > 100) {
      62             :         // prevent overflow
      63           0 :         sum_temp /= 2;
      64           0 :         temp_count /= 2;
      65             :     }
      66             :     
      67           0 : }
      68             : 
      69             : #endif // AP_AIRSPEED_MSP_ENABLED