AP_Airspeed_MS5525.h 2.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778
  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. This program is distributed in the hope that it will be useful,
  7. but WITHOUT ANY WARRANTY; without even the implied warranty of
  8. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  9. GNU General Public License for more details.
  10. You should have received a copy of the GNU General Public License
  11. along with this program. If not, see <http://www.gnu.org/licenses/>.
  12. */
  13. #pragma once
  14. /*
  15. backend driver for airspeed from I2C
  16. */
  17. #include <AP_HAL/AP_HAL.h>
  18. #include <AP_Param/AP_Param.h>
  19. #include <AP_HAL/utility/OwnPtr.h>
  20. #include <AP_HAL/I2CDevice.h>
  21. #include <utility>
  22. #include "AP_Airspeed_Backend.h"
  23. class AP_Airspeed_MS5525 : public AP_Airspeed_Backend
  24. {
  25. public:
  26. enum MS5525_ADDR {
  27. MS5525_ADDR_1 = 0,
  28. MS5525_ADDR_2 = 1,
  29. MS5525_ADDR_AUTO = 255, // does not need to be 255, just needs to be out of the address array
  30. };
  31. AP_Airspeed_MS5525(AP_Airspeed &frontend, uint8_t _instance, MS5525_ADDR address);
  32. ~AP_Airspeed_MS5525(void) {}
  33. // probe and initialise the sensor
  34. bool init() override;
  35. // return the current differential_pressure in Pascal
  36. bool get_differential_pressure(float &pressure) override;
  37. // return the current temperature in degrees C, if available
  38. bool get_temperature(float &temperature) override;
  39. private:
  40. void measure();
  41. void collect();
  42. void timer();
  43. bool read_prom(void);
  44. uint16_t crc4_prom(void);
  45. int32_t read_adc();
  46. void calculate();
  47. float pressure;
  48. float temperature;
  49. float temperature_sum;
  50. float pressure_sum;
  51. uint32_t temp_count;
  52. uint32_t press_count;
  53. uint32_t last_sample_time_ms;
  54. uint16_t prom[8];
  55. uint8_t state;
  56. int32_t D1;
  57. int32_t D2;
  58. uint32_t command_send_us;
  59. bool ignore_next;
  60. uint8_t cmd_sent;
  61. MS5525_ADDR _address;
  62. AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
  63. };