Scade example with linear annotations
Cédric Pasteur
13 years ago
parent
4d912e9349
commit
4648f7fbab
@ -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 ..
|
||||
@ -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
|
||||
|
||||
@ -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 }
|
||||
@ -0,0 +1,4 @@
|
||||
|
||||
const nm : float = 1852.0
|
||||
const t : float = 0.01
|
||||
const pi : float = 3.141592
|
||||
@ -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 }
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -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)
|
||||
@ -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)
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -0,0 +1,3 @@
|
||||
type trdrmode = TRdrMode_WIDE | TRdrMode_NARROW
|
||||
|
||||
type tsensorstate = TState_OFF | TState_ON | TState_FAIL
|
||||
@ -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;
|
||||
}
|
||||
@ -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)
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
Loading…
Reference in New Issue