fix: update stop time correctly
This commit is contained in:
parent
ffc583985a
commit
36806f9af5
GPG key ID:
7065F57ED8856128
1 changed files with 57 additions and 27 deletions
|
|
@ -7,12 +7,31 @@ module Sim (S : SimState) =
|
|||
struct
|
||||
include S
|
||||
|
||||
(** Discrete step. *)
|
||||
let step_discrete
|
||||
s step hor fder fzer cget zset csize zsize jump reset reinit
|
||||
(s : ('a, 'b, 'ms, 'ss, 'zin) state)
|
||||
(step : 'ms -> time -> 'a -> 'b * 'ms)
|
||||
(hor : 'ms -> time)
|
||||
(fder : 'ms -> time -> 'a -> 'y -> 'yder)
|
||||
(fzer : 'ms -> time -> 'a -> 'y -> 'zout)
|
||||
(cget : 'ms -> 'y)
|
||||
(zset : 'ms -> 'zin -> 'ms)
|
||||
(csize : int)
|
||||
(zsize : int)
|
||||
(jump : 'ms -> bool)
|
||||
(reset : ('y, 'yder) ivp * ('y, 'zout) zc -> 'ss -> 'ss)
|
||||
(reinit : bool)
|
||||
: 'b value * ('a, 'b, 'ms, 'ss, 'zin) state
|
||||
= let ms, ss = get_mstate s, get_sstate s in
|
||||
let zin, last = get_zin s, get_last s in
|
||||
(match last with Some { h; u; _ } -> ignore (u h) | None -> ());
|
||||
let ms = match zin with Some z -> zset ms z | None -> ms in
|
||||
(* Since the last output value might have been used before this call,
|
||||
which may have changed the solver state, we call it at its horizon to
|
||||
make sure the solver state is correct. *)
|
||||
Option.iter (fun { h; u; _ } -> ignore (u h)) last;
|
||||
(* Similarly, we update the zero-crossing information from the last step
|
||||
now, so that it is used at the right time (and not when using the last
|
||||
output value). *)
|
||||
let ms = Option.fold ~none:ms ~some:(zset ms) zin in
|
||||
let i, now, stop = get_input s, get_now s, get_stop s in
|
||||
let o, ms = step ms now (i.u now) in
|
||||
let s =
|
||||
|
|
@ -27,19 +46,29 @@ module Sim (S : SimState) =
|
|||
let zc = { init; fzer; size=zsize } in
|
||||
let ss = reset (ivp, zc) ss in
|
||||
let input = { i with h=i.h -. now; u=Utils.offset i.u now } in
|
||||
let mode, stop, now = Continuous, i.h, 0.0 in
|
||||
let mode, stop, now = Continuous, input.h, 0.0 in
|
||||
update ms ss (set_running ~mode ~input ~stop ~now s)
|
||||
end else set_running ~mode:Continuous s in
|
||||
let o = Utils.dot o in
|
||||
o, (set_last (Some o) (set_zin None s))
|
||||
|
||||
let step_continuous s step cset fout hor =
|
||||
let ms, ss, last = get_mstate s, get_sstate s, get_last s in
|
||||
(match last with None -> () | Some { h; u; _ } -> ignore (u h));
|
||||
(** Continuous step. *)
|
||||
let step_continuous
|
||||
(s : ('a, 'b, 'ms, 'ss, 'zin) state)
|
||||
(step : 'ss -> time -> (time * (time -> 'y) * 'zin option) * 'ss)
|
||||
(cset : 'ms -> 'y -> 'ms)
|
||||
(fout : 'ms -> time -> 'a -> 'y -> 'b)
|
||||
(hor : 'ms -> time)
|
||||
: 'b value * ('a, 'b, 'ms, 'ss, 'zin) state * 'ms value
|
||||
= let ms, ss, last = get_mstate s, get_sstate s, get_last s in
|
||||
(* Since the last output value might have been used before this call,
|
||||
which may have changed the solver state, we call it at its horizon to
|
||||
make sure the solver state is correct. *)
|
||||
Option.iter (fun { h; u; _ } -> ignore (u h)) last;
|
||||
let i, now, stop = get_input s, get_now s, get_stop s in
|
||||
let stop = min stop (hor ms) in
|
||||
let (h, f, z), ss = step ss (min stop (hor ms)) in
|
||||
let h = min h (min stop (hor ms)) in
|
||||
let (h, f, z), ss = step ss stop in
|
||||
let h = min h stop in
|
||||
let ms = cset ms (f h) in
|
||||
let fy t = f (now +. t) in
|
||||
let fms t = cset ms (fy t) in
|
||||
|
|
@ -60,11 +89,11 @@ module Sim (S : SimState) =
|
|||
(DNode s : ('y, 'yder, 'zin, 'zout) solver)
|
||||
: ('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) sim
|
||||
= let state = get_init m.state s.state in
|
||||
let step_discrete ?(reinit=false) st =
|
||||
let dstep ?(reinit=false) st =
|
||||
let o, s = step_discrete st m.step m.horizon m.fder m.fzer m.cget m.zset
|
||||
m.csize m.zsize m.jump s.reset reinit in
|
||||
Some o, s in
|
||||
let step_continuous st =
|
||||
let cstep st =
|
||||
let o, s, _ = step_continuous st s.step m.cset m.fout m.horizon in
|
||||
Some o, s in
|
||||
|
||||
|
|
@ -72,11 +101,11 @@ module Sim (S : SimState) =
|
|||
| Some i ->
|
||||
let mode, now, stop = Discrete, 0.0, i.h in
|
||||
let reinit = i.c = Discontinuous in
|
||||
step_discrete ~reinit (set_running ~mode ~input:i ~now ~stop st)
|
||||
dstep ~reinit (set_running ~mode ~input:i ~now ~stop st)
|
||||
| None ->
|
||||
if is_running st then match get_mode st with
|
||||
| Discrete -> step_discrete st
|
||||
| Continuous -> step_continuous st
|
||||
| Discrete -> dstep st
|
||||
| Continuous -> cstep st
|
||||
else None, st in
|
||||
|
||||
let reset (pm, ps) st =
|
||||
|
|
@ -90,13 +119,12 @@ module Sim (S : SimState) =
|
|||
'a 'b. ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode_a ->
|
||||
(unit -> ('y, 'yder, 'zin, 'zout) solver) ->
|
||||
('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) sim =
|
||||
fun (HNodeA m) get_s ->
|
||||
fun (HNodeA { body=m; assertions }) get_s ->
|
||||
let DNode s = get_s () in
|
||||
let al = List.map (fun a -> run_assert a get_s) m.assertions in
|
||||
let state = get_init m.body.state s.state, al in
|
||||
let al = List.map (fun a -> run_assert a get_s) assertions in
|
||||
let state = get_init m.state s.state, al in
|
||||
|
||||
let step_discrete ?(reinit=false) (st, al) =
|
||||
let m=m.body in
|
||||
let dstep ?(reinit=false) (st, al) =
|
||||
let o, st =
|
||||
step_discrete st m.step m.horizon m.fder m.fzer m.cget m.zset m.csize
|
||||
m.zsize m.jump s.reset reinit in
|
||||
|
|
@ -105,9 +133,9 @@ module Sim (S : SimState) =
|
|||
DNode { a with state }) al in
|
||||
Some o, (st, al) in
|
||||
|
||||
let step_continuous (st, al) =
|
||||
let cstep (st, al) =
|
||||
let ({ h; _ } as o), st, u =
|
||||
step_continuous st s.step m.body.cset m.body.fout m.body.horizon in
|
||||
step_continuous st s.step m.cset m.fout m.horizon in
|
||||
let al = List.map (fun (DNode a) ->
|
||||
(* Step assertions repeatedly until they reach the horizon. *)
|
||||
let rec step s =
|
||||
|
|
@ -120,18 +148,20 @@ module Sim (S : SimState) =
|
|||
Some o, (st, al) in
|
||||
|
||||
let step (st, al) = function
|
||||
| Some i ->
|
||||
let mode, now, stop = Discrete, 0.0, i.h in
|
||||
step_discrete (set_running ~mode ~input:i ~now ~stop st, al)
|
||||
| Some input ->
|
||||
let mode, now, stop = Discrete, 0.0, input.h in
|
||||
dstep (set_running ~mode ~input ~now ~stop st, al)
|
||||
| None ->
|
||||
if is_running st then match get_mode st with
|
||||
| Discrete -> step_discrete (st, al)
|
||||
| Continuous -> step_continuous (st, al)
|
||||
| Discrete -> dstep (st, al)
|
||||
| Continuous -> cstep (st, al)
|
||||
else None, (st, al) in
|
||||
|
||||
let reset (pm, ps) (st, al) =
|
||||
let ms = m.body.reset pm (get_mstate st) in
|
||||
let ms = m.reset pm (get_mstate st) in
|
||||
let ss = s.reset ps (get_sstate st) in
|
||||
let al = List.map (fun (DNode a) ->
|
||||
DNode { a with state = a.reset (pm, ps) a.state }) al in
|
||||
update ms ss (set_idle st), al in
|
||||
|
||||
DNode { state; step; reset }
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue