AnalogIn_Navio2.h 1.0 KB

123456789101112131415161718192021222324252627282930313233343536373839
  1. #pragma once
  2. #include "AP_HAL_Linux.h"
  3. #include <fcntl.h>
  4. #define NAVIO_ADC_MAX_CHANNELS 6
  5. class AnalogSource_Navio2: public AP_HAL::AnalogSource {
  6. public:
  7. friend class AnalogIn_Navio2;
  8. AnalogSource_Navio2(uint8_t pin);
  9. float read_average() override;
  10. float read_latest() override;
  11. void set_pin(uint8_t p) override;
  12. float voltage_average() override;
  13. float voltage_latest() override;
  14. float voltage_average_ratiometric() override;
  15. private:
  16. void set_channel(uint8_t pin);
  17. uint8_t _pin;
  18. int _fd = -1;
  19. float _value = 0.0f;
  20. };
  21. class AnalogIn_Navio2: public AP_HAL::AnalogIn {
  22. public:
  23. AnalogIn_Navio2();
  24. void init() override;
  25. AP_HAL::AnalogSource *channel(int16_t n) override;
  26. float board_voltage(void) override;
  27. float servorail_voltage(void) override;
  28. private:
  29. AnalogSource_Navio2 *_channels[NAVIO_ADC_MAX_CHANNELS];
  30. uint8_t _channels_number = NAVIO_ADC_MAX_CHANNELS;
  31. AP_HAL::AnalogSource *_board_voltage_pin = nullptr;
  32. AP_HAL::AnalogSource *_servorail_pin = nullptr;
  33. };