1048 lines
25 KiB
C++
1048 lines
25 KiB
C++
/*
|
|
* DspRIS.cpp
|
|
*
|
|
* WARNING: ALL section (code, data, rodata, bss, ...) shall be allocated onto a dedicated memory area, reserved at link time
|
|
* It shall work as a "static DLL": the Rsi Code shall not alter in any way the operative code, even in term of linker time memory allocation.
|
|
* In other word, the operative CSC shall link also without DspRis and its binary image shall be equal on an executable linker with the RisDsp
|
|
*
|
|
* The target memory shall have 3 partition:
|
|
* -the operative, spread around without constrains except it cannot use the RIS reserved partitions
|
|
* -Fixed RIS I/F partition: I/F between operative CSC and RIS CSC,
|
|
* -Fixed RIS CSC partition: all RIS section
|
|
*
|
|
* The operative partition of an executable linked without RIS shall be equal to an executable linked with the RIS
|
|
*
|
|
*/
|
|
|
|
|
|
#include "dsp_ris_internals_.h"
|
|
|
|
#include "sfp_dsp_ris.h"
|
|
|
|
#include "gc_math.h"
|
|
|
|
#include "mrk_plus.h"
|
|
#include "mrk_net.h"
|
|
|
|
#include "rtlog.h"
|
|
|
|
#include "mex_hobbit_register.h"
|
|
|
|
#include "mex.h"
|
|
|
|
#include "rtg_high_resolution_timer.h"
|
|
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#ifdef __MINGW32__
|
|
#pragma GCC optimize("-O0")
|
|
#endif
|
|
|
|
#include <new>
|
|
|
|
#define LOGID "RIS"
|
|
|
|
#define MAX_PACKET_SIZE (10*1024) /*provision for jumbo buffer*/
|
|
|
|
struct ris_dsp_message_header_t
|
|
{
|
|
uint32_t begin_marker[2]; //=="RIS0>>>>"
|
|
uint16_t version;
|
|
uint16_t type;
|
|
uint32_t cmd;
|
|
char data[1024];
|
|
|
|
};
|
|
|
|
#define RIS_MARKER0 0x30534952 /*"RIS0"*/
|
|
#define RIS_MARKER1 0x3E3E3E3E /*">>>>"*/
|
|
|
|
enum dsp_ris_features_flags_t
|
|
{
|
|
dsp_ris_feat_sim_none,
|
|
dsp_ris_feat_sim_lcu,
|
|
dsp_ris_feat_sim_pps,
|
|
dsp_ris_feat_sim_aesa,
|
|
|
|
dsp_ris_feat_sim_simple_map
|
|
};
|
|
|
|
struct dsp_ris_tgt_cmd_t
|
|
{
|
|
int id;
|
|
uint32_t data_mask;
|
|
|
|
float ground_range;
|
|
float baro_altitude;
|
|
float nav_azimuth;
|
|
float ground_speed;
|
|
float vertical_speed;
|
|
float turn_rate;
|
|
float turn_time;
|
|
|
|
float spare[4];
|
|
};
|
|
|
|
struct dsp_ris_cmd_packet_t
|
|
{
|
|
uint32_t features_enable;
|
|
|
|
dsp_ris_tgt_cmd_t tgt_cmd[16];
|
|
};
|
|
|
|
static_assert((sizeof(SFP_RIS::status_payload_t)<SFP::TX_MTU), "SFP_RIS::status_payload_t to big");
|
|
static_assert((sizeof(SFP_RIS::script_payload_t)<SFP::TX_MTU), "SFP_RIS::scirpt_payload_t to big");
|
|
|
|
#include "bsk_tiny_json.h"
|
|
|
|
static void json_pinfo(const char* fmt, ...)
|
|
{
|
|
va_list args;
|
|
va_start(args, fmt);
|
|
rtlog_vprintf("JSON", rtlog_info, fmt, args);
|
|
va_end(args);
|
|
}
|
|
|
|
class RisJsonParse
|
|
{
|
|
public:
|
|
json_t mem[2048];
|
|
const json_t* json;
|
|
|
|
class FloatParam
|
|
{
|
|
public:
|
|
float* p;
|
|
float v;
|
|
|
|
FloatParam(): p(0), v(0) {}
|
|
|
|
bool get(const json_t* obj, const char* name)
|
|
{
|
|
return (p=getFloat(obj, name, &v))!=0;
|
|
}
|
|
};
|
|
class IntParam
|
|
{
|
|
public:
|
|
int* p;
|
|
int v;
|
|
|
|
IntParam(): p(0), v(0) {}
|
|
|
|
bool get(const json_t* obj, const char* name)
|
|
{
|
|
return (p=getInt(obj, name, &v))!=0;
|
|
}
|
|
};
|
|
|
|
class UIntParam
|
|
{
|
|
public:
|
|
unsigned int* p;
|
|
unsigned int v;
|
|
|
|
UIntParam(): p(0), v(0) {}
|
|
|
|
bool get(const json_t* obj, const char* name)
|
|
{
|
|
return (p=getInt(obj, name, &v))!=0;
|
|
}
|
|
};
|
|
|
|
static float* getFloat(const json_t* obj, const char* name, float* value)
|
|
{
|
|
const json_t* p=json_getProperty(obj, name);
|
|
if (!p)
|
|
return 0;
|
|
*value=json_getReal(p);
|
|
return value;
|
|
}
|
|
static int* getInt(const json_t* obj, const char* name, int* value)
|
|
{
|
|
const json_t* p=json_getProperty(obj, name);
|
|
if (!p)
|
|
{
|
|
return 0;
|
|
}
|
|
*value=json_getInteger(p);
|
|
return value;
|
|
}
|
|
static unsigned int* getInt(const json_t* obj, const char* name, unsigned int* value)
|
|
{
|
|
const json_t* p=json_getProperty(obj, name);
|
|
if (!p)
|
|
{
|
|
return 0;
|
|
}
|
|
*value=json_getInteger(p);
|
|
return value;
|
|
}
|
|
|
|
UIntParam pActive;
|
|
UIntParam pTraceable;
|
|
FloatParam pRange;
|
|
FloatParam pAz;
|
|
IntParam pId;
|
|
FloatParam pSpeed;
|
|
FloatParam pHeading;
|
|
FloatParam pAltitude;
|
|
FloatParam pTurnRate;
|
|
FloatParam pTurnDuration;
|
|
|
|
const char* parse(char* str) //Warning: str is modified by the parser
|
|
{
|
|
json = json_create( str, mem, sizeof mem / sizeof *mem );
|
|
|
|
if (json==0)
|
|
{
|
|
return "JSON: Error parsing.";
|
|
}
|
|
|
|
json_t const* cmdObj = json_getProperty( json, "CMD");
|
|
if (cmdObj)
|
|
{
|
|
const char* v=json_getValue(cmdObj);
|
|
if (strcmp(v, "reset")==0)
|
|
{
|
|
rtlog_printf("RIS", rtlog_report|rtlog_breaking_news, "RIS: target reset!");
|
|
DspRisTargetSet(-1);
|
|
}
|
|
}
|
|
|
|
json_t const* tgtArrayObj = json_getProperty( json, "TGTS" );
|
|
if ( tgtArrayObj && (JSON_ARRAY == json_getType( tgtArrayObj )) )
|
|
{
|
|
json_pinfo("tgt array:");
|
|
for(json_t const* t= json_getChild( tgtArrayObj ); t != 0; t = json_getSibling( t ) )
|
|
{
|
|
pId.get(t, "ID");
|
|
pActive.get(t, "a");
|
|
pTraceable.get(t, "t");
|
|
pRange.get(t, "R");
|
|
pAz.get(t, "NAZ");
|
|
pSpeed.get(t, "GV");
|
|
pHeading.get(t, "H");
|
|
pAltitude.get(t, "A");
|
|
pTurnRate.get(t, "TR");
|
|
pTurnDuration.get(t, "TD");
|
|
|
|
json_pinfo("TGT %d: %d,%f", pId.v, pActive.v, pRange.v);
|
|
|
|
//void DspRisTargetSet(int id, unsigned int* flags, float* range, float* az, float* speed, float* heading, float* altitude);
|
|
if (pId.p)
|
|
{
|
|
pAz.v=gc_math::deg_to_rad(pAz.v);
|
|
pHeading.v=gc_math::deg_to_rad(pHeading.v);
|
|
pTurnRate.v=gc_math::deg_to_rad(pTurnRate.v);
|
|
|
|
DspRisTargetSet(pId.v, pActive.p, pRange.p, pAz.p, pSpeed.p, pHeading.p, pAltitude.p, pTurnRate.p, pTurnDuration.p);
|
|
}
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
};
|
|
|
|
static RisJsonParse risJ;
|
|
|
|
static const char j_test[]=
|
|
"{"
|
|
"\"CMD\": \"reset\","
|
|
"\"TGTS\": ["
|
|
"{"
|
|
"\"UID\":100,"
|
|
"\"ID\":1,"
|
|
"\"R\":20000,"
|
|
"\"NAZ\":10.0,"
|
|
"\"GV\":100,"
|
|
"\"H\":0,"
|
|
"\"A\":1820,"
|
|
"\"VV\": 10.0," //Vertical Velocity
|
|
"\"TR\": 2," //Turn Rate deg
|
|
"\"TD\": 10," //turn duration
|
|
"\"AMP:\": 0," //amplitude. ==0 use default. <:
|
|
"\"RCS:\": 0," //radar cross section
|
|
"\"a\":1"
|
|
"},"
|
|
"{"
|
|
"\"UID\":100,"
|
|
"\"ID\":2,"
|
|
"\"R\":15000,"
|
|
"\"NAZ\":-10.0,"
|
|
"\"GV\":100,"
|
|
"\"H\":10,"
|
|
"\"A\":1820,"
|
|
"\"a\":1"
|
|
"}"
|
|
"]"
|
|
"}";
|
|
|
|
class DspRisAgentBase
|
|
{
|
|
public:
|
|
int eth_if;
|
|
|
|
int active;
|
|
|
|
unsigned int sent;
|
|
unsigned int received;
|
|
unsigned int errors;
|
|
|
|
mrk_net_ip_raw_address_t server_ip;
|
|
mrk_net_ip_port_t server_port;
|
|
mrk_net_udp_socket_t ris_socket;
|
|
|
|
mrk_net_ip_raw_address_t client_ip;
|
|
mrk_net_ip_port_t client_port;
|
|
|
|
static volatile SFP_RIS::cc_sync_payload_t cc_sync;
|
|
|
|
void createPort(unsigned int base_port, int eth_if_=0)
|
|
{
|
|
eth_if=eth_if_;
|
|
server_port=base_port;
|
|
|
|
mrk_net_udp_socket(&ris_socket, "Ris");
|
|
#ifdef __MINGW32__
|
|
mrk_net_udp_bind(&ris_socket, mrk_net_str2ip("127.0.0.1"), server_port);
|
|
#else
|
|
mrk_net_udp_bind_if(&ris_socket, eth_if, server_port);
|
|
#endif
|
|
|
|
client_ip=mrk_net_myip_address_broadcast(eth_if);
|
|
client_port=server_port+1;
|
|
server_ip=mrk_net_myip_address(eth_if);
|
|
}
|
|
|
|
static inline uint32_t get_tt()
|
|
{
|
|
unsigned long t=rtg_hrt_to_microseconds(rtg_hrt_get())/64;
|
|
return t;
|
|
}
|
|
};
|
|
|
|
volatile SFP_RIS::cc_sync_payload_t DspRisAgentBase::cc_sync;
|
|
|
|
class DspRisAgentRx
|
|
{
|
|
public:
|
|
explicit DspRisAgentRx()
|
|
{
|
|
}
|
|
|
|
DspRisAgentBase cx;
|
|
|
|
mrk_thread_with_stack<8*1024> thread;
|
|
|
|
mrk_mutex_t mutex;
|
|
|
|
int connected;
|
|
unsigned int sync_replayed;
|
|
|
|
|
|
volatile int test;
|
|
|
|
unsigned int rcv_datagram;
|
|
unsigned int rcv_script;
|
|
//sfp_frag_header_payload_init(stsmsg.header, 'R', sizeof fh, uint8_t record_type, unsigned int payload_leader_size, unsigned int payload_body_size);
|
|
SFP_RIS::script_message_t cmdmsg;
|
|
|
|
unsigned int raw_packet[MAX_PACKET_SIZE/(sizeof(unsigned int))];
|
|
|
|
void go()
|
|
{
|
|
thread.create("RIS-RX", thread_exec, reinterpret_cast<unsigned long long>(this), mrk_std_priority_below_normal, 0);
|
|
}
|
|
|
|
static void thread_exec(void)
|
|
{
|
|
DspRisAgentRx* self=reinterpret_cast<DspRisAgentRx*>(mrk_thread_get_arg());
|
|
self->exec();
|
|
}
|
|
|
|
void exec()
|
|
{
|
|
struct synch_reply_t
|
|
{
|
|
SFP::frag_header_t header;
|
|
struct payload_t
|
|
{
|
|
SFP::data_tag_t sync_tag; //TAG="SY", 2w
|
|
SFP_RIS::cc_sync_payload_t sync;
|
|
} payload;
|
|
};
|
|
static synch_reply_t sync_reply;
|
|
|
|
auto sync_tag=sfp_data_tag('S', 'Y');
|
|
{
|
|
sfp_frag_header_initialize(sync_reply.header, SFP::frag_sync_marker_sender, SFP::src_rdr, SFP::flow_id_ris_cmd);
|
|
|
|
sync_reply.header.frag_tag=SFP::frag_single;
|
|
sync_reply.header.payload_total_size=sizeof sync_reply.payload;
|
|
sync_reply.header.payload_size=sizeof sync_reply.payload;
|
|
sync_reply.header.frag_total=1;
|
|
sync_reply.header.record_type=SFP::flow_id_ris_cmd;
|
|
|
|
sfp_data_tag_initialize(sync_reply.payload.sync_tag, sync_tag, 0, sizeof sync_reply.payload.sync);
|
|
}
|
|
|
|
mex_hobbit_register_instance("risrx", *this);
|
|
|
|
test=0;
|
|
cx.eth_if=0;
|
|
cx.createPort(60013);
|
|
|
|
unsigned int timeout=500; //mrk_wait_forever;
|
|
|
|
cx.active=1;
|
|
|
|
memset(&cmdmsg, 0, sizeof cmdmsg);
|
|
|
|
auto script_tag=sfp_data_tag('C', 'S');
|
|
|
|
rtlog_printf("RIS", rtlog_report|rtlog_breaking_news, "RIS-TX COM Active I/F %d, port %u", cx.eth_if, cx.server_port);
|
|
|
|
for(;;)
|
|
{
|
|
unsigned int received_len=0;
|
|
|
|
cmdmsg.payload.sync_tag.tag=0;
|
|
cmdmsg.payload.ctrl_tag.tag=0;
|
|
cmdmsg.payload.script_tag.tag=0;
|
|
|
|
mrk_net_result_t r=mrk_net_udp_receive(&cx.ris_socket, raw_packet, sizeof raw_packet, &cx.client_ip, &cx.client_port, &received_len, timeout);
|
|
|
|
memcpy(&cmdmsg, raw_packet, received_len<sizeof cmdmsg ? received_len : sizeof cmdmsg);
|
|
|
|
if (r==mrk_net_timeout)
|
|
{
|
|
if (test>0)
|
|
{
|
|
mrk_thread_sleep_ms(500);
|
|
cmdmsg.payload.script_tag.type_validity=1;
|
|
cmdmsg.payload.script_tag.tag=script_tag;
|
|
r=mrk_net_ok;
|
|
cmdmsg.payload.script_tag.size=sizeof j_test;
|
|
memcpy(cmdmsg.payload.script, j_test, sizeof j_test);
|
|
received_len=sizeof cmdmsg;
|
|
|
|
cmdmsg.payload.sync_tag.tag=sync_tag;
|
|
cmdmsg.payload.sync_tag.type_validity=1;
|
|
cmdmsg.payload.sync.cc_cookie=cx.cc_sync.cc_cookie+10;
|
|
cmdmsg.payload.sync.flags=1;
|
|
--test;
|
|
}
|
|
}
|
|
if (r==mrk_net_ok)
|
|
{
|
|
++cx.received;
|
|
|
|
connected=1;
|
|
|
|
bool valid=received_len>(sizeof cmdmsg.header);
|
|
|
|
if ((cmdmsg.payload.sync_tag.tag==sync_tag) && (cmdmsg.payload.sync_tag.type_validity))
|
|
{
|
|
//cc_sync=cmdmsg.payload.sync;
|
|
cx.cc_sync.cc_cookie=cmdmsg.payload.sync.cc_cookie;
|
|
cx.cc_sync.tx_period_ms=cmdmsg.payload.sync.tx_period_ms;
|
|
|
|
if (cmdmsg.payload.sync.flags & 1)
|
|
{
|
|
sync_reply.payload.sync=cmdmsg.payload.sync;
|
|
sync_reply.payload.sync.ris_timetag=cx.get_tt();
|
|
++sync_reply.header.transaction_id;
|
|
++sync_reply.header.record_counter;
|
|
|
|
++sync_replayed;
|
|
|
|
mrk_net_udp_send(&cx.ris_socket, &sync_reply, sizeof sync_reply, cx.client_ip, cx.client_port, 0);
|
|
}
|
|
}
|
|
|
|
valid&=
|
|
(cmdmsg.payload.script_tag.type_validity)
|
|
&& (cmdmsg.payload.script_tag.tag==script_tag);
|
|
if (valid)
|
|
{
|
|
++rcv_script;
|
|
if (cmdmsg.payload.script_tag.size<((sizeof cmdmsg.payload.script)-1))
|
|
{
|
|
cmdmsg.payload.script[cmdmsg.payload.script_tag.size]=0;
|
|
if (cmdmsg.payload.script[0]=='{')
|
|
{
|
|
const char* err=risJ.parse((char*)cmdmsg.payload.script);
|
|
if (err)
|
|
{
|
|
++cx.errors;
|
|
rtlog_printf("RIS", rtlog_info, "TX JSON error");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
mex_script_inject(reinterpret_cast<char*>(cmdmsg.payload.script), 0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
class DspRisAgentTx //: public DspRisAgentBase
|
|
{
|
|
public:
|
|
explicit DspRisAgentTx()
|
|
{
|
|
}
|
|
|
|
DspRisAgentBase cx;
|
|
|
|
mrk_thread_with_stack<4*1024> thread;
|
|
|
|
volatile unsigned int period;
|
|
|
|
SFP_RIS::status_message_t stsmsg;
|
|
|
|
void go()
|
|
{
|
|
thread.create("RIS-TX", thread_exec, reinterpret_cast<unsigned long long>(this), mrk_std_priority_below_normal, 0);
|
|
}
|
|
|
|
static void thread_exec(void)
|
|
{
|
|
DspRisAgentTx* self=reinterpret_cast<DspRisAgentTx*>(mrk_thread_get_arg());
|
|
self->exec();
|
|
}
|
|
|
|
void exec()
|
|
{
|
|
mex_hobbit_register_instance("ristx", *this);
|
|
|
|
period=20;
|
|
|
|
cx.createPort(60011, 0);
|
|
|
|
unsigned int timeout=0; //5000; //mrk_wait_forever;
|
|
|
|
cx.active=1;
|
|
|
|
memset(&stsmsg, 0, sizeof stsmsg);
|
|
|
|
sfp_frag_header_initialize(stsmsg.header, SFP::frag_sync_marker_sender, SFP::src_rdr, SFP::flow_id_ris_sts);
|
|
|
|
//sfp_frag_header_payload_init(stsmsg.header, 'R', sizeof fh, uint8_t record_type, unsigned int payload_leader_size, unsigned int payload_body_size);
|
|
{
|
|
stsmsg.header.frag_tag=SFP::frag_single;
|
|
stsmsg.header.payload_total_size=sizeof stsmsg.payload;
|
|
stsmsg.header.payload_size=sizeof stsmsg.payload;
|
|
stsmsg.header.frag_total=1;
|
|
stsmsg.header.record_type=SFP::flow_id_ris_sts;
|
|
|
|
}
|
|
sfp_data_tag_initialize(stsmsg.payload.scenario_tag, sfp_data_tag('R', 'S'), 0, sizeof stsmsg.payload.scenario);
|
|
sfp_data_tag_initialize(stsmsg.payload.target_tag, sfp_data_tag('R', 'T'), 0, sizeof stsmsg.payload.tgt);
|
|
|
|
rtlog_printf("RIS", rtlog_report|rtlog_breaking_news, "RIS-RX COM Active I/F %d, port %u", cx.eth_if, cx.server_port);
|
|
|
|
for(;;)
|
|
{
|
|
unsigned int delay=20;
|
|
//client request?
|
|
if ((cx.cc_sync.tx_period_ms>=10) && (cx.cc_sync.tx_period_ms<10000))
|
|
delay=cx.cc_sync.tx_period_ms;
|
|
if (period>20)//monitor override
|
|
delay=period;
|
|
|
|
mrk_thread_sleep_ms(delay);
|
|
|
|
if (cx.cc_sync.tx_period_ms<0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
++stsmsg.header.transaction_id;
|
|
++stsmsg.header.record_counter;
|
|
|
|
stsmsg.payload.scenario.cc_cookie=cx.cc_sync.cc_cookie;
|
|
|
|
stsmsg.payload.scenario.timetag=cx.get_tt();
|
|
|
|
mrk_net_udp_send(&cx.ris_socket, &stsmsg, sizeof stsmsg, cx.client_ip, cx.client_port, timeout);
|
|
++cx.sent;
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
class DspRisAgent
|
|
{
|
|
public:
|
|
DspRisAgentTx txAgent;
|
|
DspRisAgentRx rxAgent;
|
|
|
|
void go()
|
|
{
|
|
txAgent.go();
|
|
rxAgent.go();
|
|
}
|
|
};
|
|
|
|
#ifdef _TMS320C6X
|
|
#define ATTR_NO_INIT __attribute__((section(".far:ris_noinit_")))
|
|
#else
|
|
#define ATTR_NO_INIT
|
|
#endif
|
|
|
|
struct dsp_ris_static_noinit_data_t
|
|
{
|
|
uint64_t align;
|
|
unsigned int active[2];
|
|
|
|
mrk_mutex_t tgtMutex;
|
|
|
|
char risAgentImage[sizeof(DspRisAgent)];
|
|
DspRisAgent* risAgent;
|
|
volatile DspRisAgent* risAgentVolatile;
|
|
};
|
|
|
|
static dsp_ris_static_noinit_data_t dsp_ris_static_noinit_data ATTR_NO_INIT;
|
|
|
|
bool DspRigTgtMutexAcquire(bool please_wait)
|
|
{
|
|
mrk_tick_t tm=please_wait ? mrk_wait_forever : 0;
|
|
mrk_sts_t r=mrk_mutex_acquire(&dsp_ris_static_noinit_data.tgtMutex, tm);
|
|
return r==mrk_ok;
|
|
}
|
|
|
|
void DspRigTgtMutexRelease()
|
|
{
|
|
mrk_mutex_release(&dsp_ris_static_noinit_data.tgtMutex);
|
|
}
|
|
|
|
static void DspRisMexInstall();
|
|
|
|
#include "mex.h"
|
|
|
|
void DspRisInstall()
|
|
{
|
|
bool activated=(dsp_ris_static_noinit_data.active[0]==0x12345678) && (dsp_ris_static_noinit_data.active[1]==0x87654321);
|
|
if (activated)
|
|
return;
|
|
|
|
rtlog_const_message(LOGID, rtlog_breaking_news|rtlog_report, "RIS: Activating...");
|
|
|
|
memset(&dsp_ris_tgt_db, 0, sizeof dsp_ris_tgt_db);
|
|
|
|
#ifdef __MINGW32__
|
|
dsp_ris_tgt_db.agr.enable=1;
|
|
#endif
|
|
|
|
for(int i=0; i<DSP_RIS_MAX_TGT; ++i)
|
|
{
|
|
float factor=i-float(DSP_RIS_MAX_TGT/2);
|
|
float abs_factor=factor<0 ? -factor: factor;
|
|
|
|
dsp_ris_tgt_db.t[i].unique_id=i;
|
|
|
|
dsp_ris_tgt_db.t[i].ground_range=18*1852-abs_factor*(1852);
|
|
dsp_ris_tgt_db.t[i].speed=100;
|
|
dsp_ris_tgt_db.t[i].altitude=1828;
|
|
|
|
dsp_ris_tgt_db.t[i].az=GC_DEG_TO_RAD(factor*(100.0f/DSP_RIS_MAX_TGT));
|
|
|
|
dsp_ris_tgt_db.t[i].rng_start_meters=dsp_ris_tgt_db.t[i].ground_range;
|
|
dsp_ris_tgt_db.t[i].speed_start=dsp_ris_tgt_db.t[i].speed;
|
|
dsp_ris_tgt_db.t[i].altitude_start=dsp_ris_tgt_db.t[i].altitude;
|
|
dsp_ris_tgt_db.t[i].az_start=dsp_ris_tgt_db.t[i].az;
|
|
|
|
dsp_ris_tgt_db.t[i].enabled=-2;
|
|
}
|
|
|
|
dsp_ris_tgt_db.t[0].ground_range=10000;
|
|
dsp_ris_tgt_db.t[0].speed=100;
|
|
dsp_ris_tgt_db.t[0].altitude=1828;
|
|
dsp_ris_tgt_db.t[0].az=0;
|
|
dsp_ris_tgt_db.t[0].enabled=1;
|
|
|
|
dsp_ris_tgt_db.ignore_silence=1;
|
|
|
|
#if 0
|
|
dsp_ris_tgt_db.t[2].enabled=0;
|
|
dsp_ris_tgt_db.t[2].ground_range=10000;
|
|
dsp_ris_tgt_db.t[2].az=GC_DEG_TO_RAD(-20);
|
|
dsp_ris_tgt_db.t[2].speed=100;
|
|
dsp_ris_tgt_db.t[2].altitude=1000;
|
|
|
|
dsp_ris_tgt_db.t[3].enabled=0;
|
|
dsp_ris_tgt_db.t[3].ground_range=10000;
|
|
dsp_ris_tgt_db.t[3].az=GC_DEG_TO_RAD(20);
|
|
dsp_ris_tgt_db.t[3].speed=100;
|
|
dsp_ris_tgt_db.t[3].altitude=1000;
|
|
|
|
dsp_ris_tgt_db.t[1]=dsp_ris_tgt_db.t[0];
|
|
dsp_ris_tgt_db.t[1].enabled=0;
|
|
dsp_ris_tgt_db.t[1].ground_range=13000;
|
|
#endif
|
|
|
|
memset(&dsp_ris_static_noinit_data, 0, sizeof dsp_ris_static_noinit_data);
|
|
|
|
mrk_mutex_register(&dsp_ris_static_noinit_data.tgtMutex, "RISTGT");
|
|
|
|
dsp_ris_static_noinit_data.risAgent=new(&dsp_ris_static_noinit_data.risAgentImage) DspRisAgent;
|
|
|
|
|
|
dsp_ris_static_noinit_data.risAgentVolatile=dsp_ris_static_noinit_data.risAgent;
|
|
dsp_ris_static_noinit_data.active[0]=0x12345678;
|
|
dsp_ris_static_noinit_data.active[1]=0x87654321;
|
|
|
|
DspRisMexInstall();
|
|
|
|
dsp_ris_static_noinit_data.risAgent->go();
|
|
|
|
mex_set_prompt("DSP-RIS");
|
|
}
|
|
|
|
#include "idd_nav_types.h"
|
|
|
|
static void DspRisSfpUpdateScenario(unsigned int timetag, const iddtypes::inu_data_t& inu)
|
|
{
|
|
if (!dsp_ris_static_noinit_data.risAgent)
|
|
return;
|
|
DspRisAgent& ra=*dsp_ris_static_noinit_data.risAgent;
|
|
|
|
//ra.txAgent.stsmsg.payload.scenario.timetag=timetag;
|
|
ra.txAgent.stsmsg.payload.scenario.nav_timetag=timetag;
|
|
ra.txAgent.stsmsg.payload.scenario.nav_invalidity=0;
|
|
ra.txAgent.stsmsg.payload.scenario.platform_azimuth=inu.own_ship_attitude.vector.platform_azimuth;
|
|
|
|
ra.txAgent.stsmsg.payload.scenario.vx=inu.own_ship_velocity.vector.x;
|
|
ra.txAgent.stsmsg.payload.scenario.vy=inu.own_ship_velocity.vector.y;
|
|
ra.txAgent.stsmsg.payload.scenario.vz=inu.own_ship_velocity.vector.z;
|
|
ra.txAgent.stsmsg.payload.scenario.baro_altitude=inu.inertial_altitude.data;
|
|
|
|
ra.txAgent.stsmsg.payload.scenario.latitude=inu.geo_pos.latitude;
|
|
ra.txAgent.stsmsg.payload.scenario.longitude=inu.geo_pos.longitude;
|
|
|
|
ra.txAgent.stsmsg.payload.scenario.true_heading=inu.own_ship_attitude.true_heading;
|
|
|
|
//Targets
|
|
for(int i=0; i<DSP_RIS_MAX_TGT; ++i)
|
|
{
|
|
ra.txAgent.stsmsg.payload.tgt.tgt[i].flags=dsp_ris_tgt_db.t[i].enabled;
|
|
ra.txAgent.stsmsg.payload.tgt.tgt[i].heading=dsp_ris_tgt_db.t[i].heading;
|
|
ra.txAgent.stsmsg.payload.tgt.tgt[i].x=dsp_ris_tgt_db.t[i].x;
|
|
ra.txAgent.stsmsg.payload.tgt.tgt[i].y=dsp_ris_tgt_db.t[i].y;
|
|
ra.txAgent.stsmsg.payload.tgt.tgt[i].z=dsp_ris_tgt_db.t[i].altitude;
|
|
}
|
|
}
|
|
|
|
void DspRisSfpUpdateHeader(const DspLrpHeaderOutput::Header* hdrptr)
|
|
{
|
|
if (!dsp_ris_static_noinit_data.risAgent)
|
|
return;
|
|
DspRisAgent& ra=*dsp_ris_static_noinit_data.risAgent;
|
|
|
|
ra.txAgent.stsmsg.payload.scenario.ant_nav_az=hdrptr->ge.general_settings.antenna.position.navaz_rad;
|
|
ra.txAgent.stsmsg.payload.scenario.ant_nav_el=hdrptr->ge.general_settings.antenna.position.navel_rad;
|
|
|
|
ra.txAgent.stsmsg.payload.scenario.mode=hdrptr->ge.mode.master_mode;
|
|
|
|
}
|
|
|
|
//MEx monitor
|
|
#include "mex_rtg_io.h"
|
|
|
|
#ifdef __GNUC__
|
|
#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
|
|
#endif
|
|
|
|
|
|
static void nl()
|
|
{
|
|
mex_putchar('\n');
|
|
}
|
|
|
|
static int eth_n;
|
|
static int eth_stat_refresh;
|
|
static int eth_msg_stat;
|
|
static int eth_msg_period;
|
|
static const mex_cmd_param_descriptor_t eth_show_table[] =
|
|
{
|
|
{ "ethif", "Ethernet I/F", 0, ð_n, &mex_mt_int},
|
|
{ "r", "refresh", mex_cmd_qualifier, ð_stat_refresh },
|
|
{ "m", "message stat", mex_cmd_qualifier, ð_msg_stat },
|
|
{ "p", "show exact period", mex_cmd_qualifier, ð_msg_period },
|
|
#if 0
|
|
|
|
{ "r", "relative timetags", mex_cmd_qualifier, &drs.param_relative_timetag },
|
|
{ "a", "abs time", mex_cmd_qualifier, &drs.param_abs_time },
|
|
{ "rev", "reverse (oldest first)", mex_cmd_qualifier, &drs.reverse },
|
|
|
|
{ "t", "hide timetag (default)", mex_cmd_qualifier, &drs.tt_hide },
|
|
{ "b", "show local batch", mex_cmd_qualifier, &drs.lb },
|
|
{ "g", "show global batch", mex_cmd_qualifier, &drs.gb },
|
|
#endif
|
|
{ 0 }
|
|
};
|
|
|
|
|
|
#if 0
|
|
static unsigned period_round(unsigned int period, char& rounded)
|
|
{
|
|
rounded=' ';
|
|
|
|
if (period<1)
|
|
return period;
|
|
|
|
if (period>9999)
|
|
{
|
|
rounded='>';
|
|
return 9999;
|
|
}
|
|
if (eth_msg_period)
|
|
return period;
|
|
|
|
unsigned int p10=period/10;
|
|
p10=p10*10;
|
|
int p_remainder=period-(p10);
|
|
if (p_remainder>=8)
|
|
{
|
|
rounded='~';
|
|
period=10+p10;
|
|
}
|
|
else if (p_remainder==1)
|
|
{
|
|
rounded='~';
|
|
period=p10;
|
|
}
|
|
return period;
|
|
}
|
|
#endif
|
|
|
|
|
|
static void show_agent(volatile const DspRisAgentBase& a)
|
|
{
|
|
unsigned int sip=a.server_ip;
|
|
unsigned int cip=a.client_ip;
|
|
|
|
mex_printf("%u:%u.%u.%u.%u::%u->%u.%u.%u.%u::%u rx=%u tx=%u e=%u\n",
|
|
a.eth_if,
|
|
sip & 0x0FF,
|
|
(sip>>8) & 0x0FF,
|
|
(sip>>16) & 0x0FF,
|
|
(sip>>24) & 0x0FF,
|
|
a.server_port,
|
|
cip & 0x0FF,
|
|
(cip>>8) & 0x0FF,
|
|
(cip>>16) & 0x0FF,
|
|
(cip>>24) & 0x0FF,
|
|
a.client_port,
|
|
a.received, a.sent, a.errors);
|
|
}
|
|
|
|
static mex_res_t Cmd_ShowEth()
|
|
{
|
|
//int eth_id=1;
|
|
#if 0
|
|
int eth_mask=1<<eth_id;
|
|
|
|
if (mex_param_is_present(ð_n))
|
|
{
|
|
if (eth_n==0)
|
|
eth_mask=1;
|
|
else if (eth_n==1)
|
|
eth_mask=2;
|
|
else if (eth_n==2)
|
|
{
|
|
eth_mask=3;
|
|
}
|
|
}
|
|
#endif
|
|
//int flags=0;
|
|
//if (mex_param_is_present(ð_stat_refresh))
|
|
// flags=1;
|
|
|
|
mex_page_loop_init(1);
|
|
//one_per_line=true; //(mex_page_loop_end(0, 0)!= mex_page_event_quit) ? false : true;
|
|
|
|
bool show_msg=mex_param_is_present(ð_msg_stat);
|
|
eth_msg_period=mex_param_is_present(ð_msg_period) ? 1 : 0;
|
|
|
|
do
|
|
{
|
|
mex_page_loop_top();
|
|
|
|
volatile DspRisAgent* ra=dsp_ris_static_noinit_data.risAgentVolatile;
|
|
if (!ra)
|
|
{
|
|
mex_printf("RIS: NOT INSTALLED\n");
|
|
continue;
|
|
}
|
|
|
|
mex_printf("RIS: %s\n\n", ra->rxAgent.connected ? "CONNECTED " : "NOT CONNECTED");
|
|
|
|
//mrk_net_stat_t eth_stat=*mrk_net_stat(ra->eth_if, 0);
|
|
|
|
mex_printf("TX: ");
|
|
show_agent(ra->txAgent.cx);
|
|
|
|
mex_printf("RX: ");
|
|
show_agent(ra->rxAgent.cx);
|
|
|
|
if (show_msg)
|
|
continue;
|
|
|
|
} while (mex_page_loop_end(0, 0) != mex_page_event_quit);
|
|
nl();
|
|
|
|
return mex_ok;
|
|
}
|
|
|
|
static int sleep_time;
|
|
|
|
static mex_res_t Cmd_Sleep()
|
|
{
|
|
mrk_thread_sleep_ms(sleep_time);
|
|
return mex_ok;
|
|
}
|
|
|
|
static const mex_cmd_param_descriptor_t sleep_table[] =
|
|
{
|
|
{ "time", "sleep time(ms)", mex_cmd_required, &sleep_time, &mex_mt_uint },
|
|
{ 0 }
|
|
};
|
|
|
|
static const mex_cmd_descriptor_t cmd_sleep =
|
|
{ "sleep", "script sleep", (mex_cmd_handler_t) Cmd_Sleep, sleep_table};
|
|
|
|
static const mex_cmd_descriptor_t cmd_eth =
|
|
{ "rissts", "Show RIS status", (mex_cmd_handler_t) Cmd_ShowEth, eth_show_table};
|
|
|
|
|
|
extern const mex_cmd_descriptor_t cmd_tgtinit;
|
|
extern const mex_cmd_descriptor_t cmd_tgtset;
|
|
extern const mex_cmd_descriptor_t cmd_tgtturn;
|
|
|
|
#if 0
|
|
extern const mex_cmd_descriptor_t cmd_msg;
|
|
extern const mex_cmd_descriptor_t cmd_msg_res;
|
|
|
|
extern const mex_cmd_descriptor_t cmd_scenario;
|
|
|
|
extern const mex_cmd_descriptor_t cmd_pause;
|
|
extern const mex_cmd_descriptor_t cmd_continue;
|
|
extern const mex_cmd_descriptor_t cmd_tgtreset;
|
|
extern const mex_cmd_descriptor_t cmd_aclatch;
|
|
extern const mex_cmd_descriptor_t cmd_acunlatch;
|
|
extern const mex_cmd_descriptor_t cmd_tgtrng;
|
|
extern const mex_cmd_descriptor_t cmd_tgtaz;
|
|
extern const mex_cmd_descriptor_t cmd_tgthead;
|
|
extern const mex_cmd_descriptor_t cmd_tgtspeed;
|
|
extern const mex_cmd_descriptor_t cmd_tgtalt;
|
|
extern const mex_cmd_descriptor_t cmd_tgtmap;
|
|
|
|
extern const mex_cmd_descriptor_t cmd_bite_reset;
|
|
extern const mex_cmd_descriptor_t cmd_bite_set;
|
|
extern const mex_cmd_descriptor_t cmd_bite_clear;
|
|
|
|
extern const mex_mt_struct_t mex_mt_ris_tgtovr;
|
|
#endif
|
|
|
|
static const mex_mt_object_t ris_str[] =
|
|
{
|
|
MEX_HOTCMD_REF("sts", &cmd_eth),
|
|
MEX_HOTCMD_REF("sleep", &cmd_sleep),
|
|
|
|
MEX_HOTCMD_REF("tgtinit", &cmd_tgtinit),
|
|
MEX_HOTCMD_REF("tgtset", &cmd_tgtset),
|
|
MEX_HOTCMD_REF("tgtturn", &cmd_tgtturn),
|
|
|
|
#if 0
|
|
//MEX_HOTCMD_REF("message", &cmd_msg),
|
|
|
|
//MEX_HOTCMD_REF("scenario", &cmd_scenario),
|
|
|
|
//MEX_HOTCMD_REF("tgtinit", &cmd_tgtinit),
|
|
//MEX_HOTCMD_REF("tgtset", &cmd_tgtset),
|
|
//MEX_HOTCMD_REF("tgtreset", &cmd_tgtreset),
|
|
|
|
MEX_HOTCMD_REF("tgtrng", &cmd_tgtrng),
|
|
MEX_HOTCMD_REF("tgtaz", &cmd_tgtaz),
|
|
MEX_HOTCMD_REF("tgtvel", &cmd_tgtspeed),
|
|
MEX_HOTCMD_REF("tgtalt", &cmd_tgtalt),
|
|
MEX_HOTCMD_REF("tgthead", &cmd_tgthead),
|
|
MEX_HOTCMD_REF("tgtmap", &cmd_tgtmap),
|
|
|
|
MEX_HOTCMD_REF("aclatch", &cmd_aclatch),
|
|
MEX_HOTCMD_REF("acunlatch", &cmd_acunlatch),
|
|
MEX_HOTCMD_REF("pause", &cmd_pause),
|
|
MEX_HOTCMD_REF("continue", &cmd_continue),
|
|
|
|
MEX_HOTCMD_REF("recorder", &cmd_recorder),
|
|
MEX_HOTCMD_REF("wow", &cmd_wow),
|
|
|
|
MEX_HOTCMD_REF("biterest", &cmd_bite_reset),
|
|
MEX_HOTCMD_REF("biteset", &cmd_bite_set),
|
|
MEX_HOTCMD_REF("biteclear", &cmd_bite_clear),
|
|
|
|
{ 0, "ovr", &MEX_MT_CAST(mex_mt_ris_tgtovr), 0 },
|
|
{ 0, "dbgdeclutter", &mex_mt_uint, &cmp_dbg_display_hide_ },
|
|
|
|
#ifdef MCD_UDPFMT
|
|
MEX_CMD_REF("udp1553fmt", &cmd_udp1553fmt),
|
|
#endif
|
|
//MEX_CMD_REF("restart", &cmd_restart),
|
|
|
|
MEX_CMD_REF("_showreserved", &cmd_msg_res),
|
|
#endif
|
|
{ 0 }
|
|
};
|
|
|
|
const mex_mt_struct_t mex_mt_ris =
|
|
{
|
|
{ "ris", 0, &mex_mt_struct_methods }, ris_str };
|
|
|
|
static mex_mt_object_t ris_obj={
|
|
0, "ris", &MEX_MT_CAST(mex_mt_ris), 0
|
|
};
|
|
|
|
static void DspRisMexInstall()
|
|
{
|
|
static mex_cmd_server_t ris_srv;
|
|
mex_object_register(mex_object_root(), &ris_obj);
|
|
mex_add_command_server(&ris_srv, &ris_obj, "Radar Interface Simulator Commands");
|
|
//mex_set_user_mode(-1, "ris.");
|
|
|
|
mex_hobbit_register_instance("riscom", dsp_ris_static_noinit_data.risAgent);
|
|
}
|
|
|
|
bool DspRisIsActive()
|
|
{
|
|
bool activated=(dsp_ris_static_noinit_data.active[0]==0x12345678) && (dsp_ris_static_noinit_data.active[1]==0x87654321);
|
|
|
|
return activated;
|
|
}
|
|
|
|
void DspRisProcessEnvironment(unsigned int timetag, const iddtypes::inu_data_t* inu)
|
|
{
|
|
static unsigned int previous_time;
|
|
|
|
unsigned int dt=timetag-previous_time;
|
|
|
|
previous_time=timetag;
|
|
|
|
DspRisSfpUpdateScenario(timetag, *inu);
|
|
|
|
float delta_time=dt*64.0E-6;
|
|
|
|
if ((delta_time<0) || (delta_time>5))
|
|
return;
|
|
|
|
DspRisTgtUpdate(*inu, delta_time);
|
|
}
|
|
|
|
|