1  #include "wind_speed.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::WindSpeed) {
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_speed::access(WindSpeedSensorData *sensor_data) {
33    if (!sensor_data) return false;
34    sensor_data->kilometers_per_hour = NAN;
35    sensor_data->ok = false;
36    const config::ModbusSensorConfig *sensor_config = access_config();
37    if (!sensor_config) return false;
38  
39    uint16_t output_words[1] = {0};
40    ReadHoldingRegistersCommand command = {
41      .channel = channel_from_config(sensor_config),
42      .slave_id = sensor_config->slave_id,
43      .start_register = sensor_config->register_address,
44      .register_count = 1,
45      .output_words = output_words,
46      .error = ModbusError::NotInitialized,
47    };
48  
49    if (!networking::modbus::readHoldingRegisters(&command)) return false;
50  
51    sensor_data->kilometers_per_hour = (output_words[0] * 3.6f) / 10.0f;
52    sensor_data->ok = true;
53    return true;
54  }
55  
56  bool sensors::wind_speed::initialize() {
57    if (!access_config()) {
58      available = false;
59      return true;
60    }
61    WindSpeedSensorData sensor_data = {};
62    available = sensors::wind_speed::access(&sensor_data);
63    if (available) {
64      sensors::registry::add({
65          .kind = SensorKind::WindSpeed,
66          .name = "Wind Speed",
67          .isAvailable = sensors::wind_speed::isAvailable,
68          .instanceCount = []() -> uint8_t { return 1; },
69          .poll = [](uint8_t, void *out, size_t cap) -> bool {
70              if (cap < sizeof(WindSpeedSensorData)) return false;
71              return sensors::wind_speed::access(
72                  static_cast<WindSpeedSensorData *>(out));
73          },
74          .data_size = sizeof(WindSpeedSensorData),
75      });
76    }
77    return available;
78  }
79  
80  bool sensors::wind_speed::isAvailable() {
81    return available;
82  }