Tool height setter M6 subroutine question

Hi, This is my first post to this forum.  I just finished a retrofit on a cnc router.  I am working on getting a tool height setter working.  It is a simple Normally closed switch.  So far I have it to where I can type in an M6 in the MDI bar and the machine will move to the specified position set in my toolchange.ngc file, the machine will run the G38.2 until I manually trip the switch, then I get an "Pvalue out of range with G10 L1", which leads me to believe at least my toolchange.ngc file is at least routed correctly. 

It is where I do a T1 M6 command or a T2 M6 command where things get a little funky...When I try to incorporate the T in that line either with or without the M6 or any tool number for that matter the "Begin executing next line" button gets greyed out.  When I hit the "Execute next line" nothing happens.   This seems behave as if linuxcnc sees the T as some kind of program it can't run.  I would like to be able to populate the tool.tbl with the appropriate G43 height off set for a given tool using the height setter.  Thank you taking your time reading this, here is the code I am trying...

O<toolchange> SUB

(MSG, Toolchange subroutine called)
(MSG, Tool number param #2 =  #2)

#<tool_num> = #2
#<safe_z> = -.5       ; safe Z height above tool setter
#<probe_z> = -3.0     ; how far down to probe
#<setter_z> = 0.0     ; Z height of the setter in machine coords

G49                    ; cancel length offset
G53 G90 G01 X1.0 Y0 F50. Z[#<safe_z>]    ; move above setter

; Begin probing
G38.2 Z[#<probe_z>] F1.0
(If probe failed, LinuxCNC will stop here)

#<measured_z> = #5063  ; captured machine Z from probe

(MSG, 5063)

; Compute tool length

(MSG, Compute tool length)

#<tool_length> = [#<measured_z> - #<setter_z>]

; Store to tool table
(--- Optional: print value to operator ---)
(MSG, Tool length = #<tool_length>

G10 L1 P#<tool_num> Z#<tool_length>

; Done
O<toolchange> ENDSUB
M2

Update:

I added this to the hal file:

# --- Manual Tool Change Handshake ---
# These connect the tool change request to its "done" signal,
# and the tool prepare request to its "ready" signal.
net tool-change iocontrol.0.tool-change => iocontrol.0.tool-changed
net tool-prepare iocontrol.0.tool-prepare => iocontrol.0.tool-prepared

And this to the ini file:

TOOL_CHANGE = manual

Now the T1 M6 command goes through like the M6 command did previously, but when I run the T1 M6 command I get an error that reads "P value out of range with G10 L1" I'll keep digging.

If you call toolchange with parameter, eg.

O<toolchange>  call [1][#<toolno>]

then #2 has value of second parameter. #1 to #30  are used like that.
If you need the number of loaded tool, just use #5400.
Be aware that probing result is with current active coordinate system setting. If Z is has an offset...
Some people release from toolsetter after contact.

Thank you for the response, Based on your idea of the #5400 I tried this code, but no luck: here is the current status of my toolchange.ngc
O<toolchange> SUB

(MSG, Toolchange subroutine called)
(MSG, Tool number param #1 = #1)
#2 = #5400
#<tool_num> = #2
;#<setter_z> = -3.0
#<safe_z> = -.5 ; safe Z height above tool setter
#<probe_z> = -4.0 ; how far down to probe
#<setter_z> = -3.0 ; Z height of the setter in machine coords

G49 ; cancel length offset
G53 G90 G01 X1.0 Y0 F50. Z[#<safe_z>] ; move above setter

; Begin probing
G38.2 Z[#<probe_z>] F1.0
(If probe failed, LinuxCNC will stop here)

#<measured_z> = #5063 ; captured machine Z from probe

(MSG, 5063)

; Compute tool length

(MSG, Compute tool length)

#<tool_length> = [#<measured_z> - #<setter_z>]

; Store to tool table
(--- Optional: print value to operator ---)
(MSG, Tool length = #<tool_length>)

G10 L1 P#<tool_num> Z#<tool_length>

G43 H#<tool_num>

; Done
O<toolchange> ENDSUB
M2

I feel my problem lies in the fact that my tool number is not getting passed to the tool.tbl When I enter (debug #5400) in MDI get 0.0000 after entering in a T1 M6 command in MDI, I assume that I would see a 1 for T1? My tool height seem to be registering. When I enter (debug #5063) the z value from the tool setter shows up as the #5063. I am still getting the Pvalue out of range with G10 L1 error after the probe is tripped when the G38.2 part of my file runs.

I made this change to my hal.io:

loadusr -W hal_manualtoolchange
net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number
net tool-change iocontrol.0.tool-change => hal_manualtoolchange.change
net tool-changed iocontrol.0.tool-changed <= hal_manualtoolchange.changed
net tool-prepare iocontrol.0.tool-prepare => iocontrol.0.tool-prepared

Here are some parts of my ini file:

[RS274NGC]
REMAP = M6 modalgroup=6 ngc=toolchange
SUBROUTINE_PATH = /home/dave/linuxcnc/configs/by_interface.pico.univpwmv/subroutines/
#/home/dave/linuxcnc/configs/by_interface.pico.univpwmv/subroutines/
PARAMETER_FILE = linuxcnc.var
FEATURES = 12


TOOL_TABLE = tool.tbl

RETAIN_TOOL_NO = 1

TOOL_CHANGE = manual

More progress, but I need an M1 option stop. Is there a reason that M1 option stop does not work within this m6 routine? I was able to populate my tooltable with this code.

When I have my option stop enabled on the axis gui my resume button is greyed out and I can't resume.
Here is my current toolchange code:
(MSG, Selected tool number = #<_selected_tool>)

#<safe_z> = -.005 ; safe Z height above tool setter
#<probe_z> = -4.0 ; how far down to probe
#<setter_z> = -3.0 ; Z height of the setter in machine coords

M5
M1

G49 ; cancel length offset

G53 G90 G01 F100. Z[#<safe_z>] ; move above setter z retract

G53 G90 G01 X1.10 F100. Z[#<safe_z>] ; move to tool height setter


G10 L20 P9 Z0
G59.3


; Begin probing
G38.2 Z[#<probe_z>] F30.0
(If probe failed, LinuxCNC will stop here)

G01 G91 Z.02 F10.

G90 G38.2 Z[#<probe_z>] F.15

#<measured_z> = #5063 ; captured machine Z from probe
(MSG, 5063 value = #5063)


; Compute tool length
(MSG, Compute tool length)
#<tool_length> = [#<measured_z> - #<setter_z>]

; Store to tool table
(--- Optional: print value to operator ---)
(MSG, Tool length = #<tool_length>)

G10 L1 P#<_selected_tool> Z#<tool_length>
G43 H#<_selected_tool>


G53 G01 G90 Z0. F150.


(MSG, Tool change complete)

O<toolchange> ENDSUB
M2