471 lines
17 KiB
Plaintext
471 lines
17 KiB
Plaintext
open CstArrayInit
|
|
open Mc_TypeSensors
|
|
open Mc_ext
|
|
open TypeTracks
|
|
open TypeBase
|
|
open TypeArray
|
|
|
|
const trackarrayinit : bool = false
|
|
|
|
(* safe state machine for the computing of radar or iff state
|
|
state ident: state.0 *)
|
|
node statecmd(onoffbuttonpressed : bool (*last = false*);
|
|
currentstate : tsensorstate)
|
|
returns (onoffcmd : bool)
|
|
let
|
|
automaton
|
|
state Off
|
|
do onoffcmd = false;
|
|
unless onoffbuttonpressed & currentstate = TState_OFF then On
|
|
|
|
state On
|
|
do onoffcmd = true;
|
|
unless onoffbuttonpressed & currentstate = TState_ON then Off
|
|
end
|
|
tel
|
|
|
|
(* compute the new radar state each time on/off button
|
|
is pressed *)
|
|
node mc_rdrstatecmd(rdronoffbutton : bool; currentrdrstate : tsensorstate)
|
|
returns (rdronoffcmd : bool)
|
|
let
|
|
rdronoffcmd =
|
|
statecmd(Digital.risingEdge(rdronoffbutton), currentrdrstate);
|
|
tel
|
|
|
|
(* compute the new iff state each time on/off button
|
|
is pressed *)
|
|
node mc_iffstatecmd(iffonoffbutton : bool; currentiffstate : tsensorstate)
|
|
returns (iffonoffcmd : bool)
|
|
let
|
|
iffonoffcmd =
|
|
statecmd(Digital.risingEdge(iffonoffbutton), currentiffstate);
|
|
tel
|
|
|
|
(* safe state machine for the computing of radar mode
|
|
state ident: state.6 *)
|
|
node rdrmodecmd(currentstate : tsensorstate;
|
|
modebuttonpressed : bool(* last = false*);
|
|
currentmode : trdrmode)
|
|
returns (modecmd : bool)
|
|
let
|
|
automaton
|
|
state Wide
|
|
do modecmd = false;
|
|
unless (modebuttonpressed &
|
|
(currentstate = TState_ON &
|
|
currentmode = TRdrMode_WIDE)) then Narrow
|
|
|
|
state Narrow
|
|
do modecmd = true;
|
|
unless (modebuttonpressed &
|
|
(currentstate = TState_ON &
|
|
currentmode = TRdrMode_NARROW)) then Wide
|
|
end
|
|
tel
|
|
|
|
(* compute the new radar mode each time on/off button
|
|
is pressed *)
|
|
node mc_rdrmodecmd(currentrdrstate : tsensorstate;
|
|
rdrmodebutton : bool;
|
|
currentrdrmode : trdrmode)
|
|
returns (rdrmodecmd : bool)
|
|
let
|
|
rdrmodecmd =
|
|
rdrmodecmd(currentrdrstate, Digital.risingEdge(rdrmodebutton),
|
|
currentrdrmode);
|
|
tel
|
|
|
|
(* compute the radar mode, according to the corresponding
|
|
input command from the mission computer *)
|
|
fun radar_mode(modecmd : bool) returns (mode : trdrmode)
|
|
let
|
|
mode = if modecmd then TRdrMode_NARROW else TRdrMode_WIDE;
|
|
tel
|
|
|
|
(* compute the radar state, according to:
|
|
- the corresponding input command from the mission computer
|
|
- the failure state of the radar *)
|
|
node radar_state(onoffcmd, failure : bool)
|
|
returns (initializing : bool; st : tsensorstate)
|
|
var x : bool;
|
|
let
|
|
initializing = st = TState_OFF & onoffcmd;
|
|
(* x = fby (onoffcmd; 5; false) *)
|
|
x = false fby false fby false fby false fby false fby onoffcmd;
|
|
st =
|
|
if failure
|
|
then TState_FAIL
|
|
else if (if onoffcmd then x else false)
|
|
then TState_ON
|
|
else TState_OFF;
|
|
tel
|
|
|
|
(* elaborate and generate the (up to 2) tracks detected
|
|
by the radar (position + speed + distance + rate of
|
|
closing) *)
|
|
node radar_tracks(st : tsensorstate;
|
|
tracks : TypeArray.ttracksarray;
|
|
rdrdetectedtracks : TypeArray.tdetectedrdrtracksarray)
|
|
returns (rdrtracks : TypeArray.trdrtracksarray)
|
|
var
|
|
l22 : TypeTracks.ttrack^ksizerdrtracksarray;
|
|
l30 : TypeTracks.trdrtrack^ksizerdrtracksarray;
|
|
let
|
|
rdrtracks = if st = TState_ON then l30 else kinitrdrtrackarray;
|
|
l30 = map<<ksizerdrtracksarray>> Trackslib.elaboraterdrtrack(l22);
|
|
l22 =
|
|
map<<ksizerdrtracksarray>> Trackslib.selectdetectedtrack(
|
|
rdrdetectedtracks, tracks^ksizerdrtracksarray,
|
|
CstTracksInit.kinittrack^ksizerdrtracksarray);
|
|
tel
|
|
|
|
(* scade representation for the radar, generating:
|
|
1) the radar state
|
|
2) the radar mode
|
|
3) the (up to 2) tracks detected by the radar *)
|
|
node radar(onoffcmd, modecmd, failure : bool;
|
|
rdrdetectedtracks : TypeArray.tdetectedrdrtracksarray;
|
|
tracks : TypeArray.ttracksarray)
|
|
returns (initializing : bool;
|
|
st : tsensorstate;
|
|
mode : trdrmode;
|
|
rdrtracks : TypeArray.trdrtracksarray)
|
|
let
|
|
rdrtracks = radar_tracks(st, tracks, rdrdetectedtracks);
|
|
mode = radar_mode(modecmd);
|
|
(initializing, st) = radar_state(onoffcmd, failure);
|
|
tel
|
|
|
|
(* compute the iff state, according to:
|
|
- the corresponding input command from the mission computer
|
|
- the failure state of the iff *)
|
|
node iff_state(onoffcmd, failure : bool)
|
|
returns (initializing : bool; st : tsensorstate)
|
|
var x : bool;
|
|
let
|
|
initializing = st = TState_OFF & onoffcmd;
|
|
(* x = fby (onoffcmd; 5; false) *)
|
|
x = false fby false fby false fby false fby false fby onoffcmd;
|
|
st =
|
|
if failure
|
|
then TState_FAIL
|
|
else if (if onoffcmd then x else false)
|
|
then TState_ON
|
|
else TState_OFF;
|
|
tel
|
|
|
|
fun ifftrack_of_track(track : TypeTracks.ttrack)
|
|
returns (ifftrack : TypeTracks.tifftrack)
|
|
let
|
|
ifftrack = { i_pos = track.t_pos; i_id = track.t_id };
|
|
tel
|
|
|
|
(* elaborate and generate the (up to 2) tracks detected
|
|
by the iff (position + identifier) *)
|
|
fun iff_tracks(st : tsensorstate;
|
|
tracks : TypeArray.ttracksarray;
|
|
iffdetectedtracks : TypeArray.tdetectedifftracksarray)
|
|
returns (ifftracks : TypeArray.tifftracksarray)
|
|
var l34 : TypeTracks.ttrack^TypeArray.ksizeifftracksarray;
|
|
l40 : TypeArray.tifftracksarray;
|
|
let
|
|
l34 =
|
|
map<<ksizeifftracksarray>> Trackslib.selectdetectedtrack(
|
|
iffdetectedtracks, tracks^ksizeifftracksarray,
|
|
CstTracksInit.kinittrack^ksizeifftracksarray);
|
|
l40 = map<<ksizeifftracksarray>> ifftrack_of_track(l34);
|
|
ifftracks = if st = TState_ON then l40 else kinitifftrackarray;
|
|
tel
|
|
|
|
|
|
(* scade representation for the iff, generating:
|
|
1) the iff state
|
|
2) the (up to 2) tracks detected by the iff *)
|
|
node iff(tracks : TypeArray.ttracksarray;
|
|
failure : bool;
|
|
iffdetectedtracks : TypeArray.tdetectedifftracksarray;
|
|
onoffcmd : bool)
|
|
returns (st : tsensorstate;
|
|
ifftracks : TypeArray.tifftracksarray;
|
|
initializing : bool)
|
|
let
|
|
ifftracks = iff_tracks(st, tracks, iffdetectedtracks);
|
|
(initializing, st) = iff_state(onoffcmd, failure);
|
|
tel
|
|
|
|
node advrandr(min, max : float) returns (output1 : float)
|
|
let
|
|
output1 = (max -. min) *. rand() +. min;
|
|
tel
|
|
|
|
node advrandi(min, max, step : int) returns (output1 : int)
|
|
var l8 : int;
|
|
let
|
|
l8 = if 0 <> step then step else 1;
|
|
output1 = (int_of_float (float_of_int (max - min) *. rand())
|
|
+ min) / (l8 * l8);
|
|
tel
|
|
|
|
(* for one given track, generate:
|
|
1) its new position according to:
|
|
- its previous position, the input speed and slope
|
|
if set/reset button not pressed
|
|
- the input initial position if set/reset button pressed
|
|
2) its identifier according to the input identifier *)
|
|
node createtracks_createonetrack_init_rand()
|
|
returns (sloperadinit, speedinit, xmeterinit, ymeterinit : float;
|
|
idinit : int)
|
|
let
|
|
speedinit = advrandr(250.0, 1000.0) *. CstPhysics.t;
|
|
ymeterinit = CstPhysics.nm *. advrandr(-. 10.0, 10.0);
|
|
xmeterinit = advrandr(-. 10.0, 10.0) *. CstPhysics.nm;
|
|
sloperadinit = 2.0 *. CstPhysics.pi *. advrandr(0.0, 360.0) /. 360.0;
|
|
idinit = advrandi(0, 1000, 10);
|
|
tel
|
|
|
|
(* for one given track, generate:
|
|
1) its new position according to:
|
|
- its previous position, the input speed and slope
|
|
if set/reset button not pressed
|
|
- the input initial position if set/reset button pressed
|
|
2) its identifier according to the input identifier *)
|
|
node createtracks_createonetrack_rand(res : bool)
|
|
returns (track : TypeTracks.ttrack)
|
|
var id : int; sloperad, speedt, x0, y0, l9, l18 : float;
|
|
let
|
|
(* (sloperad, speedt, x0, y0, id) =
|
|
(activate createtracks_createonetrack_init_rand every reset initial default (
|
|
0., 0., 0., 0., 0))(); *)
|
|
(sloperad, speedt, x0, y0, id) =
|
|
if res then createtracks_createonetrack_init_rand()
|
|
else (0.0, 0.0, 0.0, 0.0, 0) fby (sloperad, speedt, x0, y0, id);
|
|
l18 = y0 -> Mathext.sinr(sloperad) *. speedt +. (y0 -> pre l18);
|
|
l9 = x0 -> (x0 -> pre l9) +. speedt *. Mathext.cosr(sloperad);
|
|
track = { t_pos = { x = l9; y = l18 }; t_id = id };
|
|
tel
|
|
|
|
(* generate up to 4 tracks (position + identifier) according
|
|
to the graphical track inputs panel. *)
|
|
node createtracks_rand(res : bool)
|
|
returns (tracks : TypeArray.ttracksarray)
|
|
let
|
|
tracks = map<<ksizetracksarray>> createtracks_createonetrack_rand(res^ksizetracksarray);
|
|
tel
|
|
|
|
node createalltracks(res : bool)
|
|
returns (tracks : TypeArray.ttracksarray)
|
|
let
|
|
(* tracks = (restart createtracks_rand every res)(res); *)
|
|
reset
|
|
tracks = createtracks_rand(res);
|
|
every res
|
|
tel
|
|
|
|
|
|
(* merge a mission track detected by the radar with a
|
|
mission track detected by the iff if they have the same
|
|
position and speed.
|
|
in that case, newrdrmissiontrack is the merged track, and newiffmissiontrack is reset to "empty".
|
|
otherwise, outputs = inputs *)
|
|
fun fusionrdrifftracks(iffmissiontrack, rdrmissiontrack
|
|
: TypeTracks.tmissiontrack)
|
|
returns (newiffmissiontrack, newrdrmissiontrack
|
|
: TypeTracks.tmissiontrack)
|
|
var l90 : bool;
|
|
let
|
|
newrdrmissiontrack =
|
|
if l90
|
|
then { m_pos = rdrmissiontrack.m_pos;
|
|
m_speed = rdrmissiontrack.m_speed;
|
|
m_id = iffmissiontrack.m_id;
|
|
m_priority = rdrmissiontrack.m_priority;
|
|
m_d = rdrmissiontrack.m_d;
|
|
m_sabs = rdrmissiontrack.m_sabs;
|
|
m_sr = rdrmissiontrack.m_sr;
|
|
m_detectedbyradar = rdrmissiontrack.m_detectedbyradar;
|
|
m_detectedbyiff = iffmissiontrack.m_detectedbyiff;
|
|
m_tracknumber = 0;
|
|
m_targettype = iffmissiontrack.m_targettype;
|
|
m_isvisible = rdrmissiontrack.m_isvisible;
|
|
m_angle = rdrmissiontrack.m_angle }
|
|
else rdrmissiontrack;
|
|
l90 =
|
|
Trackslib.comparetracks(rdrmissiontrack.m_pos, iffmissiontrack.m_pos,
|
|
rdrmissiontrack.m_speed, iffmissiontrack.m_speed);
|
|
newiffmissiontrack =
|
|
if l90
|
|
then CstTracksInit.kinitmissiontrack
|
|
else iffmissiontrack;
|
|
tel
|
|
|
|
(* merge tracks data received from both radar and iff sensors *)
|
|
fun mc_tracks_fusion_onerdrwithifftracks(rdrtrack : TypeTracks.tmissiontrack;
|
|
ifftracks : TypeTracks.tmissiontrack^ksizeifftracksarray)
|
|
returns (fusionnedrdrtrack : TypeTracks.tmissiontrack;
|
|
fusionnedifftracks : TypeTracks.tmissiontrack^ksizeifftracksarray)
|
|
let
|
|
(fusionnedifftracks, fusionnedrdrtrack) =
|
|
mapfold<<ksizeifftracksarray>> fusionrdrifftracks(ifftracks, rdrtrack);
|
|
tel
|
|
|
|
(* merge tracks data received from both radar and iff sensors *)
|
|
node mc_tracks_fusion(rdrtracks : TypeArray.trdrtracksarray;
|
|
ifftracks : TypeArray.tifftracksarray)
|
|
returns (missiontracks : TypeArray.tmissiontracksarray)
|
|
var
|
|
mergedrdrtracks : TypeTracks.tmissiontrack^ksizerdrtracksarray;
|
|
mergedifftracks : TypeTracks.tmissiontrack^ksizeifftracksarray;
|
|
l140 : TypeTracks.tmissiontrack^ksizerdrtracksarray;
|
|
l139 : TypeTracks.tmissiontrack^ksizeifftracksarray;
|
|
let
|
|
missiontracks = mergedrdrtracks @ mergedifftracks;
|
|
(mergedrdrtracks, mergedifftracks) =
|
|
mapfold<<ksizerdrtracksarray>> mc_tracks_fusion_onerdrwithifftracks(l140, l139);
|
|
l140 = map<<ksizerdrtracksarray>> Trackslib.convertrdrtracktomissiontrack(rdrtracks);
|
|
l139 = map<<ksizeifftracksarray>> Trackslib.convertifftracktomissiontrack(ifftracks);
|
|
tel
|
|
|
|
|
|
fun prio_tracknumbernotinarray(missiontracktracknumber,
|
|
prioritytrack : int; acc : bool)
|
|
returns (notinarray : bool)
|
|
let
|
|
notinarray = acc & missiontracktracknumber <> prioritytrack;
|
|
tel
|
|
|
|
(* replace the lowest priority track in priorityarray by missiontrack *)
|
|
node prio_selecthighestprioritynotinpriorityarray(
|
|
missiontrack : TypeTracks.tmissiontrack;
|
|
prioritiesarray : Mc_TypeLists.tpriorityList;
|
|
accprioritymissiontrack : TypeTracks.tmissiontrack)
|
|
returns (prioritymissiontrack : TypeTracks.tmissiontrack)
|
|
var
|
|
missiontracknotinpriorittiesarray,
|
|
missiontrackhashigherprioritythanacc : bool;
|
|
let
|
|
missiontrackhashigherprioritythanacc =
|
|
not Trackslib.trackalowerprioritythanb(missiontrack,
|
|
accprioritymissiontrack);
|
|
missiontracknotinpriorittiesarray =
|
|
fold<<4>> prio_tracknumbernotinarray(missiontrack.m_tracknumber^4,
|
|
prioritiesarray, true);
|
|
prioritymissiontrack =
|
|
if missiontracknotinpriorittiesarray & missiontrackhashigherprioritythanacc
|
|
then missiontrack
|
|
else accprioritymissiontrack;
|
|
tel
|
|
|
|
(* for each missiontrack
|
|
if priority higher than all in priorityarray and not in priorityarray
|
|
then, copy in priorityarray at index *)
|
|
node prio_selectprioritarymissiontracks(missiontracks : TypeArray.tmissiontracksarray;
|
|
prioritiesarray : Mc_TypeLists.tpriorityList;
|
|
indexpriority : int)
|
|
returns (newprioritiesarray : Mc_TypeLists.tpriorityList)
|
|
var missiontrackwithhighestpriority : TypeTracks.tmissiontrack;
|
|
let
|
|
newprioritiesarray =
|
|
[ prioritiesarray with [indexpriority] =
|
|
missiontrackwithhighestpriority.m_tracknumber ];
|
|
missiontrackwithhighestpriority =
|
|
fold<<ksizemissiontracksarray>> prio_selecthighestprioritynotinpriorityarray(
|
|
missiontracks,
|
|
prioritiesarray^ksizemissiontracksarray, CstTracksInit.kinitmissiontrack);
|
|
tel
|
|
|
|
fun prio_setpriorityinmissiontrack(prioritytracknumber : int;
|
|
priorityindex : int;
|
|
missiontrack : TypeTracks.tmissiontrack)
|
|
returns (missiontrackwithprio : TypeTracks.tmissiontrack)
|
|
let
|
|
missiontrackwithprio =
|
|
if prioritytracknumber = missiontrack.m_tracknumber
|
|
then Trackslib.setmissiontrackpriority(missiontrack, priorityindex + 1)
|
|
else missiontrack;
|
|
tel
|
|
|
|
fun prio_setpriorityinmissiontrackarray(priorityarray : Mc_TypeLists.tpriorityList;
|
|
missiontrack : TypeTracks.tmissiontrack)
|
|
returns (missiontrackwithprio : TypeTracks.tmissiontrack)
|
|
let
|
|
missiontrackwithprio =
|
|
foldi<<4>> prio_setpriorityinmissiontrack(priorityarray, missiontrack);
|
|
tel
|
|
|
|
|
|
|
|
(* set the priority in missiontracks:
|
|
1) set the highest prority
|
|
2) set the second priority=highest different from the previous
|
|
3) set the 3rd priority=highest different from the previous
|
|
3) set the 4th priority=highest different from the previous
|
|
=> the 4 priority track should be in an array (initialized to "empty")
|
|
operator selectprioritymissiontracks inputs
|
|
- missiontracks
|
|
- prioritytrack set (to perform the "different from the previous")
|
|
*test for each missiontrack: the higest, and not already in prioritytracks.
|
|
*then, set the ith element of prioritytracks with the one found
|
|
for each missiontrack, if prioritary higher than the lowest 4 prioritary
|
|
old: compute each detected track priority, and sort tracks
|
|
according to their priority *)
|
|
node mc_tracks_prio(missiontracks : TypeArray.tmissiontracksarray)
|
|
returns (missiontrackswithprio : TypeArray.tmissiontracksarray)
|
|
var prioritytracknumbers : Mc_TypeLists.tpriorityList;
|
|
let
|
|
missiontrackswithprio =
|
|
map<<ksizemissiontracksarray>> prio_setpriorityinmissiontrackarray(
|
|
prioritytracknumbers^ksizemissiontracksarray, missiontracks);
|
|
prioritytracknumbers =
|
|
prio_selectprioritarymissiontracks(missiontracks,
|
|
prio_selectprioritarymissiontracks(missiontracks,
|
|
prio_selectprioritarymissiontracks(missiontracks,
|
|
prio_selectprioritarymissiontracks(missiontracks, 0^4, 0), 1), 2),
|
|
3);
|
|
tel
|
|
|
|
(* associate a track number to each detected track *)
|
|
node mc_tracks_tracknumber(withouttracknb : TypeArray.tmissiontracksarray)
|
|
returns (withtracknumber : TypeArray.tmissiontracksarray)
|
|
var l81 : int;
|
|
let
|
|
(withtracknumber, l81) =
|
|
mapfold<<ksizemissiontracksarray>> Trackslib.calculatemissiontracknumber
|
|
((kinitmissiontrackarray -> pre withtracknumber)^ksizemissiontracksarray,
|
|
withouttracknb, 0 -> pre l81);
|
|
tel
|
|
|
|
(* 1) merge tracks data received from both radar and iff sensors
|
|
2) associate a track number to each detected track
|
|
3) compute each detected track priority, and sort tracks
|
|
according to their priority *)
|
|
node mc_tracks(rdrtracks : TypeArray.trdrtracksarray;
|
|
ifftracks : TypeArray.tifftracksarray)
|
|
returns (missiontracks : TypeArray.tmissiontracksarray)
|
|
let
|
|
missiontracks =
|
|
mc_tracks_prio(mc_tracks_tracknumber(mc_tracks_fusion(rdrtracks,
|
|
ifftracks)));
|
|
tel
|
|
|
|
(* scade representation for the mission computer, computing:
|
|
- the new radar state
|
|
- the new radar mode
|
|
- the new iff state
|
|
- the (up to 4) tracks detected by the fighter *)
|
|
node mc(currentrdrstate : tsensorstate;
|
|
currentrdrmode : trdrmode;
|
|
rdrtracks : TypeArray.trdrtracksarray;
|
|
rdronoffbutton, rdrmodebutton, iffonoffbutton : bool;
|
|
currentiffstate : tsensorstate;
|
|
ifftracks : TypeArray.tifftracksarray)
|
|
returns (rdronoffcmd, rdrmodecmd : bool;
|
|
missiontracks : TypeArray.tmissiontracksarray;
|
|
iffonoffcmd : bool)
|
|
let
|
|
missiontracks = mc_tracks(rdrtracks, ifftracks);
|
|
iffonoffcmd = mc_iffstatecmd(iffonoffbutton, currentiffstate);
|
|
rdrmodecmd = mc_rdrmodecmd(currentrdrstate, rdrmodebutton, currentrdrmode);
|
|
rdronoffcmd = mc_rdrstatecmd(rdronoffbutton, currentrdrstate);
|
|
tel
|