MavlinkSonarArray/qwiicMux.cpp at master · devbender/MavlinkSonarArray · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
/*  ======================================================================
 *  *** Mavlink Sonar Array ***
 *
 *  File: qwiicMux.cpp
 *  Description: Qwiic Mux class file
 *
 *  MIT License
 *  Copyright (c) 2021 Juan Benitez <juan.a.benitez(a)gmail.com>
 *
 *  ======================================================================
 */

#include "qwiicMux.h"

boolean qwiicMux::init() {
  uint8_t write = 0;
  Wire.beginTransmission(deviceAddress);
  Wire.write(write);
  Wire.endTransmission();

  Wire.requestFrom(deviceAddress, 1);
  if (Wire.available()) {
    if (Wire.read() != 0)
      return false;
  }

  setPortState(0xA4);
  uint8_t response = getPortState();
  setPortState(0x00);

  if (response == 0xA4)
    return true;
   else
    return false;
}


uint8_t qwiicMux::setPort(uint8_t portNumber) {
  uint8_t portValue = 0;

  if (portNumber > 7)
    portValue = 0;
  else
    portValue = 1 << portNumber;

  Wire.beginTransmission(deviceAddress);
  Wire.write(portValue);
  Wire.endTransmission();

  return getPort();
}


void qwiicMux::setPortState(uint8_t portBits) {
  Wire.beginTransmission(deviceAddress);
  Wire.write(portBits);
  Wire.endTransmission();
}


int qwiicMux::getPort() {

  uint8_t portBits;

  Wire.requestFrom(deviceAddress, 1);
  if (Wire.available()) {
    portBits = Wire.read();

    for (uint8_t x = 0; x < 8; x++) {
      if (portBits & (1 << x))
        return x;
    }
  }
  return -1;
}


uint8_t qwiicMux::getPortState() {
  uint8_t portBits = 0;

  Wire.requestFrom(deviceAddress, 1);
  if (Wire.available())
    portBits = Wire.read();

  return portBits;
}


void qwiicMux::enablePort(uint8_t portNumber) {
  if (portNumber > 7) portNumber = 7;

  uint8_t settings = getPortState();
  settings |= (1 << portNumber);

  setPortState(settings);
}


void qwiicMux::disablePort(uint8_t portNumber) {
  if (portNumber > 7) portNumber = 7;

  uint8_t settings = getPortState();
  settings &= ~(1 << portNumber);

  setPortState(settings);
}