diff --git a/include/canopen.hrl b/include/canopen.hrl index cb2fcd3..51cd7a5 100644 --- a/include/canopen.hrl +++ b/include/canopen.hrl @@ -254,10 +254,9 @@ %% -define(COBID_TO_CANID(ID), - if ?is_cobid_extended((ID)) -> - ((ID) band ?COBID_ENTRY_ID_MASK) bor ?CAN_EFF_FLAG; - true -> - ((ID) band ?CAN_SFF_MASK) + case ?is_cobid_extended((ID)) of + true -> ((ID) band ?COBID_ENTRY_ID_MASK) bor ?CAN_EFF_FLAG; + false -> ((ID) band ?CAN_SFF_MASK) end). -define(CANID_TO_COBID(ID), @@ -418,6 +417,7 @@ -type uint32() :: non_neg_integer(). -type nmt_role() :: slave | master | autonomous. +-type nmt_conf() :: default | undefined | string(). -type nid_type() :: nodeid | xnodeid. %% SDO configuration parameters @@ -460,7 +460,7 @@ last ::uint1(), %% Flag indicating if last segment is received node_pid :: pid(), %% Pid of the node client :: term(), %% Delayed gen_server:reply caller - ctx :: record(sdo_ctx), %% general parameters + ctx :: #sdo_ctx{}, %% general parameters buf :: term(), %% Data buffer mref :: term() %% Ref to application }). @@ -530,6 +530,7 @@ toggle = 0, %% Node guard toggle for nodeid xtoggle = 0, %% Node guard toggle for xnodeid nmt_role = autonomous, %% NMT role (master/slav/autonomous) + nmt_conf = default, %% where to read nmt slave config node_guard_timer, %% Node guard supervision of master node_guard_error = false, %% Lost contact with NMT master node_life_time = 0, %% Node guard supervision of master diff --git a/priv/nmt.conf b/priv/nmt.conf new file mode 100644 index 0000000..f4a3e3b --- /dev/null +++ b/priv/nmt.conf @@ -0,0 +1,10 @@ +%% +%% {{nodeid,N}, [{mode, automatic | manual } | +%% {guard_time, GT} | +%% {life_factor, LF} | +%% {heartbeat_time, Time} ]} +%% N must be a number between 1..126 +%% Both GT and LF must be set to activate node guarding +%% mode = automatice only works in supervision = none +%% +{{nodeid,10}, [{mode, automatic}] }. diff --git a/src/co_api.erl b/src/co_api.erl index b8e1a34..4575903 100644 --- a/src/co_api.erl +++ b/src/co_api.erl @@ -140,6 +140,7 @@ {product, integer()} | {revision, integer()} | {nmt_role, nmt_role()} | + {nmt_conf, nmt_conf()} | {supervision, supervision()} | {inact_timeout, timeout() } | {dict, obj_dict()} | @@ -247,6 +248,15 @@ verify_option(nmt_role, NewValue) ok; verify_option(nmt_role, _NewValue) -> {error, "Option nmt_role can only be set to slave/master/autonomous."}; +verify_option(nmt_conf, NewValue) + when NewValue =:= default; + NewValue =:= undefined; + NewValue =:= ""; + is_list(NewValue) -> + ok; +verify_option(nmt_conf, _NewValue) -> + {error, "Option nmt_conf can only be set to default/file-name."}; + verify_option(supervision, NewValue) when NewValue == node_guard; NewValue == heartbeat; diff --git a/src/co_lib.erl b/src/co_lib.erl index adb5096..a77a5e3 100644 --- a/src/co_lib.erl +++ b/src/co_lib.erl @@ -68,8 +68,8 @@ %% Utilities -export([utc_time/0, sec/0, - debug/1]). - + debug/1, + text_expand/2]). %%-------------------------------------------------------------------- %% @doc @@ -1245,3 +1245,89 @@ debug(true) -> ale:trace(on, self(), debug); debug(false) -> ale:trace(off, self(), debug). + +%% +%% Utility to exand environment "variables" in unicode text +%% variables are written as ${var} where var is a encoded atom +%% operating system enviroment is accessed through $(VAR) +%% and application library dir $/app/ +%% +text_expand(Text, Env) when is_list(Text) -> + %% assume unicode character list! + text_expand_(Text, [], Env); +text_expand(Text, Env) when is_binary(Text) -> + %% assume utf8 encoded data! + text_expand_(unicode:characters_to_list(Text), [], Env). + +text_expand_([$$,${|Text], Acc, Env) -> + text_expand_collect_(Text, [], [${,$$], env, Acc, Env); +text_expand_([$$,$(|Text], Acc, Env) -> + text_expand_collect_(Text, [], [$(,$$], shell, Acc, Env); +text_expand_([$$,$/|Text], Acc, Env) -> + text_expand_collect_(Text, [], [$/,$$], lib, Acc, Env); +text_expand_([$\\,C|Text], Acc, Env) -> + text_expand_(Text, [C|Acc], Env); +text_expand_([C|Text], Acc, Env) -> + text_expand_(Text, [C|Acc], Env); +text_expand_([], Acc, _Env) -> + lists:reverse(Acc). + + +text_expand_collect_([$)|Text], Var, _Pre, shell, Acc, Env) -> + case os:getenv(rev_variable(Var)) of + false -> + text_expand_(Text, Acc, Env); + Value -> + Acc1 = lists:reverse(Value, Acc), + text_expand_(Text, Acc1, Env) + end; +text_expand_collect_([$/|Text], Var, _Pre, lib, Acc, Env) -> + try erlang:list_to_existing_atom(rev_variable(Var)) of + App -> + case code:lib_dir(App) of + {error,_} -> + text_expand_(Text, Acc, Env); + Value -> + Acc1 = lists:reverse(Value, Acc), + text_expand_(Text, Acc1, Env) + end + catch + error:_ -> + text_expand_(Text, Acc, Env) + end; +text_expand_collect_([$}|Text], Var, _Pre, env, Acc, Env) -> + try erlang:list_to_existing_atom(rev_variable(Var)) of + Key -> + case lists:keyfind(Key, 1, Env) of + false -> + text_expand_(Text, Acc, Env); + {_,Val} -> + Value = lists:flatten(io_lib:format("~w", [Val])), + Acc1 = lists:reverse(Value, Acc), + text_expand_(Text, Acc1, Env) + end + catch + error:_ -> + text_expand_(Text, Acc, Env) + end; +text_expand_collect_([C|Text], Var, Pre, Shell, Acc, Env) -> + if C >= $a, C =< $z; + C >= $A, C =< $Z; + C >= $0, C =< $9; + C == $_; C == $@; + C == $\s; C == $\t -> %% space and tab allowed in begining and end + text_expand_collect_(Text, [C|Var], Pre, Shell, Acc, Env); + true -> + %% char not allowed in variable named + text_expand_(Text, [C | Var ++ Pre ++ Acc], Env) + end; +text_expand_collect_([], Var, Pre, _Shell, Acc, Env) -> + text_expand_([], Var ++ Pre ++ Acc, Env). + +rev_variable(Var) -> + trim_hd(lists:reverse(trim_hd(Var))). + +trim_hd([$\s|Cs]) -> trim_hd(Cs); +trim_hd([$\t|Cs]) -> trim_hd(Cs); +trim_hd(Cs) -> Cs. + diff --git a/src/co_nmt.erl b/src/co_nmt.erl index f6a5ef6..8914cbc 100644 --- a/src/co_nmt.erl +++ b/src/co_nmt.erl @@ -45,7 +45,8 @@ send_nmt_command/1, send_nmt_command/2, save/0, - load/0]). + load/0, + load/1]). %% gen_server callbacks -export([init/1, @@ -68,6 +69,7 @@ -record(ctx, { supervision = none, %% Type of supervision + conf = default, %% config file location node_pid, %% Pid of co_node dict, %% co_node dictionary subscribers = [], %% Receives error notifications @@ -78,6 +80,7 @@ -record(nmt_slave, { id, %% node id (key) + mode = manual :: manual | auto, guard_time = 0, life_factor = 0, heartbeat_time = 0, @@ -301,6 +304,13 @@ save() -> load() -> gen_server:call(?NMT_MASTER, load). +%%-------------------------------------------------------------------- +%% @doc +%% Restores saved nmt configuration (slaves to supervise). +%% @end +%%-------------------------------------------------------------------- +load(File) -> + gen_server:call(?NMT_MASTER, {load, File}). %%-------------------------------------------------------------------- %% @doc @@ -342,6 +352,7 @@ init(Args) -> %% Trace output enable/disable Dbg = proplists:get_value(debug,Args,false), co_lib:debug(Dbg), + Conf = proplists:get_value(conf,Args,default), case proplists:get_value(node_pid, Args) of NodePid when is_pid(NodePid) -> @@ -353,6 +364,7 @@ init(Args) -> HB = ets:new(co_hb_table, [{keypos,1}, protected, named_table, ordered_set]), Ctx = #ctx {node_pid = NodePid, + conf = Conf, supervision = Supervision, heartbeat_table = HB, nmt_table = NMT}, @@ -360,7 +372,7 @@ init(Args) -> load_conf(Ctx), %% Load heartbeat if needed - if Supervision == heartbeat -> + if Supervision =:= heartbeat -> activate_heartbeat(Ctx); true -> do_nothing @@ -443,7 +455,7 @@ handle_call({add_slave, SlaveId = {_Flag, _NodeId}}, _From, ?dbg("handle_call: add_slave {~p,~.16#}", [_Flag, _NodeId]), case ets:lookup(NmtTable, SlaveId) of [_Slave] -> ok; %% already handled - [] -> add_slave(SlaveId, Ctx) + [] -> add_slave_(SlaveId, Ctx) end, {reply, ok, Ctx}; @@ -474,6 +486,20 @@ handle_call(load, _From, Ctx) -> Reply = load_conf(Ctx), {reply, Reply, Ctx}; +handle_call({load,Conf}, _From, Ctx) -> + File = if Conf =:= default; + Conf =:= undefined; + Conf =:= "" -> + filename:join(code:priv_dir(canopen), ?NMT_CONF); + true -> + co_lib:text_expand(Conf, []) + end, + Ctx1 = Ctx#ctx { conf = File }, + case load_conf(Ctx1) of + ok -> {reply, ok, Ctx1}; + Error -> {reply, Error, Ctx} + end; + handle_call({debug, TrueOrFalse}, _From, Ctx) -> co_lib:debug(TrueOrFalse), co_mgr:debug(TrueOrFalse), @@ -723,18 +749,26 @@ code_change(_OldVsn, Ctx, _Extra) -> %%% Internal functions %%%=================================================================== -load_conf(_Ctx=#ctx {nmt_table = NmtTable}) -> - File = filename:join(code:priv_dir(canopen), ?NMT_CONF), +load_conf(_Ctx=#ctx {nmt_table = NmtTable, conf = Conf}) -> + File = if Conf =:= default; + Conf =:= undefined; + Conf =:= "" -> + filename:join(code:priv_dir(canopen), ?NMT_CONF); + true -> + co_lib:text_expand(Conf, []) + end, ?dbg("load_conf: load nmt configuration from ~p", [File]), case filelib:is_regular(File) of true -> - case file:consult(File) of - {ok, [List]} -> - ?dbg("handle_call: loading list ~p", [List]), - try ets:insert(NmtTable, List) of - true -> ok + case file:open(File, [read]) of + {ok, Fd} -> + try load_nmt_slaves(Fd, NmtTable) of + ok -> ok catch - error:Reason -> {error, Reason} + error:LoadError -> + {error, LoadError} + after + file:close(Fd) end; Error -> Error @@ -743,6 +777,61 @@ load_conf(_Ctx=#ctx {nmt_table = NmtTable}) -> {error, no_nmt_conf_exists} end. +load_nmt_slaves(Fd, NmtTable) -> + case io:read(Fd, '') of + {ok, {ID={Flag,N}, SlaveConf}} when Flag =:= nodeid, + is_integer(N), N > 0, + N < 16#7f; + Flag =:= xnodeid, + is_integer(N), N > 0, + N < 16#01FFFFFF -> + case ets:lookup(NmtTable, ID) of + [] -> + Slave = update_slave(#nmt_slave { id=ID }, SlaveConf), + ets:insert(NmtTable, Slave), + update_slave_mode(Slave, NmtTable); + [Slave] -> + Slave1 = update_slave(Slave, SlaveConf), + ets:insert(NmtTable, Slave1), + update_slave_mode(Slave, NmtTable) + end, + load_nmt_slaves(Fd, NmtTable); + {ok, Conf} -> + lager:error("load_nmt_slaved: bad config data ~p", [Conf]), + {error, bad_nmt_conf}; + eof -> + ok + end. + +update_slave(Slave, Config) -> + GuardTime = proplists:get_value(guard_time, Config, + Slave#nmt_slave.guard_time), + LifeFactor = proplists:get_value(life_factor, Config, + Slave#nmt_slave.life_factor), + HeartBeatTime = proplists:get_value(heartbeat_time, Config, + Slave#nmt_slave.heartbeat_time), + Mode = proplists:get_value(mode, Config, + Slave#nmt_slave.mode), + Slave#nmt_slave { mode = Mode, + guard_time = GuardTime, + life_factor = LifeFactor, + heartbeat_time = HeartBeatTime }. + +update_slave_mode(Slave, NmtTable) -> + if Slave#nmt_slave.mode =:= automatic, + Slave#nmt_slave.node_state =/= ?Operational -> + Cmd = start, + case send_nmt(Slave#nmt_slave.id, co_lib:encode_nmt_command(Cmd)) of + ok -> + update_slave_state(Slave, Cmd, NmtTable), + ok; + Error -> + Error + end; + true -> + ok + end. + activate_heartbeat(Ctx=#ctx {node_pid = NodePid}) -> case co_api:value(NodePid, {?IX_CONSUMER_HEARTBEAT_TIME, 0}) of {ok, 0} -> @@ -756,7 +845,7 @@ activate_heartbeat(Ctx=#ctx {node_pid = NodePid}) -> {error, Error} -> ?dbg("activate_heartbeat: Failed reading consumer table, " "reason ~p.", [Error]), - ?ee("Failed reading hearbeat consumer table, reason ~p, " + ?ee("Failed reading heartbeat consumer table, reason ~p, " "no supervision possible.\n", [Error]) end. @@ -806,7 +895,7 @@ deactivate_heartbeat(SlaveId, _Ctx=#ctx {nmt_table = NmtTable}) -> ok end. -add_slave(SlaveId = {_Flag, _NodeId}, +add_slave_(SlaveId = {_Flag, _NodeId}, Ctx=#ctx {supervision = Supervision, nmt_table = NmtTable}) -> ?dbg("add_slave: {~p, ~.16#}", [_Flag, _NodeId]), Slave = #nmt_slave {id = SlaveId}, @@ -820,7 +909,7 @@ add_slave(SlaveId = {_Flag, _NodeId}, %% Heartbeat is performed based on dictionary %% and thus not dynamically changed %%activate_heartbeat(Slave, Ctx) - Ctx + Slave %% Ctx end, ets:insert(NmtTable, NewSlave), send_to_slave(NewSlave, start, Ctx). @@ -904,7 +993,7 @@ send_to_slave(Slave=#nmt_slave {id = SlaveId}, Cmd, Error end. -send_all(Cmd, Ctx=#ctx {nmt_table = NmtTable}) -> +send_all(Cmd, #ctx {nmt_table = NmtTable}) -> send_nmt({nodeid,0}, co_lib:encode_nmt_command(Cmd)), ets:foldl(fun(Slave,[]) -> update_slave_state(Slave, Cmd, NmtTable), @@ -933,11 +1022,13 @@ handle_bootup(SlaveId = {_Flag, _NodeId}, [] -> %% First message ?dbg("handle_bootup: new node, creating entry", []), - add_slave(SlaveId, Ctx); + add_slave_(SlaveId, Ctx); [Slave] -> %% Slave rebooted - ets:insert(NmtTable, Slave#nmt_slave {node_state = ?PreOperational, - contact = ok}) + Slave1 = Slave#nmt_slave {node_state = ?PreOperational, + contact = ok}, + ets:insert(NmtTable, Slave1), + update_slave_mode(Slave1, NmtTable) end. @@ -956,10 +1047,10 @@ handle_node_guard(SlaveId = {Flag, NodeId}, State, Toggle, Ctx); true -> ok end, - add_slave(SlaveId, Ctx); + add_slave_(SlaveId, Ctx); [Slave] -> - if Slave#nmt_slave.toggle == Toggle andalso - Slave#nmt_slave.node_state == State -> + if Slave#nmt_slave.toggle =:= Toggle andalso + Slave#nmt_slave.node_state =:= State -> node_guard_ok(Slave, Toggle, State, NmtTable); Slave#nmt_slave.node_state =/= State -> @@ -1017,7 +1108,7 @@ handle_heartbeat(SlaveId = {Flag, NodeId}, State, if State == ?Initialisation -> %% bootup, next state should be pre_op heartbeat_ok(Slave, ?PreOperational, NmtTable); - Slave#nmt_slave.node_state == State -> + Slave#nmt_slave.node_state =:= State -> heartbeat_ok(Slave, State, NmtTable); Slave#nmt_slave.node_state =/= State -> %% This can be because it is a node with both @@ -1099,7 +1190,7 @@ send_nmt(_SlaveId = {xnodeid, _NodeId}, _Cmd) -> {error, xnodeid_not_possible}; send_nmt(_SlaveId = {_Flag, NodeId}, Cmd) -> ?dbg("send_nmt: slave {~p, ~.16#}, ~p", [_Flag, NodeId, Cmd]), - can:send(#can_frame { id = ?COBID_TO_CANID(?NMT_ID), + can:send(#can_frame { id = ?NMT_ID, len = 2, data = <>}). diff --git a/src/co_node.erl b/src/co_node.erl index 90844fe..c9a7d00 100644 --- a/src/co_node.erl +++ b/src/co_node.erl @@ -228,6 +228,7 @@ init({Serial, NodeName, Opts}) -> state = ?Initialisation, debug = Dbg, nmt_role = proplists:get_value(nmt_role,Opts,autonomous), + nmt_conf = proplists:get_value(nmt_conf,Opts,default), supervision = proplists:get_value(supervision,Opts,none), sub_table = SubTable, res_table = ResTable, @@ -825,6 +826,7 @@ handle_call({option, Option}, _From, Ctx=#co_ctx {name = _Name}) -> nodeid -> {Option, Ctx#co_ctx.nodeid}; xnodeid -> {Option, Ctx#co_ctx.xnodeid}; nmt_role -> {Option, Ctx#co_ctx.nmt_role}; + nmt_conf -> {Option, Ctx#co_ctx.nmt_conf}; supervision -> {Option, Ctx#co_ctx.supervision}; inact_timeout -> {Option, Ctx#co_ctx.inact_timeout}; id -> id(Ctx); @@ -881,16 +883,17 @@ handle_call({option, Option, NewValue}, _From, Ctx#co_ctx {name = NewValue}; NodeIdChange when - NodeIdChange == nodeid; - NodeIdChange == xnodeid; - NodeIdChange == use_serial_as_xnodeid -> + NodeIdChange =:= nodeid; + NodeIdChange =:= xnodeid; + NodeIdChange =:= use_serial_as_xnodeid -> nodeid_change(NodeIdChange, NewValue, Ctx); NmtChange when - NmtChange == nmt_role; - NmtChange == supervision -> + NmtChange =:= nmt_role; + NmtChange =:= nmt_conf; + NmtChange =:= supervision -> nmt_change(NmtChange, NewValue, Ctx); - + _Other -> {error, "Unknown option " ++ atom_to_list(Option)} end, case Reply of @@ -1314,6 +1317,13 @@ nodeid_change(use_serial_as_xnodeid, false, Ctx) -> %% ROLE %% No change +nmt_change(nmt_conf, NewValue, Ctx) -> + case co_nmt:load(NewValue) of + ok -> + Ctx#co_ctx {nmt_conf = NewValue}; + Error -> + Error + end; nmt_change(nmt_role, NewRole, Ctx=#co_ctx {nmt_role = NewRole}) -> Ctx; %% autonomous to master @@ -1659,11 +1669,11 @@ handle_node_guard(Frame, NodeId = {TypeOfNid, _Nid}, node_guard_error = NgError, name=_Name}) when ?is_can_frame_rtr(Frame) -> ?dbg("~s: handle_node_guard: node ~p request ~w, now ~p", - [_Name,NodeId,Frame, now()]), + [_Name,NodeId,Frame, os:timestamp()]), %% Reset NMT master supervision timer if Timer =/= undefined -> erlang:cancel_timer(Timer); - true -> + true -> do_nothing end, %% If NMT supervision has been down resend bootup ?? Toggle ?? @@ -2056,10 +2066,11 @@ handle_tpdo_process(T=#tpdo {pid = _Pid, offset = _Offset}, _Reason, -activate_nmt(Ctx=#co_ctx {supervision = Sup, dict = Dict, +activate_nmt(Ctx=#co_ctx {supervision = Sup, dict = Dict, nmt_conf = Conf, name = _Name, debug = Dbg}) -> ?dbg("~s: activate_nmt", [_Name]), co_nmt:start_link([{node_pid, self()}, + {conf, Conf}, {dict, Dict}, {supervision, Sup}, {debug, Dbg}]), @@ -3117,7 +3128,7 @@ stop_timer(TimerRef) -> time_of_day() -> - now_to_time_of_day(now()). + now_to_time_of_day(os:timestamp()). set_time_of_day(_Time) -> ?dbg("set_time_of_day: ~p", [_Time]),