farmbot_os/lib/farmbot/firmware/firmware.ex

594 lines
19 KiB
Elixir

defmodule Farmbot.Firmware do
@moduledoc "Allows communication with the firmware."
use GenStage
use Farmbot.Logger
alias Farmbot.Firmware.{Vec3, EstopTimer}
import Farmbot.System.ConfigStorage,
only: [get_config_value: 3, update_config_value: 4, get_config_as_map: 0]
# If any command takes longer than this, exit.
@call_timeout 500_000
@doc "Move the bot to a position."
def move_absolute(%Vec3{} = vec3, x_spd, y_spd, z_spd) do
call = {:move_absolute, [vec3, x_spd, y_spd, z_spd]}
GenStage.call(__MODULE__, call, @call_timeout)
end
@doc "Calibrate an axis."
def calibrate(axis) do
GenStage.call(__MODULE__, {:calibrate, [axis]}, @call_timeout)
end
@doc "Find home on an axis."
def find_home(axis) do
GenStage.call(__MODULE__, {:find_home, [axis]}, @call_timeout)
end
@doc "Home every axis."
def home_all() do
GenStage.call(__MODULE__, {:home_all, []}, @call_timeout)
end
@doc "Home an axis."
def home(axis) do
GenStage.call(__MODULE__, {:home, [axis]}, @call_timeout)
end
@doc "Manually set an axis's current position to zero."
def zero(axis) do
GenStage.call(__MODULE__, {:zero, [axis]}, @call_timeout)
end
@doc """
Update a paramater.
For a list of paramaters see `Farmbot.Firmware.Gcode.Param`
"""
def update_param(param, val) do
GenStage.call(__MODULE__, {:update_param, [param, val]}, @call_timeout)
end
@doc false
def read_all_params do
GenStage.call(__MODULE__, {:read_all_params, []}, @call_timeout)
end
@doc """
Read a paramater.
For a list of paramaters see `Farmbot.Firmware.Gcode.Param`
"""
def read_param(param) do
GenStage.call(__MODULE__, {:read_param, [param]}, @call_timeout)
end
@doc "Emergency lock Farmbot."
def emergency_lock() do
GenStage.call(__MODULE__, {:emergency_lock, []}, @call_timeout)
end
@doc "Unlock Farmbot from Emergency state."
def emergency_unlock() do
GenStage.call(__MODULE__, {:emergency_unlock, []}, @call_timeout)
end
@doc "Set a pin mode (:input | :output)"
def set_pin_mode(pin, mode) do
GenStage.call(__MODULE__, {:set_pin_mode, [pin, mode]}, @call_timeout)
end
@doc "Read a pin."
def read_pin(pin, mode) do
GenStage.call(__MODULE__, {:read_pin, [pin, mode]}, @call_timeout)
end
@doc "Write a pin."
def write_pin(pin, mode, value) do
GenStage.call(__MODULE__, {:write_pin, [pin, mode, value]}, @call_timeout)
end
@doc "Request version."
def request_software_version do
GenStage.call(__MODULE__, {:request_software_version, []}, @call_timeout)
end
@doc "Set angle of a servo pin."
def set_servo_angle(pin, value) do
GenStage.call(__MODULE__, {:set_servo_angle, [pin, value]}, @call_timeout)
end
@doc "Flag for all params reported."
def params_reported do
GenStage.call(__MODULE__, :params_reported)
end
@doc "Start the firmware services."
def start_link do
GenStage.start_link(__MODULE__, [], name: __MODULE__)
end
## GenStage
defmodule Command do
@moduledoc false
defstruct [
fun: nil,
args: nil,
from: nil
]
defimpl Inspect, for: __MODULE__ do
def inspect(obj, _) do
"#{obj.fun}(#{Enum.join(obj.args, ", ")})"
end
end
end
defmodule State do
@moduledoc false
defstruct [
handler: nil,
handler_mod: nil,
idle: false,
timer: nil,
pins: %{},
params: %{},
params_reported: false,
initialized: false,
initializing: false,
current: nil,
timeout_ms: 150_000,
queue: :queue.new()
]
end
def init([]) do
handler_mod =
Application.get_env(:farmbot, :behaviour)[:firmware_handler] || raise("No fw handler.")
case handler_mod.start_link() do
{:ok, handler} ->
Process.flag(:trap_exit, true)
{
:producer_consumer,
%State{handler: handler, handler_mod: handler_mod},
subscribe_to: [handler], dispatcher: GenStage.BroadcastDispatcher
}
{:error, reason} ->
old = Application.get_all_env(:farmbot)[:behaviour]
new = Keyword.put(old, :firmware_handler, Farmbot.Firmware.StubHandler)
Application.put_env(:farmbot, :behaviour, new)
{:stop, {:handler_init, reason}}
end
end
def terminate(reason, state) do
unless reason in [:normal, :shutdown] do
old = Application.get_all_env(:farmbot)[:behaviour]
new = Keyword.put(old, :firmware_handler, Farmbot.Firmware.StubHandler)
Application.put_env(:farmbot, :behaviour, new)
end
unless :queue.is_empty(state.queue) do
list = :queue.to_list(state.queue)
for cmd <- list do
:ok = do_reply(%{state | current: cmd}, {:error, reason})
end
end
end
def handle_info({:EXIT, _pid, :normal}, state) do
{:stop, :normal, state}
end
def handle_info({:EXIT, _, reason}, state) do
Logger.error 1, "Firmware handler: #{state.handler_mod} died: #{inspect reason}"
case state.handler_mod.start_link() do
{:ok, handler} ->
new_state = %{state | handler: handler}
{:noreply, [{:informational_settings, %{busy: false}}], %{new_state | initialized: false, idle: false}}
err -> {:stop, err, %{state | handler: false}}
end
end
# TODO(Connor): Put some sort of exit strategy here.
# If a firmware command keeps timingout/failing, Farmbot OS just keeps trying
# it. This can lead to infinate failures.
def handle_info({:command_timeout, timeout_command}, state) do
case state.current do
# Check if this timeout is actually talking about the current command.
^timeout_command = current ->
Logger.warn 1, "Timed out waiting for Firmware response. Retrying #{inspect current}) "
case apply(state.handler_mod, current.fun, [state.handler | current.args]) do
:ok ->
timer = Process.send_after(self(), {:command_timeout, current}, state.timeout_ms)
{:noreply, [], %{state | current: current, timer: timer}}
{:error, _} = res ->
do_reply(state, res)
{:noreply, [], %{state | current: nil, queue: :queue.new()}}
end
# If this timeout was not talking about the current command
%Command{} = current ->
Logger.debug 3, "Got stray timeout for command: #{inspect current}"
{:noreply, [], %{state | timer: nil}}
# If there is no current command, we got a different kind of stray.
# This is ok i guess.
nil -> {:noreply, [], %{state | timer: nil}}
end
end
def handle_call(:params_reported, _, state) do
{:reply, state.params_reported, [], state}
end
def handle_call({fun, _}, _from, state = %{initialized: false})
when fun not in [:read_all_params, :update_param, :emergency_unlock, :emergency_lock] do
{:reply, {:error, :uninitialized}, [], state}
end
def handle_call({fun, args}, from, state) do
next_current = struct(Command, from: from, fun: fun, args: args)
current_current = state.current
cond do
fun == :emergency_lock ->
if current_current do
do_reply(state, {:error, :emergency_lock})
end
do_begin_cmd(next_current, state, [])
match?(%Command{}, current_current) ->
do_queue_cmd(next_current, state)
is_nil(current_current) ->
do_begin_cmd(next_current, state, [])
end
end
defp do_begin_cmd(%Command{fun: fun, args: args, from: _from} = current, state, dispatch) do
# Logger.busy 3, "FW Starting: #{fun}: #{inspect from}"
case apply(state.handler_mod, fun, [state.handler | args]) do
:ok ->
timer = Process.send_after(self(), {:command_timeout, current}, state.timeout_ms)
if fun == :emergency_unlock do
Farmbot.System.GPIO.Leds.led_status_ok()
new_dispatch = [{:informational_settings, %{busy: false, locked: false}} | dispatch]
{:noreply, new_dispatch, %{state | current: current, timer: timer}}
else
{:noreply, dispatch, %{state | current: current, timer: timer}}
end
{:error, _} = res ->
do_reply(%{state | current: current}, res)
{:noreply, dispatch, %{state | current: nil}}
end
end
defp do_queue_cmd(%Command{fun: _fun, args: _args, from: _from} = current, state) do
# Logger.busy 3, "FW Queuing: #{fun}: #{inspect from}"
new_q = :queue.in(current, state.queue)
{:noreply, [], %{state | queue: new_q}}
end
def handle_events(gcodes, _from, state) do
{diffs, state} = handle_gcodes(gcodes, state)
# if after handling the current buffer of gcodes,
# Try to start the next command in the queue if it exists.
if List.last(gcodes) == :idle && state.current == nil do
case :queue.out(state.queue) do
{{:value, next_current}, new_queue} ->
do_begin_cmd(next_current, %{state | queue: new_queue, current: next_current}, diffs)
{:empty, queue} -> # nothing to do if the queue is empty.
{:noreply, diffs, %{state | queue: queue}}
end
else
{:noreply, diffs, state}
end
end
defp handle_gcodes(codes, state, acc \\ [])
defp handle_gcodes([], state, acc), do: {Enum.reverse(acc), state}
defp handle_gcodes([code | rest], state, acc) do
case handle_gcode(code, state) do
{nil, new_state} -> handle_gcodes(rest, new_state, acc)
{key, diff, new_state} -> handle_gcodes(rest, new_state, [{key, diff} | acc])
end
end
defp handle_gcode({:debug_message, message}, state) do
if get_config_value(:bool, "settings", "arduino_debug_messages") do
Logger.debug 3, "Arduino debug message: #{message}"
end
{nil, state}
end
defp handle_gcode(code, state) when code in [:error, :invalid_command] do
Logger.warn 1, "Got error gcode (#{code})!"
maybe_cancel_timer(state.timer, state.current)
if state.current do
formatted_args = Enum.map(state.current.args, fn(arg) ->
cond do
is_atom(arg) -> to_string(arg)
is_binary(arg) -> to_string(arg)
true -> inspect(arg)
end
end)
Logger.error 1, "Failed to execute #{state.current.fun} #{inspect formatted_args}"
do_reply(state, {:error, :firmware_error})
{nil, %{state | current: nil}}
else
{nil, state}
end
end
defp handle_gcode({:report_current_position, x, y, z}, state) do
{:location_data, %{position: %{x: round(x), y: round(y), z: round(z)}}, state}
end
defp handle_gcode({:report_encoder_position_scaled, x, y, z}, state) do
{:location_data, %{scaled_encoders: %{x: x, y: y, z: z}}, state}
end
defp handle_gcode({:report_encoder_position_raw, x, y, z}, state) do
{:location_data, %{raw_encoders: %{x: x, y: y, z: z}}, state}
end
defp handle_gcode({:report_end_stops, xa, xb, ya, yb, za, zb}, state) do
diff = %{end_stops: %{xa: xa, xb: xb, ya: ya, yb: yb, za: za, zb: zb}}
{:location_data, diff, state}
{nil, state}
end
defp handle_gcode({:report_pin_mode, pin, mode_atom}, state) do
# Logger.debug 3, "Got pin mode report: #{pin}: #{mode_atom}"
mode = if(mode_atom == :digital, do: 0, else: 1)
case state.pins[pin] do
%{mode: _, value: _} = pin_map ->
{:pins, %{pin => %{pin_map | mode: mode}}, %{state | pins: %{state.pins | pin => %{pin_map | mode: mode}}}}
nil ->
{:pins, %{pin => %{mode: mode, value: -1}}, %{state | pins: Map.put(state.pins, pin, %{mode: mode, value: -1})}}
end
end
defp handle_gcode({:report_pin_value, pin, value}, state) do
# Logger.debug 3, "Got pin value report: #{pin}: #{value} old: #{inspect state.pins[pin]}"
case state.pins[pin] do
%{mode: _, value: _} = pin_map ->
{:pins, %{pin => %{pin_map | value: value}}, %{state | pins: %{state.pins | pin => %{pin_map | value: value}}}}
nil ->
{:pins, %{pin => %{mode: nil, value: value}}, %{state | pins: Map.put(state.pins, pin, %{mode: nil, value: value})}}
end
end
defp handle_gcode({:report_parameter_value, param, value}, state) when (value == -1) do
maybe_update_param_from_report(to_string(param), nil)
{:mcu_params, %{param => nil}, %{state | params: Map.put(state.params, param, value)}}
end
defp handle_gcode({:report_parameter_value, param, value}, state) when is_number(value) do
maybe_update_param_from_report(to_string(param), value)
{:mcu_params, %{param => value}, %{state | params: Map.put(state.params, param, value)}}
end
defp handle_gcode(:idle, %{initialized: false, initializing: false} = state) do
Logger.busy 1, "Initializing Firmware."
old = get_config_as_map()["hardware_params"]
case old["param_version"] do
nil ->
Logger.debug 3, "Setting up fresh params."
spawn __MODULE__, :do_read_params_and_report_position, [%{}]
_ ->
Logger.debug 3, "Setting up old params."
spawn __MODULE__, :do_read_params_and_report_position, [Map.delete(old, "param_version")]
end
{nil, %{state | initializing: true}}
end
defp handle_gcode(:idle, %{initialized: false, initializing: true} = state) do
{nil, state}
end
defp handle_gcode(:idle, state) do
maybe_cancel_timer(state.timer, state.current)
Farmbot.BotState.set_busy(false)
if state.current do
# This might be a bug in the FW
if state.current.fun in [:home, :home_all] do
Logger.warn 1, "Got idle during home. Ignoring. This might be bad."
timer = Process.send_after(self(), {:command_timeout, state.current}, state.timeout_ms)
{nil, %{state | timer: timer}}
else
Logger.warn 1, "Got idle while executing a command."
do_reply(state, {:error, :timeout})
{:informational_settings, %{busy: false, locked: false}, %{state | current: nil, idle: true}}
end
else
{:informational_settings, %{busy: false, locked: false}, %{state | idle: true}}
end
end
defp handle_gcode(:report_params_complete, state) do
Logger.success 1, "Firmware initialized."
{nil, %{state | initializing: false, initialized: true, params_reported: true}}
end
defp handle_gcode({:report_software_version, version}, state) do
case String.last(version) do
"F" ->
update_config_value(:string, "settings", "firmware_hardware", "farmduino")
"R" ->
update_config_value(:string, "settings", "firmware_hardware", "arduino")
"G" ->
update_config_value(:string, "settings", "firmware_hardware", "farmduino_v14")
_ -> :ok
end
{:informational_settings, %{firmware_version: version}, state}
end
defp handle_gcode(:report_axis_home_complete_x, state) do
Logger.success 2, "X Axis homing complete."
{nil, state}
end
defp handle_gcode(:report_axis_home_complete_y, state) do
Logger.success 2, "Y Axis homing complete."
{nil, state}
end
defp handle_gcode(:report_axis_home_complete_z, state) do
Logger.success 2, "Z Axis homing complete."
{nil, state}
end
defp handle_gcode(:busy, state) do
Farmbot.BotState.set_busy(true)
maybe_cancel_timer(state.timer, state.current)
timer = Process.send_after(self(), {:command_timeout, state.current}, state.timeout_ms)
{:informational_settings, %{busy: true}, %{state | idle: false, timer: timer}}
end
defp handle_gcode(:done, state) do
maybe_cancel_timer(state.timer, state.current)
Farmbot.BotState.set_busy(false)
if state.current do
do_reply(state, :ok)
{nil, %{state | current: nil}}
else
{nil, state}
end
end
defp handle_gcode(:report_emergency_lock, state) do
Farmbot.System.GPIO.Leds.led_status_err
maybe_send_email()
if state.current do
do_reply(state, {:error, :emergency_lock})
{:informational_settings, %{locked: true}, %{state | current: nil}}
else
{:informational_settings, %{locked: true}, state}
end
end
defp handle_gcode({:report_calibration, axis, status}, state) do
maybe_cancel_timer(state.timer, state.current)
Logger.busy 1, "Axis #{axis} calibration: #{status}"
{nil, state}
end
defp handle_gcode({:report_axis_calibration, param, val}, state) do
spawn __MODULE__, :report_calibration_callback, [5, param, val]
{nil, state}
end
defp handle_gcode(:noop, state) do
{nil, state}
end
defp handle_gcode(:received, state) do
{nil, state}
end
defp handle_gcode({:echo, _code}, state) do
{nil, state}
end
defp handle_gcode(code, state) do
Logger.warn(3, "unhandled code: #{inspect(code)}")
{nil, state}
end
defp maybe_cancel_timer(nil, current_command) do
if current_command do
Logger.debug 3, "[WEIRD] - No timer to cancel for command: #{inspect current_command}"
:ok
else
Logger.debug 3, "[PROBABLY OK] - No timer to cancel, and no command here."
:ok
end
end
defp maybe_cancel_timer(timer, current_command) do
if Process.read_timer(timer) do
Logger.debug 3, "[NORMAL] - Canceled timer: #{inspect timer} for command: #{inspect current_command}"
Process.cancel_timer(timer)
:ok
else
:ok
end
end
defp maybe_update_param_from_report(param, val) when is_binary(param) do
real_val = if val, do: (val / 1), else: nil
Logger.debug 3, "Firmware reported #{param} => #{val || -1}"
update_config_value(:float, "hardware_params", to_string(param), real_val)
end
@doc false
def do_read_params_and_report_position(old) when is_map(old) do
for {key, float_val} <- old do
cond do
(float_val == -1) -> :ok
is_nil(float_val) -> :ok
is_number(float_val) ->
val = round(float_val)
update_param(:"#{key}", val)
end
end
update_param(:param_config_ok, 1)
update_param(:param_use_eeprom, 0)
read_all_params()
request_software_version()
end
@doc false
def report_calibration_callback(tries, param, value)
def report_calibration_callback(0, _param, _value) do
:ok
end
def report_calibration_callback(tries, param, val) do
case Farmbot.Firmware.update_param(param, val) do
:ok ->
case get_config_value(:float, "hardware_params", to_string(param)) do
^val ->
Logger.success 1, "Calibrated #{param}: #{val}"
:ok
_ -> report_calibration_callback(tries - 1, param, val)
end
{:error, reason} ->
Logger.error 1, "Failed to set #{param}: #{val} (#{inspect reason})"
report_calibration_callback(tries - 1, param, val)
end
end
defp do_reply(state, reply) do
maybe_cancel_timer(state.timer, state.current)
case state.current do
%Command{fun: :emergency_unlock, from: from} ->
# i really don't want this to be here..
EstopTimer.cancel_timer()
:ok = GenServer.reply from, reply
%Command{fun: :emergency_lock, from: from} ->
:ok = GenServer.reply from, {:error, :emergency_lock}
%Command{fun: _fun, from: from} ->
# Logger.success 3, "FW Replying: #{fun}: #{inspect from}"
:ok = GenServer.reply from, reply
nil ->
Logger.error 1, "FW Nothing to send reply: #{inspect reply} to!."
:error
end
end
defp maybe_send_email do
import Farmbot.System.ConfigStorage, only: [get_config_value: 3]
if get_config_value(:bool, "settings", "email_on_estop") do
if !EstopTimer.timer_active? do
EstopTimer.start_timer()
end
end
end
end