diff --git a/examples/MissionComputer_for_Core/cstArrayInit.epi b/examples/MissionComputer_for_Core/cstArrayInit.epi new file mode 100644 index 0000000..b02e8c2 --- /dev/null +++ b/examples/MissionComputer_for_Core/cstArrayInit.epi @@ -0,0 +1,35 @@ +open TypeTracks +open TypeBase + +const kinitifftrackarray : TypeArray.tifftracksarray = + { i_pos = { x = 0.0; y = 0.0 }; i_id = 0 } ^ TypeArray.ksizeifftracksarray + + const kinitmissiontrackarray : TypeArray.tmissiontracksarray = + { m_pos = { x = 0.0; + y = 0.0 }; + m_speed = { sx = 0.0; + sy = 0.0 }; + m_id = 0; + m_priority = 0; + m_d = 0.0; + m_sabs = 0.0; + m_sr = 0.0; + m_detectedbyradar = false; + m_detectedbyiff = false; + m_tracknumber = 0; + m_targettype = TypeBase.Ttargettype_unknown; + m_isvisible = false; + m_angle = 0.0 } ^ TypeArray.ksizemissiontracksarray + + const kinitrdrtrackarray : TypeArray.trdrtracksarray = + { r_pos = { x = 0.0; + y = 0.0 }; + r_s = { sx = 0.0; + sy = 0.0 }; + r_d = 0.0; + r_sabs = 0.0; + r_sr = 0.0 } ^ TypeArray.ksizerdrtracksarray + + const kinittrackarray : TypeArray.ttracksarray = + { t_pos = { x = 0.0; y = 0.0 }; t_id = 0 } ^ TypeArray.ksizetracksarray + diff --git a/examples/MissionComputer_for_Core/cstBaseInit.epi b/examples/MissionComputer_for_Core/cstBaseInit.epi new file mode 100644 index 0000000..fa35c3e --- /dev/null +++ b/examples/MissionComputer_for_Core/cstBaseInit.epi @@ -0,0 +1,4 @@ +open TypeBase + +const kInitPosition : TypeBase.tposition = { x = 0.0; y = 0.0 } +const kInitSpeed : TypeBase.tspeed = { sx = 0.0; sy = 0.0 } \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/cstPhysics.epi b/examples/MissionComputer_for_Core/cstPhysics.epi new file mode 100644 index 0000000..1c74de4 --- /dev/null +++ b/examples/MissionComputer_for_Core/cstPhysics.epi @@ -0,0 +1,4 @@ + +const nm : float = 1852.0 +const t : float = 0.01 +const pi : float = 3.141592 \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/cstTracksInit.epi b/examples/MissionComputer_for_Core/cstTracksInit.epi new file mode 100644 index 0000000..31fa85d --- /dev/null +++ b/examples/MissionComputer_for_Core/cstTracksInit.epi @@ -0,0 +1,34 @@ +open TypeTracks +open TypeBase + +const kinittrack : TypeTracks.ttrack = { t_pos = { x = 0.0; + y = 0.0 }; + t_id = 0 } + + const kinitrdrtrack : TypeTracks.trdrtrack = { r_pos = { x = 0.0; + y = 0.0 }; + r_s = { sx = 0.0; + sy = 0.0 }; + r_d = 0.0; + r_sabs = 0.0; + r_sr = 0.0 } + + const kinitmissiontrack : TypeTracks.tmissiontrack = { m_pos = { x = 0.0; + y = 0.0 }; + m_speed = { sx = 0.0; + sy = 0.0 }; + m_id = 0; + m_priority = 0; + m_d = 0.0; + m_sabs = 0.0; + m_sr = 0.0; + m_detectedbyradar = false; + m_detectedbyiff = false; + m_tracknumber = 0; + m_targettype = TypeBase.Ttargettype_unknown; + m_isvisible = false; + m_angle = 0.0 } + + const kinitifftrack : TypeTracks.tifftrack = { i_pos = { x = 0.0; + y = 0.0 }; + i_id = 0 } \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/debug.ept b/examples/MissionComputer_for_Core/debug.ept new file mode 100644 index 0000000..a491684 --- /dev/null +++ b/examples/MissionComputer_for_Core/debug.ept @@ -0,0 +1,44 @@ +open Mc +open Mc_TypeSensors + +(* Top node of the Mission Computer SCADE model. + The Fighter (MC + Radar + Iff), its environment + (CreateTracks) and links to the graphical interface (GUI) + are constituting this model. *) +node fighterdebug(res, rdronoffclicked, iffonoffclicked : bool) + returns (missiontracks : TypeArray.tmissiontracksarray) +var + l4 : TypeArray.trdrtracksarray; + l3 : trdrmode; + l6 : TypeArray.tifftracksarray; + l5 : tsensorstate; + l12, l11, l10 : bool; + l172 : tsensorstate; + l179 : TypeArray.ttracksarray; + l200, l201:bool; (*TODO*) +let + l179 = createalltracks(res); + (l10, l11, missiontracks, l12) = + mc(l172, l3, l4, rdronoffclicked, false, iffonoffclicked, l5, l6); + (l5, l6, l200) = iff(l179, false, [1, 2, 3], false -> pre l12); + (l201, l172, l3, l4) = + radar(false -> pre l10, false -> pre l11, false, [0, 1, 2, 3, 4], + l179); +tel + +(* top node of the mission computer scade model. + the fighter (mc + radar + iff), its environment + (createtracks) and links to the graphical interface (gui) + are constituting this model. *) +node dv_fighterdebug(res, rdronoffclicked, iffonoffclicked : bool) + returns (proof3 : bool) +let + proof3 = + Dv.dv_proof3(fighterdebug(res, rdronoffclicked, iffonoffclicked)); +tel + +fun dv_debug(missiontracks : TypeArray.tmissiontracksarray) + returns (proof3 : bool) +let + proof3 = Dv.dv_proof3(missiontracks); +tel \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/digital.ept b/examples/MissionComputer_for_Core/digital.ept new file mode 100644 index 0000000..aca52ac --- /dev/null +++ b/examples/MissionComputer_for_Core/digital.ept @@ -0,0 +1,7 @@ +(* Detects a rising edge (false to true transition ). + The output is true during the transition clock cycle. + The output is initialized to false. *) +node risingEdge(re_Input : bool) returns (re_Output : bool) +let + re_Output = not (re_Input -> pre re_Input) & re_Input; +tel \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/dv.ept b/examples/MissionComputer_for_Core/dv.ept new file mode 100644 index 0000000..3d0d90b --- /dev/null +++ b/examples/MissionComputer_for_Core/dv.ept @@ -0,0 +1,82 @@ +open TypeArray +open CstArrayInit +open Mc_TypeSensors +open Mc + +fun dv_detectedbyiff(missiontrack : TypeTracks.tmissiontrack; accin : bool) + returns (accout : bool) +let + accout = accin & not (missiontrack.m_tracknumber <> 0); +tel + +fun dv_sametracknumber(missiontrack1, + missiontrack2 : TypeTracks.tmissiontrack; + accin : bool) + returns (accout : bool) +let + accout = + accin or + missiontrack1.m_tracknumber = missiontrack2.m_tracknumber & + missiontrack2.m_tracknumber <> 0; +tel + +fun dv_tracknumberexist(missiontrack : TypeTracks.tmissiontrack; + missiontracks : TypeArray.tmissiontracksarray; + accin : bool) + returns (accout : bool) +var l36 : bool; +let + l36 = + fold dv_sametracknumber <>( + missiontrack^ksizemissiontracksarray, missiontracks, false); + accout = accin or l36; +tel + +node dv_proof1(currentrdrstate : tsensorstate; + rdronoffbutton, rdronoffcmd : bool) + returns (proof1 : bool) +let + proof1 = + Verif.implies(Digital.risingEdge(rdronoffbutton) & + currentrdrstate = TState_FAIL, rdronoffcmd = + (false -> pre rdronoffcmd)); +tel + +fun dv_proof2(ifftracks : TypeArray.tifftracksarray; + missiontracks : TypeArray.tmissiontracksarray) + returns (proof2 : bool) +var l33 : bool; +let + l33 = + fold dv_detectedbyiff <>(missiontracks, true); + proof2 = Verif.implies(ifftracks = kinitifftrackarray, l33); +tel + +(* verifiy that all non null tracknumbers are different *) +fun dv_proof3(missiontracks : TypeArray.tmissiontracksarray) + returns (proof3 : bool) +var l33 : bool; +let + l33 = + fold dv_tracknumberexist <>( + missiontracks, missiontracks^ksizemissiontracksarray, false); + proof3 = not l33; +tel + +node dv_observer(currentrdrstate : tsensorstate; + currentrdrmode : trdrmode; + rdrtracks : TypeArray.trdrtracksarray; + rdronoffbutton, rdrmodebutton, iffonoffbutton : bool; + currentiffstate : tsensorstate; + ifftracks : TypeArray.tifftracksarray) + returns (proof1, proof2, proof3 : bool) +var l3 : TypeArray.tmissiontracksarray; l1,l4,l5 : bool; +let + proof3 = dv_proof3(l3); + proof2 = dv_proof2(ifftracks, l3); + proof1 = dv_proof1(currentrdrstate, rdronoffbutton, l1); + (l1, l4, l3, l5) = + mc(currentrdrstate, currentrdrmode, rdrtracks, rdronoffbutton, + rdrmodebutton, iffonoffbutton, currentiffstate, ifftracks); +tel + diff --git a/examples/MissionComputer_for_Core/math.ept b/examples/MissionComputer_for_Core/math.ept new file mode 100644 index 0000000..7067155 --- /dev/null +++ b/examples/MissionComputer_for_Core/math.ept @@ -0,0 +1,12 @@ +fun abs(a : float) returns (o : float) +let + o = if 0.0 <=. a then a else -. a; +tel + +(* -- Returns 1.0 if input is greater than 0.0, + -- -1.0 if input is less than 0.0 + -- and 0.0 if input is equal to 0.0 *) +fun sign(a : float) returns (o : float) +let + o = if a >. 0.0 then 1.0 else if 0.0 =. a then 0.0 else -. 1.0; +tel \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/mathext.c b/examples/MissionComputer_for_Core/mathext.c new file mode 100644 index 0000000..fae22e7 --- /dev/null +++ b/examples/MissionComputer_for_Core/mathext.c @@ -0,0 +1,32 @@ +#include +#include "mathext.h" + +void atanr_step(float a, atanr_out *out) +{ + out->o = atan(a); +} + +void acosr_step(float a, acosr_out *out) +{ + out->o = acos(a); +} + +void cosr_step(float a, cosr_out *out) +{ + out->o = cos(a); +} + +void asinr_step(float a, asinr_out *out) +{ + out->o = asin(a); +} + +void sinr_step(float a, sinr_out *out) +{ + out->o = sin(a); +} + +void sqrtr_step(float a, sqrtr_out *out) +{ + out->o = sqrt(a); +} diff --git a/examples/MissionComputer_for_Core/mathext.epi b/examples/MissionComputer_for_Core/mathext.epi new file mode 100644 index 0000000..788eb57 --- /dev/null +++ b/examples/MissionComputer_for_Core/mathext.epi @@ -0,0 +1,17 @@ +(* atan() *) +val fun atanr(a : float) returns (o : float) + +(* acos() *) +val fun acosr(a : float) returns (o : float) + +(* cos() *) +val fun cosr(a : float) returns (o : float) + +(* asin() *) +val fun asinr(a : float) returns (o : float) + +(* sin() *) +val fun sinr(a : float) returns (o : float) + +(* sqrt() *) +val fun sqrtr(a : float) returns (o : float) \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/mathext.h b/examples/MissionComputer_for_Core/mathext.h new file mode 100644 index 0000000..32eb3e8 --- /dev/null +++ b/examples/MissionComputer_for_Core/mathext.h @@ -0,0 +1,46 @@ +#ifndef MATHEXT_H +#define MATHEXT_H + +/* atan(x) */ +typedef struct atanr_out { + float o; +} atanr_out; + +void atanr_step(float a, atanr_out *o); + +/* acos(x) */ +typedef struct acosr_out { + float o; +} acosr_out; + +void acosr_step(float a, acosr_out *o); + +/* cos(x) */ +typedef struct cosr_out { + float o; +} cosr_out; + +void cosr_step(float a, cosr_out *o); + +/* asin(x) */ +typedef struct asinr_out { + float o; +} asinr_out; + +void asinr_step(float a, asinr_out *o); + +/* sin(x) */ +typedef struct sinr_out { + float o; +} sinr_out; + +void sinr_step(float a, sinr_out *o); + +/* sin(x) */ +typedef struct sqrtr_out { + float o; +} sqrtr_out; + +void sqrtr_step(float a, sqrtr_out *o); + +#endif diff --git a/examples/MissionComputer_for_Core/mc.ept b/examples/MissionComputer_for_Core/mc.ept new file mode 100644 index 0000000..afb5c5b --- /dev/null +++ b/examples/MissionComputer_for_Core/mc.ept @@ -0,0 +1,484 @@ +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 Trackslib.elaboraterdrtrack <>(l22); + l22 = + map 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 Trackslib.selectdetectedtrack <>( + iffdetectedtracks, tracks^ksizeifftracksarray, + CstTracksInit.kinittrack^ksizeifftracksarray); + l40 = map 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) -> pre (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 + 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 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 mc_tracks_fusion_onerdrwithifftracks <>( + l140, l139); + l140 = + map + Trackslib.convertrdrtracktomissiontrack + <>(rdrtracks); + l139 = + map + 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 prio_tracknumbernotinarray <<4>>(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 + 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 prio_setpriorityinmissiontrack <<4>>(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 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 + 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 diff --git a/examples/MissionComputer_for_Core/mc_TypeInputs.epi b/examples/MissionComputer_for_Core/mc_TypeInputs.epi new file mode 100644 index 0000000..fbe4c9b --- /dev/null +++ b/examples/MissionComputer_for_Core/mc_TypeInputs.epi @@ -0,0 +1,10 @@ +type tinputspanel = { + p_slope : float; + p_speed : float; + p_id : int; + p_x0 : float; + p_y0 : float; + p_reset : bool + } + +type tinputspanelarray = tinputspanel^4 diff --git a/examples/MissionComputer_for_Core/mc_TypeLists.epi b/examples/MissionComputer_for_Core/mc_TypeLists.epi new file mode 100644 index 0000000..01067fa --- /dev/null +++ b/examples/MissionComputer_for_Core/mc_TypeLists.epi @@ -0,0 +1,5 @@ +type tpriority = { missionTrackIndex : int; trackNumber : int } + +(* TrackNumbers of the tracks with highest priority, + sorted from the highest priority *) +type tpriorityList = int^4 diff --git a/examples/MissionComputer_for_Core/mc_TypeSensors.epi b/examples/MissionComputer_for_Core/mc_TypeSensors.epi new file mode 100644 index 0000000..acd8fa9 --- /dev/null +++ b/examples/MissionComputer_for_Core/mc_TypeSensors.epi @@ -0,0 +1,3 @@ +type trdrmode = TRdrMode_WIDE | TRdrMode_NARROW + +type tsensorstate = TState_OFF | TState_ON | TState_FAIL \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/mc_ext.c b/examples/MissionComputer_for_Core/mc_ext.c new file mode 100644 index 0000000..55dd0a2 --- /dev/null +++ b/examples/MissionComputer_for_Core/mc_ext.c @@ -0,0 +1,127 @@ +#include +#include +#include "mc_ext.h" + +/*$************************************** +NAME : MC_Tracks_Prio_SortTracks +INPUTS : +InputTrack1 : TMissionTrack +InputTrack2 : TMissionTrack +InputTrack3 : TMissionTrack +InputTrack4 : TMissionTrack +OUPUTS : +OutputTrack1 : TMissionTrack +OutputTrack2 : TMissionTrack +OutputTrack3 : TMissionTrack +OutputTrack4 : TMissionTrack +***************************************$*/ + +void mc_tracks_prio_sorttracks( + const TMissionTrack *InputTrack1, const TMissionTrack *InputTrack2, + const TMissionTrack *InputTrack3, const TMissionTrack *InputTrack4, + mc_tracks_prio_sorttracks_out *out) +{ + TMissionTrack _LO1_newA = *InputTrack1; + TMissionTrack _LO1_newB = *InputTrack1; + TMissionTrack _LO2_newA = *InputTrack1; + TMissionTrack _LO2_newB = *InputTrack1; + TMissionTrack _LO3_newA = *InputTrack1; + TMissionTrack _LO3_newB = *InputTrack1; + TMissionTrack _LO4_newA = *InputTrack1; + TMissionTrack _LO4_newB = *InputTrack1; + TMissionTrack _LO5_newA = *InputTrack1; + TMissionTrack _LO5_newB = *InputTrack1; + TMissionTrack _LO6_newA = *InputTrack1; + TMissionTrack _LO6_newB = *InputTrack1; + + TMissionTrack _LI_A = *InputTrack1; + TMissionTrack _LI_B = *InputTrack2; + + SortBlockPriorities(&_LI_A, &_LI_B, &_LO4_newA, &_LO4_newB); + + _LI_A = *InputTrack3; + _LI_B = *InputTrack4; + SortBlockPriorities(&_LI_A, &_LI_B, &_LO6_newA, &_LO6_newB); + + SortBlockPriorities(&_LO4_newB, &_LO6_newA, &_LO2_newA, &_LO2_newB); + + SortBlockPriorities(&_LO4_newA, &_LO2_newA, &_LO1_newA, &_LO1_newB); + + out->OutputTrack1 = _LO1_newA; + + SortBlockPriorities(&_LO2_newB, &_LO6_newB, &_LO5_newA, &_LO5_newB); + + SortBlockPriorities(&_LO1_newB, &_LO5_newA, &_LO3_newA, &_LO3_newB); + + out->OutputTrack2 = _LO3_newA; + out->OutputTrack3 = _LO3_newB; + out->OutputTrack4 = _LO5_newB; +} + +/* ROLE :, +Sort two mission tracks according to:, +1) their (rate of closing / distance) ratio, +2) target type, +3) detection or not by the Radar */ +void SortBlockPriorities(const TMissionTrack *InputTrackA, const TMissionTrack *InputTrackB, TMissionTrack *OutputTrackA, TMissionTrack *OutputTrackB) +{ + bool bInvertTracks = false; + real vrDivDResultTrackA = 0.0; + real vrDivDResultTrackB = 0.0; + + vrDivDResultTrackA = CalculateVrDivD(InputTrackA->Vr, InputTrackA->D); + vrDivDResultTrackB = CalculateVrDivD(InputTrackB->Vr, InputTrackB->D); + + bInvertTracks = (InputTrackA->targetType == TTargetType_FRIEND); + bInvertTracks = bInvertTracks || !(InputTrackA->detectedByRadar); + if ( ( fabs(vrDivDResultTrackA) < 0.0001 ) && ( fabs(vrDivDResultTrackB) < 0.0001 ) ) { + bInvertTracks = bInvertTracks || + ( (InputTrackA->detectedByRadar) && + (InputTrackB->detectedByRadar) && + ( InputTrackA->D > InputTrackB->D ) ); + + } else { + bInvertTracks = bInvertTracks || + ( (InputTrackA->detectedByRadar) && + (InputTrackB->detectedByRadar) && + (vrDivDResultTrackA < vrDivDResultTrackB) ); + } + + if (bInvertTracks) { + *OutputTrackA = *InputTrackB; + *OutputTrackB = *InputTrackA; + } else { + *OutputTrackA = *InputTrackA; + *OutputTrackB = *InputTrackB; + } +} + +/* ROLE :, +Calculate: result = rate of closing / distance */ +real CalculateVrDivD(const float _I0_Vr, const float _I1_D) +{ + bool bDIsNotZero = (_I1_D > 0.1); + + if (bDIsNotZero) { + return ( _I0_Vr / _I1_D ) ; + } else { + return ( 0.0 ); + } +} + +void rand_step(rand_out *out) +{ + float a = (float)(rand()); + kcg_real b = (float)RAND_MAX; + out->o = a/b; +} + +void int_of_float_step(float a, int_of_float_out *out) +{ + return (int) a; +} + +void float_of_int_step(int a, int_of_float_out *out) +{ + return (float) a; +} diff --git a/examples/MissionComputer_for_Core/mc_ext.epi b/examples/MissionComputer_for_Core/mc_ext.epi new file mode 100644 index 0000000..854ea5d --- /dev/null +++ b/examples/MissionComputer_for_Core/mc_ext.epi @@ -0,0 +1,14 @@ +(* compute each detected track priority, and sort tracks + according to their priority *) +val fun mc_tracks_prio_sorttracks(inputtrack1 : TypeTracks.tmissiontrack; + inputtrack2 : TypeTracks.tmissiontrack; + inputtrack3 : TypeTracks.tmissiontrack; + inputtrack4 : TypeTracks.tmissiontrack) + returns (outputtrack1 : TypeTracks.tmissiontrack; + outputtrack2 : TypeTracks.tmissiontrack; + outputtrack3 : TypeTracks.tmissiontrack; + outputtrack4 : TypeTracks.tmissiontrack) + +val fun int_of_float(a:float) returns (o:int) +val fun float_of_int(a:int) returns (o:float) +val fun rand() returns (output1 : float) \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/mc_ext.h b/examples/MissionComputer_for_Core/mc_ext.h new file mode 100644 index 0000000..1abda07 --- /dev/null +++ b/examples/MissionComputer_for_Core/mc_ext.h @@ -0,0 +1,48 @@ +#ifndef MC_EXT_H +#define MC_EXT_H + +#include "typeArray_types.h" + +typedef struct mc_tracks_prio_sorttracks_out { + TMissionTrack OutputTrack1; + TMissionTrack OutputTrack2; + TMissionTrack OutputTrack3; + TMissionTrack OutputTrack4; +} mc_tracks_prio_sorttracks_out; + +/* =============== */ +/* CYCLIC FUNCTION */ +/* =============== */ +void mc_tracks_prio_sorttracks( + const TMissionTrack *InputTrack1, const TMissionTrack *InputTrack2, + const TMissionTrack *InputTrack3, const TMissionTrack *InputTrack4, + mc_tracks_prio_sorttracks_out *out); + +void SortBlockPriorities(const TMissionTrack *InputTrackA, const TMissionTrack *InputTrackB, TMissionTrack *OutputTrackA, TMissionTrack *OutputTrackB); + +real CalculateVrDivD(const float _I0_Vr, const float _I1_D); + + +/* rand() */ +typedef struct { + float o; +} rand_out; + +void rand_step(rand_out *out); + +/* int_of_float */ +typedef struct { + int o; +} int_of_float_out; + +void int_of_float_step(float a, int_of_float_out *out); + +/* float_of_int */ +typedef struct { + float o; +} float_of_int_out; + +void float_of_int_step(int a, float_of_int_out *out); + +#endif + diff --git a/examples/MissionComputer_for_Core/trackslib.ept b/examples/MissionComputer_for_Core/trackslib.ept new file mode 100644 index 0000000..62927f9 --- /dev/null +++ b/examples/MissionComputer_for_Core/trackslib.ept @@ -0,0 +1,419 @@ + (* calculate arctan(y/x) *) +open TypeBase +open TypeTracks + + node myarctan(y, x : float) returns (atan : float) + var l6 : float; l4 : bool; l1 : float; + let + atan = + if l4 + then if x <. 0.0 then CstPhysics.pi +. l1 else l1 + else CstPhysics.pi /. 2.0 *. Math.sign(y); + (* l6 = (activate div every l4 initial default 0.0)(y, x); *) + l6 = if l4 then y /. x else 0.0 -> pre l6; + l4 = Math.abs(x) >. 0.1; + l1 = Mathext.atanr(l6); + tel + +(* compute if a given track is equal to one of the mission tracks + belonging to the mission track array at the previous tick *) +fun missiontrackequalsprevious(previousone, actualone : TypeTracks.tmissiontrack) + returns (equal : bool) +let + equal = + 0 <> previousone.m_id & previousone.m_id = actualone.m_id or + Math.abs(previousone.m_pos.x -. actualone.m_pos.x) <. 100.0 & + Math.abs(previousone.m_pos.y -. actualone.m_pos.y) <. 100.0 & + not (Math.abs(previousone.m_pos.x) <. 0.1 & + Math.abs(previousone.m_pos.y) <. 0.1 & + Math.abs(actualone.m_pos.x) <. 0.1 & + Math.abs(actualone.m_pos.y) <. 0.1 ) +tel + + (* compute track visibility (appearance on radar screen) + according to track position and speed *) +fun calctrackvisible1(position : TypeBase.tposition; + speed : TypeBase.tspeed) + returns (trackvisible : bool) +let + trackvisible = + not (Math.abs(position.x) <. 0.001 & Math.abs(position.y) <. 0.001 & + Math.abs(speed.sx) <. 0.001 & + Math.abs(speed.sy) <. 0.001); +tel + +fun missiontrackexist1(acc_tracknumber : int; + missiontrack, + previousmissiontrack : TypeTracks.tmissiontrack) + returns (tracknumbertoset : int) +let + tracknumbertoset = + if missiontrackequalsprevious(missiontrack, previousmissiontrack) & + 0 <> previousmissiontrack.m_tracknumber + then previousmissiontrack.m_tracknumber + else acc_tracknumber; +tel + +(* compute if a given track is equal to one of the mission tracks + belonging to the mission track array at the previous tick *) +fun missiontrackequalsprevious_orig(previousone, actualone : TypeTracks.tmissiontrack) + returns (equal : bool) +var l43 : bool; +let + l43 = previousone.m_tracknumber <> 0; + equal = + l43 & + (l43 & 0 <> previousone.m_id & previousone.m_id = actualone.m_id or + Math.abs(previousone.m_pos.x -. actualone.m_pos.x) <. 100.0 & + Math.abs(previousone.m_pos.y -. actualone.m_pos.y) <. 100.0 & + not (Math.abs(previousone.m_pos.x) <. 0.1 & + Math.abs(previousone.m_pos.y) <. 0.1 & + Math.abs(actualone.m_pos.x) <. 0.1 & + Math.abs(actualone.m_pos.y) <. 0.1)); +tel + +fun util_radtodeg(input1 : float) returns (output1 : float) +let + output1 = input1 /. (2.0 *. CstPhysics.pi) *. 360.0; +tel + +fun util_degtorad(input1 : float) returns (output1 : float) +let + output1 = 2.0 *. CstPhysics.pi *. input1 /. 360.0; +tel + +(* if speedabs is small (speed.x and speed.y are also small), trackangle is set to 0. + otherwise, trackangle is computed to be in the range [-180, 180] + degrees thanks to the acosr; sign is given, from the asinr. *) +fun calctrackangle(speed : TypeBase.tspeed; speedabs : TypeBase.tmetresseconde) + returns (trackangle : float) +var l51 : bool; l48, l47 : float; +let + trackangle = + util_radtodeg(if l51 then 0.0 else Mathext.acosr(l47) *. l48) *. + (l48 *. + Math.sign(Mathext.asinr(speed.sy /. (if l51 then 1.0 else speedabs)))); + l51 = speedabs <. 0.01; + l48 = Math.sign(l47); + l47 = speed.sx /. (if l51 then 1.0 else speedabs); +tel + +(* compute track visibility (appearance on radar screen) + according to track position *) +fun calctrackvisible(position : TypeBase.tposition) + returns (trackvisible : bool) +let + trackvisible = + not (Math.abs(position.x) <. 0.001 & Math.abs(position.y) <. 0.001); +tel + +fun missiontrackexist( missiontrack, + previousmissiontrack : TypeTracks.tmissiontrack; + acc_tracknumber : int) + returns (tracknumbertoset : int) +let + tracknumbertoset = + if missiontrackequalsprevious(missiontrack, previousmissiontrack) + then previousmissiontrack.m_tracknumber + else acc_tracknumber; +tel + +(* calculate: result = rate of closing / distance *) +node calculatevrdivd(vr, d : float) returns (result : float) +var l13 : float; l11 : bool; +let + result = if l11 then l13 else 0.0; +(* l13 = (activate div every l11 initial default 0.0)(vr, d); *) + l13 = if l11 then vr /. d else 0.0 -> pre l13; + l11 = d >. 0.1; +tel + +(* sort two mission tracks according to: + 1) their (rate of closing / distance) ratio + 2) target type + 3) detection or not by the radar *) +node trackalowerprioritythanb(a, b : TypeTracks.tmissiontrack) + returns (prioritary : bool) +let + prioritary = + a.m_targettype = TypeBase.Ttargettype_friend or not a.m_detectedbyradar or + a.m_detectedbyradar & + calculatevrdivd(a.m_sr, a.m_d) <. calculatevrdivd(b.m_sr, b.m_d) & + b.m_detectedbyradar; +tel + +(* compute if two tracks speeds are equal *) +fun comparespeeds(speed1, speed2 : TypeBase.tspeed) + returns (equal : bool) +let + equal = + Math.abs(speed1.sx -. speed2.sx) <. 1.0 & + Math.abs(speed1.sy -. speed2.sy) <. 1.0; +tel + +(* compute a "prioritized" track number according to its + priority and target type *) +fun calculateprioritizedtracknb(missiontrack : TypeTracks.tmissiontrack) + returns (prioritizedtracknb : int) +let + prioritizedtracknb = + if missiontrack.m_targettype <> TypeBase.Ttargettype_friend & + missiontrack.m_priority <> 0 + then missiontrack.m_tracknumber + else 0; +tel + +(* sort two real inputs *) +fun sortreals(a, b : float) returns (newa, newb : float) +var l2 : bool; +let + l2 = a <. b; + newb = if l2 then a else b; + newa = if l2 then b else a; +tel + +(* compute if two tracks positions are equal *) +fun comparepositions(pos1, pos2 : TypeBase.tposition) + returns (equal : bool) +let + equal = + Math.abs(pos1.x -. pos2.x) <. 0.1 & Math.abs(pos1.y -. pos2.y) <. 0.1; +tel + +(* compute if two tracks are equal (according to their position + and speed) *) +fun comparetracks(pos1, pos2 : TypeBase.tposition; + v1, v2 : TypeBase.tspeed) + returns (equal : bool) +let + equal = comparepositions(pos1, pos2) & comparespeeds(v1, v2); +tel + + +(* set the track number of a mission track *) +fun setmissiontracknumber(missiontrack : TypeTracks.tmissiontrack; number : int) + returns (newmissiontrack : TypeTracks.tmissiontrack) +let + newmissiontrack = { missiontrack with .m_tracknumber = number }; +tel + +(* compute if a mission track is null (or empty) according to + its position and speed *) +fun missiontrackisnull(missiontrack : TypeTracks.tmissiontrack) + returns (isnull : bool) +let + isnull = + comparetracks(missiontrack.m_pos, CstBaseInit.kInitPosition, + missiontrack.m_speed, CstBaseInit.kInitSpeed); +tel + +(* calculate the new track number for a mission track, according to: + 1) the mission track data + 2) the previous mission tracks array + 3) the current (highest) track number *) +fun calculatemissiontracknumber( + previousmissiontracks : TypeArray.tmissiontracksarray; + missiontrack : TypeTracks.tmissiontrack; + currenttracknumber : int) + returns (newmissiontrack : TypeTracks.tmissiontrack; + newtracknumber : int) +var setnewtracknumber : bool; previoustracknumber : int; +let + setnewtracknumber = + previoustracknumber = 0 & not missiontrackisnull(missiontrack); + newtracknumber = + if setnewtracknumber then currenttracknumber + 1 else currenttracknumber; + previoustracknumber = + fold missiontrackexist <> + (missiontrack^TypeArray.ksizemissiontracksarray, + previousmissiontracks, 0); + newmissiontrack = + setmissiontracknumber(missiontrack, if setnewtracknumber + then newtracknumber + else previoustracknumber); +tel + +(* compute a mission track target type according to its identifier *) +fun calculatetracktargettypefromid(id : int) + returns (targettype : TypeBase.ttargettype) +let + targettype = + if 0 = id + then Typebase.Ttargettype_unknown + else if id <= 500 + then TypeBase.Ttargettype_friend + else TypeBase.Ttargettype_foe; +tel + +(* calculate the derivative of a value x(n) according to its + ante-previous value x(n-2) *) +node myderivative(in, period : float) returns (out : float) +var l2 : float; +let + (* l2 = fby (in; 2; 0.0); *) + l2 = 0.0 fby (0.0 fby in); + out = + if Math.abs(l2) <. 0.1 or Math.abs(in) <. 0.1 + then 0.0 + else 0.0 -> (in -. l2) /. (2.0 *. period); + tel + +(* calculate a track speed vector according to the position vector *) +node calculatetrackspeedfrompos(position : TypeBase.tposition) + returns (speed : TypeBase.tspeed) +let + speed = + { sx = myderivative(position.x, CstPhysics.t); + sy = myderivative(position.y, CstPhysics.t) }; +tel + +(* generate the (up to 2) tracks detected by a sensor (radar + or iff) from the environment (made of 4 tracks) *) +fun selectdetectedtrack(index : int; + tracks : TypeArray.ttracksarray; + defaulttrack : TypeTracks.ttrack) + returns (trackselected : TypeTracks.ttrack) +let + trackselected = tracks.[index] default defaulttrack; +tel + +(* set the priority of a mission track *) +fun setmissiontrackpriority(missiontrack : TypeTracks.tmissiontrack; + priority : int) + returns (newmissiontrack : TypeTracks.tmissiontrack) +let + newmissiontrack = + { missiontrack with .m_priority = + if missiontrack.m_detectedbyradar then priority else 0 } +tel + +(* invert two mission tracks if the first one is null (or empty) *) +fun sortblockmissiontrack(a, b : TypeTracks.tmissiontrack) + returns (newa, newb : TypeTracks.tmissiontrack) +var l7 : bool; +let + l7 = missiontrackisnull(a); + newb = if l7 then a else b; + newa = if l7 then b else a; +tel + +(* sort two mission tracks according to: + 1) their (rate of closing / distance) ratio + 2) target type + 3) detection or not by the radar *) +node sortblockpriorities(a, b : TypeTracks.tmissiontrack) + returns (newa, newb : TypeTracks.tmissiontrack) +var l25 : bool; +let + l25 = trackalowerprioritythanb(a, b); + newb = if l25 then a else b; + newa = if l25 then b else a; +tel + +(* convert an iff track (position + identifier) into a mission + track (position + speed + distance + rate of closing + + detected by radar/iff + tracknumber + target type) *) +node convertifftracktomissiontrack(ifftrack : TypeTracks.tifftrack) + returns (missiontrack : TypeTracks.tmissiontrack) +let + missiontrack = + { m_pos = ifftrack.i_pos; + m_speed = if CstBaseInit.kInitPosition = ifftrack.i_pos + then CstBaseInit.kInitSpeed + else calculatetrackspeedfrompos(ifftrack.i_pos); + m_id = ifftrack.i_id; + m_priority = 0; + m_d = 0.0; + m_sabs = 0.0; + m_sr = 0.0; + m_detectedbyradar = false; + m_detectedbyiff = not (ifftrack.i_pos = CstBaseInit.kInitPosition & + ifftrack.i_id = 0); + m_tracknumber = 0; + m_targettype = calculatetracktargettypefromid(ifftrack.i_id); + m_isvisible = calctrackvisible(ifftrack.i_pos); + m_angle = 0.0 }; +tel + +(* convert an radar track (position + speed + distance + + rate of closing) into a mission track (position + speed + + distance + rate of closing + detected by radar/iff + + tracknumber + target type) *) +fun convertrdrtracktomissiontrack(rdrtrack : TypeTracks.trdrtrack) + returns (missiontrack : TypeTracks.tmissiontrack) +let + missiontrack = + { m_pos = rdrtrack.r_pos; + m_speed = rdrtrack.r_s; + m_id = 0; + m_priority = 0; + m_d = rdrtrack.r_d; + m_sabs = rdrtrack.r_sabs; + m_sr = rdrtrack.r_sr; + m_detectedbyradar = not (rdrtrack.r_pos = CstBaseInit.kInitPosition & + rdrtrack.r_s = CstBaseInit.kInitSpeed & + rdrtrack.r_d = 0.0 & + rdrtrack.r_sabs = 0.0 & + rdrtrack.r_sr = 0.0); + m_detectedbyiff = false; + m_tracknumber = 0; + m_targettype = TypeBase.Ttargettype_unknown; + m_isvisible = calctrackvisible(rdrtrack.r_pos); + m_angle = calctrackangle(rdrtrack.r_s, rdrtrack.r_sabs) }; +tel + +(* calculate the magnitude of a vector (2d) *) +fun vectnorme(a, b : float) returns (c : float) +let + c = Mathext.sqrtr(a *. a +. b *. b); +tel + +(* extract the x and y (position) values from a track (ttrack type) *) +fun extracttrackposxy(track : TypeTracks.ttrack) + returns (x, y : TypeBase.tmetres) +let + y = track.t_pos.y; + x = track.t_pos.x; +tel + +(* elaborate radar track data (position, speed, distance, rate of closing) + according to an environment track (position only) *) +node elaboraterdrtrack(track : TypeTracks.ttrack) + returns (rdrtrack : TypeTracks.trdrtrack) +var d, v, vr, vx, vy, x, y : float; l142 : TypeBase.tspeed; +let + (*activate ifblock1 if d = 0.0 + then vr = 0.0; + else var xnorm, ynorm : real; + let + ynorm = y / d; + xnorm = x / d; + vr = - (vx * xnorm + vy * ynorm); + tel + returns vr;*) + switch d = 0.0 + | true do vr = 0.0 + | false + var xnorm, ynorm : float; + do + ynorm = y /. d; + xnorm = x /. d; + vr = -. (vx *. xnorm +. vy *. ynorm); + end; + + (x, y) = extracttrackposxy(track); + rdrtrack = + { r_pos = { x = x; + y = y }; + r_s = { sx = vx; + sy = vy }; + r_d = d; + r_sabs = v; + r_sr = vr }; + v = vectnorme(vx, vy); + d = vectnorme(x, y); + vy = l142.sy; + vx = l142.sx; + l142 = calculatetrackspeedfrompos({ x = x; y = y }); +tel + diff --git a/examples/MissionComputer_for_Core/typeArray.epi b/examples/MissionComputer_for_Core/typeArray.epi new file mode 100644 index 0000000..270d53d --- /dev/null +++ b/examples/MissionComputer_for_Core/typeArray.epi @@ -0,0 +1,18 @@ +const ksizetracksarray : int = 10 +const ksizeifftracksarray : int = 3 +const ksizerdrtracksarray : int = 5 +const ksizemissiontracksarray : int = + ksizeifftracksarray + ksizerdrtracksarray + + +type trdrtracksarray = TypeTracks.trdrtrack^ksizerdrtracksarray + +type tifftracksarray = TypeTracks.tifftrack^ksizeifftracksarray + +type tdetectedifftracksarray = int^ksizeifftracksarray + +type tdetectedrdrtracksarray = int^ksizerdrtracksarray + +type ttracksarray = TypeTracks.ttrack^ksizetracksarray + +type tmissiontracksarray = TypeTracks.tmissiontrack^ksizemissiontracksarray \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/typeBase.epi b/examples/MissionComputer_for_Core/typeBase.epi new file mode 100644 index 0000000..9457355 --- /dev/null +++ b/examples/MissionComputer_for_Core/typeBase.epi @@ -0,0 +1,8 @@ +type tid = int +type tpriority = int +type tmetres = float +type tmetresseconde = float +type tposition = { x : tmetres; y : tmetres } +type tspeed = { sx : tmetresseconde; sy : tmetresseconde } + +type ttargettype = Ttargettype_unknown | Ttargettype_friend | Ttargettype_foe \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/typeTracks.epi b/examples/MissionComputer_for_Core/typeTracks.epi new file mode 100644 index 0000000..d4d23ef --- /dev/null +++ b/examples/MissionComputer_for_Core/typeTracks.epi @@ -0,0 +1,27 @@ +type trdrtrack = { + r_pos : TypeBase.tposition; + r_s : TypeBase.tspeed; + r_d : TypeBase.tmetres; + r_sabs : TypeBase.tmetresseconde; + r_sr : TypeBase.tmetresseconde + } + +type tifftrack = { i_pos : TypeBase.tposition; i_id : TypeBase.tid } + +type ttrack = { t_pos : TypeBase.tposition; t_id : TypeBase.tid } + +type tmissiontrack = { + m_pos : TypeBase.tposition; + m_speed : TypeBase.tspeed; + m_id : TypeBase.tid; + m_priority : TypeBase.tpriority; + m_d : TypeBase.tmetres; + m_sabs : TypeBase.tmetresseconde; + m_sr : TypeBase.tmetresseconde; + m_detectedbyradar : bool; + m_detectedbyiff : bool; + m_tracknumber : int; + m_targettype : TypeBase.ttargettype; + m_isvisible : bool; + m_angle : float + } \ No newline at end of file diff --git a/examples/MissionComputer_for_Core/verif.ept b/examples/MissionComputer_for_Core/verif.ept new file mode 100644 index 0000000..1ce1671 --- /dev/null +++ b/examples/MissionComputer_for_Core/verif.ept @@ -0,0 +1,5 @@ +(* This node implements the "implies" logical operator (not(A) or B). *) +fun implies(a, b : bool) returns (c : bool) +let + c = (not a) or b; +tel