AP_Baro_FBM320.h 1.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748
  1. #pragma once
  2. #include <AP_HAL/AP_HAL.h>
  3. #include <AP_HAL/Device.h>
  4. #include <AP_HAL/utility/OwnPtr.h>
  5. #include "AP_Baro_Backend.h"
  6. #ifndef HAL_BARO_FBM320_I2C_ADDR
  7. #define HAL_BARO_FBM320_I2C_ADDR 0x6C
  8. #endif
  9. #ifndef HAL_BARO_FBM320_I2C_ADDR2
  10. #define HAL_BARO_FBM320_I2C_ADDR2 0x6D
  11. #endif
  12. class AP_Baro_FBM320 : public AP_Baro_Backend {
  13. public:
  14. AP_Baro_FBM320(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
  15. /* AP_Baro public interface: */
  16. void update() override;
  17. static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
  18. private:
  19. bool init(void);
  20. bool read_calibration(void);
  21. void timer(void);
  22. void calculate_PT(int32_t UT, int32_t UP, int32_t &pressure, int32_t &temperature);
  23. AP_HAL::OwnPtr<AP_HAL::Device> dev;
  24. uint8_t instance;
  25. uint32_t count;
  26. float pressure_sum;
  27. float temperature_sum;
  28. uint8_t step;
  29. int32_t value_T;
  30. // Internal calibration registers
  31. struct fbm320_calibration {
  32. uint16_t C0, C1, C2, C3, C6, C8, C9, C10, C11, C12;
  33. uint32_t C4, C5, C7;
  34. } calibration;
  35. };