1  #include "wind_direction.h"
 2  #include "registry.h"
 3  
 4  #include <config.h>
 5  #include "../hardware/rs485.h"
 6  #include "../networking/modbus.h"
 7  
 8  #include <math.h>
 9  
10  namespace {
11  
12  bool available = false;
13  
14  const config::ModbusSensorConfig *access_config() {
15    for (size_t index = 0; index < config::modbus::DEVICE_COUNT; index++) {
16      const config::ModbusSensorConfig &sensor_config = config::modbus::DEVICES[index];
17      if (sensor_config.kind == config::ModbusSensorKind::WindDirection) {
18        return &sensor_config;
19      }
20    }
21    return nullptr;
22  }
23  
24  hardware::rs485::Channel channel_from_config(const config::ModbusSensorConfig *sensor_config) {
25    return sensor_config && sensor_config->channel == 0
26        ? hardware::rs485::Channel::Bus0
27        : hardware::rs485::Channel::Bus1;
28  }
29  
30  }
31  
32  bool sensors::wind_direction::access(WindDirectionSensorData *sensor_data) {
33    if (!sensor_data) return false;
34    sensor_data->degrees = NAN;
35    sensor_data->slice = 0xFF;
36    sensor_data->ok = false;
37    const config::ModbusSensorConfig *sensor_config = access_config();
38    if (!sensor_config) return false;
39  
40    uint16_t output_words[2] = {0, 0};
41    ReadHoldingRegistersCommand command = {
42      .channel = channel_from_config(sensor_config),
43      .slave_id = sensor_config->slave_id,
44      .start_register = sensor_config->register_address,
45      .register_count = 2,
46      .output_words = output_words,
47      .error = ModbusError::NotInitialized,
48    };
49  
50    if (!networking::modbus::readHoldingRegisters(&command)) return false;
51    if (output_words[1] > 15) return false;
52  
53    sensor_data->degrees = output_words[0] / 10.0f;
54    sensor_data->slice = static_cast<uint8_t>(output_words[1]);
55    sensor_data->ok = true;
56    return true;
57  }
58  
59  bool sensors::wind_direction::initialize() {
60    if (!access_config()) {
61      available = false;
62      return true;
63    }
64    WindDirectionSensorData sensor_data = {};
65    available = sensors::wind_direction::access(&sensor_data);
66    if (available) {
67      sensors::registry::add({
68          .kind = SensorKind::WindDirection,
69          .name = "Wind Direction",
70          .isAvailable = sensors::wind_direction::isAvailable,
71          .instanceCount = []() -> uint8_t { return 1; },
72          .poll = [](uint8_t, void *out, size_t cap) -> bool {
73              if (cap < sizeof(WindDirectionSensorData)) return false;
74              return sensors::wind_direction::access(
75                  static_cast<WindDirectionSensorData *>(out));
76          },
77          .data_size = sizeof(WindDirectionSensorData),
78      });
79    }
80    return available;
81  }
82  
83  bool sensors::wind_direction::isAvailable() {
84    return available;
85  }