Autopilot Altitude Select

[iwik]

Hi all,
Can someone help with the following problem i am trying to solve. Code included
does not work and i am struggling to find an answer.
Here is what i am trying to accomplish.
Have a real Autopilot which does not have Altitude Hold. However in real life an accessory was available.
This did not have Altitude select or Vertical speed(you have to trim plane at set Alt) adjustment.

Var 0002,  Link FSUIPC_IN, Offset $6020, Length 8  // Current Altitude
Var 0003,  Link FSUIPC_IN, Offset $07D0, Length 4  // Autopilot alt hold Switch
{
  CALL &DoSomething
}


Var 0004,  Link FSUIPC_IN, Offset $07BC, Length 4     // Autopilot On/Off

{
  CALL &DoSomething
}

Var 0005, name DoSomething, Link SUBRUTINE 
{
  L0 = v0003 
 L1 = V0004
 C0 = L0 = 1 
 C1 = L1 = 1
 IF C0 and C1
  {
  L0 = V0002 * 3.28084
  V0006 = L0 / 5.00616455078125E-005
  }
Else
  {
   v0006 = 0
  }
}
Var 0006,  Link FSUIPC_OUT, offset $07d4, Length 4  // Selected Altitude

So what i need to do is have the current Altitude captured($6020) as a snapshot and then write it to offset $07D4.
This will then be my Target altitude and upon setting Alt in Autopilot will settle at desired Altitude.
My code is attached just keeps chasing itself and never settles at my first setting of AP Alt set.
sorry about attaching as i cant remember what the syntax is to include code.
Thanks
Kindest Regards
Les
Update: Corrected last line to change Offsett $07d4 to length 4 and
addd V002 = L0 * 3.28084