Scade example with linear annotations

This commit is contained in:
Cédric Pasteur 2011-09-09 16:31:41 +02:00
parent 4d912e9349
commit 4648f7fbab
25 changed files with 1589 additions and 0 deletions

66
examples/MC_memalloc/build Executable file
View file

@ -0,0 +1,66 @@
#!/bin/bash
HEPTC=../../compiler/heptc.byte
HEPTC_OPTIONS=""
GCC=gcc
GCC_OPTIONS="-g -O2 -std=c99 -I ../../../lib/c"
VERBOSE=1
interfaces=(typeBase.epi typeTracks.epi typeArray.epi cstArrayInit.epi cstBaseInit.epi cstPhysics.epi cstTracksInit.epi mc_TypeInputs.epi mc_TypeLists.epi mc_TypeSensors.epi)
ext_libs=(mc_ext.epi mathext.epi)
sources=(math.ept trackslib.ept digital.ept mc.ept dv.ept verif.ept debug.ept)
exit_if_failed() {
if [ $? != 0 ]; then
exit $?
fi
}
for f in ${interfaces[@]}; do
if [ $VERBOSE ] ; then
echo "**** Compiling interface: $f *******"
fi
$HEPTC $HEPTC_OPTIONS -target c $f
exit_if_failed
done
for f in ${ext_libs[@]}; do
if [ $VERBOSE ] ; then
echo "**** Compiling external lib: $f *******"
fi
$HEPTC $HEPTC_OPTIONS $f
exit_if_failed
done
for f in ${sources[@]}; do
if [ $VERBOSE ] ; then
echo "**** Compiling source file: $f *******"
fi
$HEPTC $HEPTC_OPTIONS -target c $f
exit_if_failed
done
#$HEPTC $HEPTC_OPTIONS -target c -s dv_fighterdebug debug.ept
source_dirs=(debug_c dv_c digital_c math_c mc_c trackslib_c verif_c typeBase_c typeTracks_c typeArray_c cstArrayInit_c cstBaseInit_c cstPhysics_c cstTracksInit_c mc_TypeInputs_c mc_TypeLists_c mc_TypeSensors_c)
other_files=(mathext.c mathext.h mc_ext.c mc_ext.h main.c)
if [ -d _build ]; then
rm -r _build
fi
mkdir _build
cd _build
for f in ${source_dirs[@]}; do
cp ../$f/* .
done
for f in ${other_files[@]}; do
cp ../$f .
done
$GCC $GCC_OPTIONS *.c -o mission_control
exit_if_failed
cp mission_control ..
cd ..

View file

@ -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

View file

@ -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 }

View file

@ -0,0 +1,4 @@
const nm : float = 1852.0
const t : float = 0.01
const pi : float = 3.141592

View file

@ -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 }

View file

@ -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

View file

@ -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

View file

@ -0,0 +1,83 @@
open TypeArray
open TypeTracks
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<<ksizemissiontracksarray>> 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<<ksizemissiontracksarray>> 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<<ksizemissiontracksarray>> 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

View file

@ -0,0 +1,75 @@
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "debug.h"
void print_mission_track(TypeArray__tmissiontracksarray mt) {
for(int i = 0; i < TypeArray__ksizemissiontracksarray; i++) {
printf("track nb %d\n", i);
printf("pos: x=%f -- y=%f \n", mt[i].m_pos.x, mt[i].m_pos.y);
printf("speed: sx=%f -- sy=%f\n", mt[i].m_speed.sx, mt[i].m_speed.sy);
}
}
/* fighterdebug(res, rdronoffclicked, iffonoffclicked)
res est le reset: mettre a 1 le premier coup puis a 0
*/
int main(int argc, char** argv) {
int step_c;
int step_max;
Debug__fighterdebug_mem _mem;
Debug__fighterdebug_out _res;
int res;
int rdronoffclicked;
int iffonoffclicked;
step_c = 0;
step_max = 0;
if ((argc==2)) {
step_max = atoi(argv[1]);
};
Debug__fighterdebug_reset(&_mem);
Debug__fighterdebug_step(true, false, false, &_res, &_mem);
Debug__fighterdebug_step(false, false, false, &_res, &_mem);
Debug__fighterdebug_step(false, true, false, &_res, &_mem);
Debug__fighterdebug_step(false, false, false, &_res, &_mem);
Debug__fighterdebug_step(false, false, false, &_res, &_mem);
Debug__fighterdebug_step(false, true, false, &_res, &_mem);
Debug__fighterdebug_step(false, false, true, &_res, &_mem);
Debug__fighterdebug_step(false, false, false, &_res, &_mem);
Debug__fighterdebug_step(false, false, true, &_res, &_mem);
Debug__fighterdebug_step(false, false, false, &_res, &_mem);
printf("init:\n");
printf("=> \n");
print_mission_track(_res.missiontracks);
fflush(stdout);
while ((!(step_max)||(step_c<step_max))) {
step_c = (step_c+1);
/*
printf("res ? ");
scanf("%d", &res);;
printf("rdronoffclicked ? ");
scanf("%d", &rdronoffclicked);;
printf("iffonoffclicked ? ");
scanf("%d", &iffonoffclicked);;
*/
Debug__fighterdebug_step(res, rdronoffclicked, iffonoffclicked, &_res,
&_mem);
printf("=> \n");
print_mission_track(_res.missiontracks);
fflush(stdout);
};
return 0;
}

View file

@ -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

View file

@ -0,0 +1,14 @@
#include <math.h>
#include "mathext.h"
#define WRAP_FUN_DEF(FNAME, CNAME, TY_IN, TY_OUT) \
void FNAME ## _step(TY_IN a, FNAME ## _out *out) { \
out->o = CNAME(a); \
}
WRAP_FUN_DEF(Mathext__atanr, atan, float, float)
WRAP_FUN_DEF(Mathext__acosr, acos, float, float)
WRAP_FUN_DEF(Mathext__cosr, cos, float, float)
WRAP_FUN_DEF(Mathext__asinr, asin, float, float)
WRAP_FUN_DEF(Mathext__sinr, sin, float, float)
WRAP_FUN_DEF(Mathext__sqrtr, sqrt, float, float)

View file

@ -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)

View file

@ -0,0 +1,18 @@
#ifndef MATHEXT_H
#define MATHEXT_H
#define WRAP_FUN_DECL(FNAME, TY_IN, TY_OUT) \
typedef struct { \
TY_OUT o; \
} FNAME ## _out; \
\
void FNAME ## _step(TY_IN, FNAME ## _out *)
WRAP_FUN_DECL(Mathext__atanr, float, float);
WRAP_FUN_DECL(Mathext__acosr, float, float);
WRAP_FUN_DECL(Mathext__cosr, float, float);
WRAP_FUN_DECL(Mathext__asinr, float, float);
WRAP_FUN_DECL(Mathext__sinr, float, float);
WRAP_FUN_DECL(Mathext__sqrtr, float, float);
#endif

482
examples/MC_memalloc/mc.ept Normal file
View file

@ -0,0 +1,482 @@
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 : TypeTracks.tmissiontrack at r1;
rdrmissiontrack : TypeTracks.tmissiontrack at r2)
returns (newiffmissiontrack : TypeTracks.tmissiontrack at r1;
newrdrmissiontrack : TypeTracks.tmissiontrack at r2)
var l90 : bool;
let
newrdrmissiontrack =
if l90
then
{ ({ ({ ({ rdrmissiontrack with .m_id = iffmissiontrack.m_id })
with .m_detectedbyiff = iffmissiontrack.m_detectedbyiff })
with .m_tracknumber = 0 })
with .m_targettype = iffmissiontrack.m_targettype }
else rdrmissiontrack;
(*
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 at r1;
ifftracks : TypeTracks.tmissiontrack^ksizeifftracksarray at r2)
returns (fusionnedrdrtrack : TypeTracks.tmissiontrack at r1;
fusionnedifftracks : TypeTracks.tmissiontrack^ksizeifftracksarray at r2)
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 at rdr;
l139 : TypeTracks.tmissiontrack^ksizeifftracksarray at iff;
let
missiontracks = mergedrdrtracks @ mergedifftracks;
(mergedrdrtracks, mergedifftracks) =
mapfold<<ksizerdrtracksarray>> mc_tracks_fusion_onerdrwithifftracks(l140, l139);
init<<rdr>> l140 = map<<ksizerdrtracksarray>> Trackslib.convertrdrtracktomissiontrack(rdrtracks);
init<<iff>> 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 at r)
returns (prioritymissiontrack : TypeTracks.tmissiontrack at r)
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 at prio;
indexpriority : int)
returns (newprioritiesarray : Mc_TypeLists.tpriorityList at prio)
var initmtrack, missiontrackwithhighestpriority : TypeTracks.tmissiontrack at r;
let
newprioritiesarray =
[ prioritiesarray with [indexpriority] =
missiontrackwithhighestpriority.m_tracknumber ];
init<<r>> initmtrack = CstTracksInit.kinitmissiontrack;
missiontrackwithhighestpriority =
fold<<ksizemissiontracksarray>> prio_selecthighestprioritynotinpriorityarray(
missiontracks,
prioritiesarray^ksizemissiontracksarray, initmtrack);
tel
fun prio_setpriorityinmissiontrack(prioritytracknumber : int;
priorityindex : int;
missiontrack : TypeTracks.tmissiontrack at r)
returns (missiontrackwithprio : TypeTracks.tmissiontrack at r)
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 at r)
returns (missiontrackwithprio : TypeTracks.tmissiontrack at r)
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 at r)
returns (missiontrackswithprio : TypeArray.tmissiontracksarray at r)
var initprio, prioritytracknumbers : Mc_TypeLists.tpriorityList at prio;
let
missiontrackswithprio =
map<<ksizemissiontracksarray>> prio_setpriorityinmissiontrackarray
<(prioritytracknumbers)> (missiontracks);
init<<prio>> initprio = 0^4;
prioritytracknumbers =
prio_selectprioritarymissiontracks(missiontracks,
prio_selectprioritarymissiontracks(missiontracks,
prio_selectprioritarymissiontracks(missiontracks,
prio_selectprioritarymissiontracks(missiontracks, initprio, 0), 1), 2),
3);
tel
(* associate a track number to each detected track *)
node mc_tracks_tracknumber(withouttracknb : TypeArray.tmissiontracksarray at r)
returns (withtracknumber : TypeArray.tmissiontracksarray at r)
var l81 : int;
let
(withtracknumber, l81) =
mapfold<<ksizemissiontracksarray>> Trackslib.calculatemissiontracknumber
<(kinitmissiontrackarray -> pre withtracknumber)>
(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)
var fused_tracks : TypeArray.tmissiontracksarray at r;
let
init<<r>> fused_tracks = mc_tracks_fusion(rdrtracks, ifftracks);
missiontracks = mc_tracks_prio(mc_tracks_tracknumber(fused_tracks));
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

View file

@ -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

View file

@ -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

View file

@ -0,0 +1,3 @@
type trdrmode = TRdrMode_WIDE | TRdrMode_NARROW
type tsensorstate = TState_OFF | TState_ON | TState_FAIL

View file

@ -0,0 +1,127 @@
#include <math.h>
#include <stdlib.h>
#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 TypeTracks__tmissiontrack *InputTrack1, const TypeTracks__tmissiontrack *InputTrack2,
const TypeTracks__tmissiontrack *InputTrack3, const TypeTracks__tmissiontrack *InputTrack4,
Mc_ext__mc_tracks_prio_sorttracks_out *out)
{
TypeTracks__tmissiontrack _LO1_newA = *InputTrack1;
TypeTracks__tmissiontrack _LO1_newB = *InputTrack1;
TypeTracks__tmissiontrack _LO2_newA = *InputTrack1;
TypeTracks__tmissiontrack _LO2_newB = *InputTrack1;
TypeTracks__tmissiontrack _LO3_newA = *InputTrack1;
TypeTracks__tmissiontrack _LO3_newB = *InputTrack1;
TypeTracks__tmissiontrack _LO4_newA = *InputTrack1;
TypeTracks__tmissiontrack _LO4_newB = *InputTrack1;
TypeTracks__tmissiontrack _LO5_newA = *InputTrack1;
TypeTracks__tmissiontrack _LO5_newB = *InputTrack1;
TypeTracks__tmissiontrack _LO6_newA = *InputTrack1;
TypeTracks__tmissiontrack _LO6_newB = *InputTrack1;
TypeTracks__tmissiontrack _LI_A = *InputTrack1;
TypeTracks__tmissiontrack _LI_B = *InputTrack2;
Mc_ext__SortBlockPriorities(&_LI_A, &_LI_B, &_LO4_newA, &_LO4_newB);
_LI_A = *InputTrack3;
_LI_B = *InputTrack4;
Mc_ext__SortBlockPriorities(&_LI_A, &_LI_B, &_LO6_newA, &_LO6_newB);
Mc_ext__SortBlockPriorities(&_LO4_newB, &_LO6_newA, &_LO2_newA, &_LO2_newB);
Mc_ext__SortBlockPriorities(&_LO4_newA, &_LO2_newA, &_LO1_newA, &_LO1_newB);
out->OutputTrack1 = _LO1_newA;
Mc_ext__SortBlockPriorities(&_LO2_newB, &_LO6_newB, &_LO5_newA, &_LO5_newB);
Mc_ext__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 Mc_ext__SortBlockPriorities(const TypeTracks__tmissiontrack *InputTrackA, const TypeTracks__tmissiontrack *InputTrackB, TypeTracks__tmissiontrack *OutputTrackA, TypeTracks__tmissiontrack *OutputTrackB)
{
bool bInvertTracks = false;
float vrDivDResultTrackA = 0.0;
float vrDivDResultTrackB = 0.0;
vrDivDResultTrackA = Mc_ext__CalculateVrDivD(InputTrackA->m_sr, InputTrackA->m_d);
vrDivDResultTrackB = Mc_ext__CalculateVrDivD(InputTrackB->m_sr, InputTrackB->m_d);
bInvertTracks = (InputTrackA->m_targettype == TypeBase__Ttargettype_unknown);
bInvertTracks = bInvertTracks || !(InputTrackA->m_detectedbyradar);
if ( ( fabs(vrDivDResultTrackA) < 0.0001 ) && ( fabs(vrDivDResultTrackB) < 0.0001 ) ) {
bInvertTracks = bInvertTracks ||
( (InputTrackA->m_detectedbyradar) &&
(InputTrackB->m_detectedbyradar) &&
( InputTrackA->m_d > InputTrackB->m_d ) );
} else {
bInvertTracks = bInvertTracks ||
( (InputTrackA->m_detectedbyradar) &&
(InputTrackB->m_detectedbyradar) &&
(vrDivDResultTrackA < vrDivDResultTrackB) );
}
if (bInvertTracks) {
*OutputTrackA = *InputTrackB;
*OutputTrackB = *InputTrackA;
} else {
*OutputTrackA = *InputTrackA;
*OutputTrackB = *InputTrackB;
}
}
/* ROLE :,
Calculate: result = rate of closing / distance */
float Mc_ext__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 Mc_ext__rand_step(Mc_ext__rand_out *out)
{
float a = (float)(rand());
float b = (float)RAND_MAX;
out->o = a/b;
}
void Mc_ext__int_of_float_step(float a, Mc_ext__int_of_float_out *out)
{
out->o = (int) a;
}
void Mc_ext__float_of_int_step(int a, Mc_ext__float_of_int_out *out)
{
out->o = (float) a;
}

View file

@ -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 (o : float)

View file

@ -0,0 +1,48 @@
#ifndef MC_EXT_H
#define MC_EXT_H
#include "typeArray.h"
typedef struct Mc_ext__mc_tracks_prio_sorttracks_out {
TypeTracks__tmissiontrack OutputTrack1;
TypeTracks__tmissiontrack OutputTrack2;
TypeTracks__tmissiontrack OutputTrack3;
TypeTracks__tmissiontrack OutputTrack4;
} Mc_ext__mc_tracks_prio_sorttracks_out;
/* =============== */
/* CYCLIC FUNCTION */
/* =============== */
void Mc_ext__mc_tracks_prio_sorttracks(
const TypeTracks__tmissiontrack *InputTrack1, const TypeTracks__tmissiontrack *InputTrack2,
const TypeTracks__tmissiontrack *InputTrack3, const TypeTracks__tmissiontrack *InputTrack4,
Mc_ext__mc_tracks_prio_sorttracks_out *out);
void Mc_ext__SortBlockPriorities(const TypeTracks__tmissiontrack *InputTrackA, const TypeTracks__tmissiontrack *InputTrackB, TypeTracks__tmissiontrack *OutputTrackA, TypeTracks__tmissiontrack *OutputTrackB);
float Mc_ext__CalculateVrDivD(const float _I0_Vr, const float _I1_D);
/* rand() */
typedef struct {
float o;
} Mc_ext__rand_out;
void Mc_ext__rand_step(Mc_ext__rand_out *out);
/* int_of_float */
typedef struct {
int o;
} Mc_ext__int_of_float_out;
void Mc_ext__int_of_float_step(float a, Mc_ext__int_of_float_out *out);
/* float_of_int */
typedef struct {
float o;
} Mc_ext__float_of_int_out;
void Mc_ext__float_of_int_step(int a, Mc_ext__float_of_int_out *out);
#endif

View file

@ -0,0 +1,429 @@
(* 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 at r; number : int)
returns (newmissiontrack : TypeTracks.tmissiontrack at r)
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 at r;
currenttracknumber : int)
returns (newmissiontrack : TypeTracks.tmissiontrack at r;
newtracknumber : int)
var setnewtracknumber : bool; previoustracknumber : int;
let
setnewtracknumber =
previoustracknumber = 0 & not missiontrackisnull(missiontrack);
newtracknumber =
if setnewtracknumber then currenttracknumber + 1 else currenttracknumber;
previoustracknumber =
fold <<TypeArray.ksizemissiontracksarray>> missiontrackexist
<(missiontrack)>
(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(inv, period : float) returns (o : float)
var l2 : float;
let
(* l2 = fby (inv; 2; 0.0); *)
l2 = 0.0 fby (0.0 fby inv);
o =
if Math.abs(l2) <. 0.1 or Math.abs(inv) <. 0.1
then 0.0
else 0.0 -> (inv -. 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 at r;
priority : int)
returns (newmissiontrack : TypeTracks.tmissiontrack at r)
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
fun position_equal(p1, p2 : TypeBase.tposition) returns (res:bool)
let
res = (p1.x = p2.x) & (p1.y = p2.y)
tel
fun speed_equal(s1, s2 : TypeBase.tspeed) returns (res:bool)
let
res = (s1.sx = s2.sx) & (s1.sy = s2.sy)
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 position_equal(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 (position_equal(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 (position_equal(rdrtrack.r_pos, CstBaseInit.kInitPosition) &
speed_equal(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

View file

@ -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

View file

@ -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

View file

@ -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
}

View file

@ -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