init: inital commit

lib/localiser/localisation/sensor/server.ex

@@ -0,0 +1,170 @@
+defmodule Localiser.Localisation.Sensor.Server do
+  use GenServer
+
+  require Logger
+
+  alias Localiser.Domain.Sensors
+  alias Localiser.Domain.Schema.{Sensor, SensorCalibration}
+  alias Localiser.MQTT.Connection, as: MQTTConnection
+
+  @default_rssi_ref -59
+  @default_path_loss_exp 2.0
+
+  # mode: :ok | {:calibrating, buffer :: [integer()], target :: pos_integer()}
+  defstruct [:sensor_id, :sensor_db_id, :floor_x, :floor_y, :rssi_ref, :path_loss_exp, mode: :ok]
+
+  def start_link({sensor, room}) do
+    GenServer.start_link(__MODULE__, {sensor, room}, name: via(sensor.sensor_id))
+  end
+
+  def via(sensor_id) do
+    {:via, Registry, {Localiser.Registry, {:sensor, sensor_id}}}
+  end
+
+  # Returns %{sensor_id, floor_x, floor_y, distance} for a raw RSSI reading.
+  def measure(sensor_id, rssi) do
+    GenServer.call(via(sensor_id), {:measure, rssi})
+  end
+
+  # Returns true if the sensor is currently collecting calibration samples.
+  def calibrating?(sensor_id) do
+    GenServer.call(via(sensor_id), :calibrating?)
+  end
+
+  # Feeds a raw RSSI value into the calibration buffer.
+  def calibration_reading(sensor_id, rssi) do
+    GenServer.cast(via(sensor_id), {:calibration_reading, rssi})
+  end
+
+  # Starts calibration mode. sample_target: number of RSSI samples to collect.
+  def begin_calibration(sensor_id, sample_target \\ 50) do
+    GenServer.cast(via(sensor_id), {:begin_calibration, sample_target})
+  end
+
+  # Aborts an in-progress calibration without saving.
+  def abort_calibration(sensor_id) do
+    GenServer.cast(via(sensor_id), :abort_calibration)
+  end
+
+  @impl true
+  def init({sensor, room}) do
+    Phoenix.PubSub.subscribe(Localiser.PubSub, "sensors")
+
+    calibration = Sensors.latest_calibration(sensor)
+    {rssi_ref, path_loss_exp} = calibration_params(calibration)
+
+    state = %__MODULE__{
+      sensor_id: sensor.sensor_id,
+      sensor_db_id: sensor.id,
+      floor_x: (room.offset_x || 0.0) + (sensor.x || 0.0),
+      floor_y: (room.offset_y || 0.0) + (sensor.y || 0.0),
+      rssi_ref: rssi_ref,
+      path_loss_exp: path_loss_exp
+    }
+
+    {:ok, state}
+  end
+
+  @impl true
+  def handle_call({:measure, rssi}, _from, state) do
+    distance = rssi_to_distance(rssi, state.rssi_ref, state.path_loss_exp)
+
+    measurement = %{
+      sensor_id: state.sensor_id,
+      floor_x: state.floor_x,
+      floor_y: state.floor_y,
+      distance: distance
+    }
+
+    {:reply, measurement, state}
+  end
+
+  @impl true
+  def handle_call(:calibrating?, _from, state) do
+    {:reply, match?({:calibrating, _, _}, state.mode), state}
+  end
+
+  @impl true
+  def handle_cast({:begin_calibration, target}, state) do
+    MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_start"}))
+    {:noreply, %{state | mode: {:calibrating, [], target}}}
+  end
+
+  @impl true
+  def handle_cast(:abort_calibration, %{mode: {:calibrating, _, _}} = state) do
+    MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
+    {:noreply, %{state | mode: :ok}}
+  end
+
+  def handle_cast(:abort_calibration, state), do: {:noreply, state}
+
+  @impl true
+  def handle_cast({:calibration_reading, rssi}, %{mode: {:calibrating, buffer, target}} = state) do
+    buffer = [rssi | buffer]
+
+    if length(buffer) >= target do
+      finalize_calibration(buffer, state)
+    else
+      {:noreply, %{state | mode: {:calibrating, buffer, target}}}
+    end
+  end
+
+  def handle_cast({:calibration_reading, _rssi}, state), do: {:noreply, state}
+
+  # Position updated (sensor dragged in layout).
+  @impl true
+  def handle_info({:sensor_enrolled, %Sensor{sensor_id: sid} = sensor, room}, %{sensor_id: sid} = state) do
+    floor_x = (room.offset_x || 0.0) + (sensor.x || 0.0)
+    floor_y = (room.offset_y || 0.0) + (sensor.y || 0.0)
+    {:noreply, %{state | floor_x: floor_x, floor_y: floor_y}}
+  end
+
+  # Ignore PubSub messages not relevant to this server.
+  def handle_info(_msg, state), do: {:noreply, state}
+
+  # Private
+
+  defp finalize_calibration(buffer, state) do
+    rssi_ref = median(buffer)
+    sensor_struct = %Sensor{id: state.sensor_db_id, sensor_id: state.sensor_id}
+
+    case Sensors.add_calibration(sensor_struct, %{
+           rssi_ref: rssi_ref,
+           path_loss_exp: state.path_loss_exp,
+           calibrated_at: DateTime.utc_now()
+         }) do
+      {:ok, _calibration} ->
+        Logger.info("[Sensor.Server] Calibration complete for #{state.sensor_id}: rssi_ref=#{rssi_ref}")
+        MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
+        Phoenix.PubSub.broadcast(Localiser.PubSub, "sensors", {:calibration_complete, state.sensor_id})
+        {:noreply, %{state | rssi_ref: rssi_ref, mode: :ok}}
+
+      {:error, reason} ->
+        Logger.error("[Sensor.Server] Failed to save calibration for #{state.sensor_id}: #{inspect(reason)}")
+        {:noreply, %{state | mode: :ok}}
+    end
+  end
+
+  defp median(list) do
+    sorted = Enum.sort(list)
+    len = length(sorted)
+    mid = div(len, 2)
+
+    if rem(len, 2) == 0 do
+      round((Enum.at(sorted, mid - 1) + Enum.at(sorted, mid)) / 2)
+    else
+      Enum.at(sorted, mid)
+    end
+  end
+
+  # d = 10 ^ ((rssi_ref - rssi) / (10 * n))
+  defp rssi_to_distance(rssi, rssi_ref, path_loss_exp) do
+    :math.pow(10.0, (rssi_ref - rssi) / (10.0 * path_loss_exp))
+  end
+
+  defp calibration_params(nil), do: {@default_rssi_ref, @default_path_loss_exp}
+
+  defp calibration_params(%SensorCalibration{rssi_ref: rssi_ref, path_loss_exp: path_loss_exp}) do
+    {rssi_ref, path_loss_exp}
+  end
+end