562 lines
13 KiB
C++
562 lines
13 KiB
C++
/*
|
|
* dsp_ris_mex.cpp
|
|
*
|
|
*/
|
|
|
|
#include "dsp_ris_internals_.h"
|
|
|
|
#include "mex.h"
|
|
#include "gc_math.h"
|
|
|
|
struct ris_rfloat_t
|
|
{
|
|
float value;
|
|
bool relative;
|
|
};
|
|
|
|
|
|
extern "C" int mex_mt_helper_show_float(mex_mt_printf_style_function_t f, float value, int width, int precision);
|
|
|
|
static int mt_show_rfloat(const struct mex_mt_tag_t* const mt,
|
|
mex_mt_printf_style_function_t f, const void* address, int width, int mode, const mex_mt_reference_t* ref)
|
|
{
|
|
(void) mt;
|
|
(void) mode;
|
|
(void) ref;
|
|
return mex_mt_helper_show_float(f, *(float*) address, width, -1);
|
|
}
|
|
|
|
extern int mex_mt_helper_read_float(float* f, const char** str);
|
|
|
|
static mex_cmd_param_parse_result_t mt_read_rfloat(
|
|
const struct mex_mt_tag_t* const /*mt*/, void* address, const char** str, const mex_mt_reference_t* /*ref*/)
|
|
{
|
|
ris_rfloat_t* result=reinterpret_cast<ris_rfloat_t*>(address);
|
|
result->relative=false;
|
|
result->value=0;
|
|
if (**str=='%')
|
|
{
|
|
result->relative=true;
|
|
++(*str);
|
|
}
|
|
int nok=mex_mt_helper_read_float(&(result->value), str);
|
|
if (nok)
|
|
return mex_cmd_param_parse_error;
|
|
|
|
return mex_cmd_param_parse_ok;
|
|
}
|
|
|
|
static const mex_mt_methods_t ris_mt_relfloat_methods =
|
|
{ 0, mt_show_rfloat, mt_read_rfloat, 0, 0, 0};
|
|
|
|
const mex_mt_t ris_mt_relfloat =
|
|
{ "rfloat", sizeof(float), &ris_mt_relfloat_methods };
|
|
|
|
|
|
static float from_angle(float v)
|
|
{
|
|
return gc_math::rad_to_deg(v); //v*cnv->angles;
|
|
}
|
|
|
|
struct target_params_t
|
|
{
|
|
int id;
|
|
ris_rfloat_t range;
|
|
ris_rfloat_t az;
|
|
ris_rfloat_t heading;
|
|
ris_rfloat_t speed;
|
|
ris_rfloat_t altitude;
|
|
|
|
int restart;
|
|
int start;
|
|
int az_abs;
|
|
int traceable;
|
|
|
|
int altitude_is_el;
|
|
|
|
struct par_validity_t
|
|
{
|
|
bool set_az;
|
|
bool set_range;
|
|
bool set_speed;
|
|
bool set_heading;
|
|
bool set_altitude;
|
|
bool set_elevation;
|
|
int set_id;
|
|
int start;
|
|
int restart;
|
|
} validity;
|
|
ris_rfloat_t generic_value;
|
|
};
|
|
|
|
static target_params_t tgt_params;
|
|
|
|
#ifdef __MINGW32__
|
|
#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
|
|
#endif
|
|
|
|
static const mex_cmd_param_descriptor_t target_table[] =
|
|
{
|
|
{ "id", "target id", 0, &tgt_params.id, &mex_mt_int},
|
|
{ "rng", "range (NM)", 0, &tgt_params.range, &ris_mt_relfloat},
|
|
{ "az", "azimuth (deg)", 0, &tgt_params.az, &ris_mt_relfloat},
|
|
{ "velocity", "speed (feet/s)", 0, &tgt_params.speed, &ris_mt_relfloat},
|
|
{ "heading", "heading (deg)", 0, &tgt_params.heading, &ris_mt_relfloat},
|
|
{ "altitude", "altitude (feet)", 0, &tgt_params.altitude, &ris_mt_relfloat},
|
|
|
|
{ "r", "restart", mex_cmd_qualifier, &tgt_params.restart },
|
|
{ "s", "start", mex_cmd_qualifier, &tgt_params.start },
|
|
{ "t", "traceable", mex_cmd_qualifier, &tgt_params.traceable },
|
|
{ "e", "set elevation instead of altitude (experimental)", mex_cmd_qualifier, &tgt_params.altitude_is_el },
|
|
|
|
{ 0 }
|
|
};
|
|
|
|
|
|
static void RisTgtTakeMutex()
|
|
{
|
|
}
|
|
|
|
static void RisTgtReleaseMutex()
|
|
{
|
|
}
|
|
|
|
static void RisTargetsRestart(int i)
|
|
{
|
|
auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];
|
|
|
|
t.az=t.az_start;
|
|
t.ground_range=t.rng_start_meters;
|
|
t.altitude=t.altitude_start;
|
|
t.el=t.el_start;
|
|
t.speed=t.speed_start;
|
|
++t.unique_id;
|
|
}
|
|
|
|
|
|
static bool angle_convert(float& az, float v)
|
|
{
|
|
az=0;
|
|
if ((v>361) || (v<-361))
|
|
return false;
|
|
az=gc_math::angle_bound_pi(gc_math::deg_to_rad(v));
|
|
return true;
|
|
}
|
|
|
|
static bool speed_convert(float& s, float v, bool relative)
|
|
{
|
|
s=0;
|
|
if (v>4000)
|
|
return false;
|
|
if (relative)
|
|
{
|
|
if (v<-4000)
|
|
return false;
|
|
}
|
|
else if (v<0)
|
|
return false;
|
|
|
|
s=gc_math::feet_to_meters(v);
|
|
return true;
|
|
}
|
|
|
|
static bool range_convert(float& r, float v)
|
|
{
|
|
r=0;
|
|
if ((v<0) || (v>200))
|
|
return false;
|
|
r=gc_math::nm_to_meters(v);
|
|
return true;
|
|
}
|
|
|
|
static bool altitude_convert(float& r, float v)
|
|
{
|
|
r=0;
|
|
if ((v<-40000) || (v>40000))
|
|
return false;
|
|
r=gc_math::feet_to_meters(v);
|
|
return true;
|
|
}
|
|
|
|
class SimpleTgtSimInfo
|
|
{
|
|
public:
|
|
enum { k_max_tgt=DSP_RIS_MAX_TGT};
|
|
};
|
|
|
|
static mex_res_t Cmd_TargetApply(int mode)
|
|
{
|
|
//RadarInterfaceSimulator& s=RadarInterfaceSimulator::instance();
|
|
int start=tgt_params.validity.start; //mex_param_is_present(&tgt_params.stop);
|
|
bool restart=tgt_params.validity.restart; //mex_param_is_present(&tgt_params.restart);
|
|
|
|
|
|
int t_start=0;
|
|
int t_stop=SimpleTgtSimInfo::k_max_tgt;
|
|
bool only_active=start==0;
|
|
if (tgt_params.validity.set_id) //mex_param_is_present(&tgt_params.id))
|
|
{
|
|
if ((tgt_params.id<0) || (tgt_params.id>=SimpleTgtSimInfo::k_max_tgt))
|
|
return mex_err_invalid_range;
|
|
t_start=tgt_params.id;
|
|
t_stop=t_start+1;
|
|
only_active=false;
|
|
}
|
|
|
|
float az=0;
|
|
float el=0;
|
|
float range=0;
|
|
float heading=0;
|
|
float altitude=0;
|
|
float speed=0;
|
|
|
|
bool set_az=tgt_params.validity.set_az;//mex_param_is_present(&tgt_params.az);
|
|
bool set_range=tgt_params.validity.set_range;//mex_param_is_present(&tgt_params.range);
|
|
bool set_speed=tgt_params.validity.set_speed;//mex_param_is_present(&tgt_params.speed);
|
|
bool set_heading=tgt_params.validity.set_heading;//mex_param_is_present(&tgt_params.heading);
|
|
bool set_altitude=tgt_params.validity.set_altitude;//mex_param_is_present(&tgt_params.altitude);
|
|
bool set_elevation=tgt_params.validity.set_elevation;//mex_param_is_present(&tgt_params.altitude_is_el);
|
|
|
|
if (set_az)
|
|
{
|
|
bool ok=angle_convert(az, tgt_params.az.value);
|
|
if (!ok)
|
|
{
|
|
mex_msg_printf(mex_msg_error, "TGT", "invalid azimuth (%f)", tgt_params.az.value);
|
|
return mex_ok;
|
|
}
|
|
}
|
|
if (set_heading)
|
|
{
|
|
bool ok=angle_convert(heading, tgt_params.heading.value);
|
|
if (!ok)
|
|
{
|
|
mex_msg_printf(mex_msg_error, "TGT", "invalid heading (%f)", tgt_params.heading.value);
|
|
return mex_ok;
|
|
}
|
|
}
|
|
if (set_speed)
|
|
{
|
|
bool ok=speed_convert(speed, tgt_params.speed.value, tgt_params.speed.relative);
|
|
if (!ok)
|
|
{
|
|
mex_msg_printf(mex_msg_error, "TGT", "invalid velocity (%f)", tgt_params.speed.value);
|
|
return mex_ok;
|
|
}
|
|
}
|
|
if (set_range)
|
|
{
|
|
bool ok=false;
|
|
if (tgt_params.range.relative)
|
|
{
|
|
range=tgt_params.range.value;
|
|
ok=(tgt_params.range.value>=0) && (tgt_params.range.value<200.0);
|
|
}
|
|
else
|
|
ok=range_convert(range, tgt_params.range.value);
|
|
if (!ok)
|
|
{
|
|
mex_msg_printf(mex_msg_error, "TGT", "invalid range (%f)", tgt_params.range.value);
|
|
return mex_ok;
|
|
}
|
|
}
|
|
if (set_altitude)
|
|
{
|
|
if (set_elevation)
|
|
{
|
|
bool ok=angle_convert(el, tgt_params.heading.value);
|
|
if (!ok)
|
|
{
|
|
mex_msg_printf(mex_msg_error, "TGT", "invalid elevation (%f)", tgt_params.altitude.value);
|
|
return mex_ok;
|
|
}
|
|
set_altitude=false;
|
|
}
|
|
else
|
|
{
|
|
bool ok=altitude_convert(altitude, tgt_params.altitude.value);
|
|
if (!ok)
|
|
{
|
|
mex_msg_printf(mex_msg_error, "TGT", "invalid altitude (%f)", tgt_params.altitude);
|
|
return mex_ok;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
{
|
|
RisTgtTakeMutex();
|
|
//AvionicsNavigationData* avn=AvionicsDataStore::getData()->getItsNavigationData();
|
|
//ac_data.actualize(avn->getInu(), avn->getNavBlended());
|
|
//RisData* d=RadarInterfaceSimulator::instance().risData();
|
|
//ac_data.actualize(d->nav);
|
|
RisTgtReleaseMutex();
|
|
}
|
|
|
|
const iddtypes::inu_data_t& inu=DspRisTgtInuData();
|
|
|
|
if (tgt_params.range.relative)
|
|
{
|
|
//AvionicsControlData::settings_t& cmd=AvionicsDataStore::getData()->getItsControlData()->getBasicSettings();
|
|
//float scale=cmd.range_scale_m;
|
|
//range=(range/100)*scale;
|
|
}
|
|
|
|
if (tgt_params.speed.relative)
|
|
{
|
|
float v=dsp_ris_tgt_db.vxy; //sqrtf(inu.own_ship_velocity.vxac_data.vx*ac_data.vx+ac_data.vy*ac_data.vy);
|
|
speed+=v;
|
|
}
|
|
|
|
if (tgt_params.heading.relative)
|
|
{
|
|
heading+=inu.own_ship_attitude.vector.platform_azimuth; //ac_data.platform_azimuth;
|
|
}
|
|
|
|
//bool abs_mode=mex_param_is_present(&tgt_params.az_abs);
|
|
|
|
float reference_az=tgt_params.az.relative ? inu.own_ship_attitude.vector.platform_azimuth : 0;
|
|
float reference_altitude=tgt_params.altitude.relative ? inu.inertial_altitude.data : 0;
|
|
|
|
|
|
az+=reference_az;
|
|
altitude+=reference_altitude;
|
|
|
|
//SimpleTgtSimInfo& si=SimpleTgtSimInfo::instance();
|
|
|
|
int traceable=mex_param_is_present(&tgt_params.traceable);
|
|
|
|
RisTgtTakeMutex();
|
|
for(int i=t_start; i<t_stop; ++i)
|
|
{
|
|
auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];
|
|
|
|
if (start>0)
|
|
{
|
|
//if (t.rng_start_meters)
|
|
t.enabled=1;
|
|
}
|
|
else if (start<0)
|
|
t.enabled=0;
|
|
|
|
if (only_active && !t.enabled)
|
|
continue;
|
|
|
|
if (mode==0)
|
|
{
|
|
if (set_az)
|
|
t.az_start=az;
|
|
if (set_range)
|
|
t.rng_start_meters=range;
|
|
if (set_heading)
|
|
t.heading_start=heading;
|
|
if (set_speed)
|
|
{
|
|
t.speed_start=speed;
|
|
//t.rng_rate=0;
|
|
}
|
|
|
|
if (set_altitude)
|
|
{
|
|
t.altitude_start=altitude;
|
|
t.altitude_invalid=false;
|
|
}
|
|
if (set_elevation)
|
|
{
|
|
t.el_start=el;
|
|
t.altitude_invalid=true;
|
|
}
|
|
}//end only init
|
|
|
|
if (traceable>0)
|
|
t.not_traceable=0;
|
|
else if (traceable<0)
|
|
t.not_traceable=1;
|
|
|
|
if (restart || ((start>0) && (mode==0)))
|
|
{
|
|
#if 0
|
|
t.az=t.az_start;
|
|
t.range=t.rng_start_meters;
|
|
t.speed=t.speed_start;
|
|
t.heading=t.heading_start;
|
|
t.altitude=t.altitude_start;
|
|
t.el=t.el_start;
|
|
if (mode==0)
|
|
{
|
|
t.restarted=1;
|
|
}
|
|
#endif
|
|
RisTargetsRestart(i);
|
|
}
|
|
|
|
if (mode==1)
|
|
{
|
|
if (set_az)
|
|
t.az=az;
|
|
if (set_range)
|
|
{
|
|
//t.rng_meters=range;
|
|
t.ground_range=range;
|
|
}
|
|
if (set_heading)
|
|
t.heading=heading;
|
|
if (set_speed)
|
|
{
|
|
t.speed=speed;
|
|
}
|
|
if (set_altitude)
|
|
{
|
|
t.altitude=altitude;
|
|
t.altitude_invalid=false;
|
|
}
|
|
if (set_elevation)
|
|
{
|
|
t.el=el;
|
|
t.altitude_invalid=true;
|
|
}
|
|
if (restart)
|
|
t.enabled=1;
|
|
t.restarted=restart;
|
|
}
|
|
}
|
|
RisTgtReleaseMutex();
|
|
|
|
if (mode==0)
|
|
{
|
|
for(int i=t_start; i<t_stop; ++i)
|
|
{
|
|
auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];
|
|
|
|
if (only_active && !t.enabled)
|
|
continue;
|
|
mex_printf(
|
|
"%c%c%u: range=%7.3f, az=%8.3f, el=%8.3f vel=%5.3f, heading=%8.3f, altitude=%5.0f\n",
|
|
t.enabled ? '+' : '-',
|
|
t.not_traceable ? '-' : '+',
|
|
i,
|
|
gc_math::meters_to_nm(t.rng_start_meters), from_angle(t.az_start), from_angle(t.el_start), gc_math::meters_to_feet(t.speed_start), from_angle(t.heading_start), gc_math::meters_to_feet(t.altitude_start));
|
|
}
|
|
return mex_ok;
|
|
}
|
|
|
|
mex_page_loop_init(1);
|
|
do
|
|
{
|
|
mex_page_loop_top();
|
|
|
|
mex_printf(" %3.3s %7.7s %6.6s %8.8s %8.8s %8.8s %8.8s %8.8s %8.8s %8.8s %8.8s %s \n",
|
|
"TRK", "GR[NM]", "(R[m])", "AZ^", "VEL[FT]", "HEAD", "ALT[FT]", "(EL)", "(RR[ms])", "(RV[ms])", "(hit)", "(cc)");
|
|
|
|
for(int i=t_start; i<t_stop; ++i)
|
|
{
|
|
auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];
|
|
|
|
//mex_printf("%c%c%2u: %3d range=%7.3f, az=%8.3f, el=%8.3f, vel=%5.3f, heading=%8.3f, altitude=%5.0f\n",
|
|
mex_printf("%c%c%2u: %3d %7.2f %6d %8.3f %8.3f %8.3f%c %8.1f %8.3f %8.3f %8.3f %8u %5u \n",
|
|
t.enabled ? '+' : '-',
|
|
t.not_traceable ? '-' : '+',
|
|
i,
|
|
(t.unique_id & 0x0FF), //t.unique_id<0 ? -1 : (t.unique_id & 0x0FF),
|
|
gc_math::meters_to_nm(t.ground_range),
|
|
int(t.slant_range),
|
|
from_angle(t.az),
|
|
gc_math::meters_to_feet(t.speed),
|
|
from_angle(t.heading),
|
|
|
|
((t.turn_duration>0) && (t.turn)) ? ((t.turn>0) ? '>' : '<') : '=',
|
|
|
|
gc_math::meters_to_feet(t.altitude),
|
|
from_angle(t.el),
|
|
t.range_rate,
|
|
t.rad_vel,
|
|
t.hits,
|
|
t.ext_updated & 0x0FFFF
|
|
);
|
|
}
|
|
} while (mex_page_loop_end(0, 0) != mex_page_event_quit);
|
|
|
|
return mex_ok;
|
|
}
|
|
|
|
|
|
static mex_res_t Cmd_Target(int mode)
|
|
{
|
|
tgt_params.validity.set_az=mex_param_is_present(&tgt_params.az);
|
|
tgt_params.validity.set_range=mex_param_is_present(&tgt_params.range);
|
|
tgt_params.validity.set_speed=mex_param_is_present(&tgt_params.speed);
|
|
tgt_params.validity.set_heading=mex_param_is_present(&tgt_params.heading);
|
|
tgt_params.validity.set_altitude=mex_param_is_present(&tgt_params.altitude);
|
|
tgt_params.validity.set_elevation=mex_param_is_present(&tgt_params.altitude_is_el);
|
|
tgt_params.validity.start=mex_param_is_present(&tgt_params.start);
|
|
tgt_params.validity.restart=mex_param_is_present(&tgt_params.restart);
|
|
tgt_params.validity.set_id=mex_param_is_present(&tgt_params.id);
|
|
|
|
return Cmd_TargetApply(mode);
|
|
}
|
|
|
|
static mex_res_t Cmd_TgtInit()
|
|
{
|
|
return Cmd_Target(0);
|
|
}
|
|
|
|
static mex_res_t Cmd_TgtSet()
|
|
{
|
|
return Cmd_Target(1);
|
|
}
|
|
|
|
static float turn_deg;
|
|
static float turn_time;
|
|
|
|
static const mex_cmd_param_descriptor_t target_turn_table[] =
|
|
{
|
|
{ "id", "target id", 0, &tgt_params.id, &mex_mt_int},
|
|
{ "deg", "deg/s", 0, &turn_deg, &mex_mt_float},
|
|
{ "time", "turn duration", 0, &turn_time, &mex_mt_float},
|
|
{ 0 }
|
|
};
|
|
|
|
static mex_res_t Cmd_TgtTurn()
|
|
{
|
|
int t_start=0;
|
|
int t_stop=SimpleTgtSimInfo::k_max_tgt;
|
|
if (tgt_params.validity.set_id) //mex_param_is_present(&tgt_params.id))
|
|
{
|
|
if ((tgt_params.id<0) || (tgt_params.id>=SimpleTgtSimInfo::k_max_tgt))
|
|
return mex_err_invalid_range;
|
|
t_start=tgt_params.id;
|
|
t_stop=t_start+1;
|
|
}
|
|
|
|
bool set_turn=mex_param_is_present(&turn_deg);
|
|
bool set_time=mex_param_is_present(&turn_time);
|
|
|
|
turn_deg=gc_math::deg_to_rad(turn_deg);
|
|
|
|
for(int i=t_start; i<t_stop; ++i)
|
|
{
|
|
auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];
|
|
|
|
if (set_turn)
|
|
t.turn=turn_deg;
|
|
if (set_time)
|
|
{
|
|
t.turn_duration=turn_time;
|
|
}
|
|
}
|
|
|
|
return mex_ok;
|
|
}
|
|
|
|
|
|
extern const mex_cmd_descriptor_t cmd_tgtinit=
|
|
{ "tgtset", "Manipulate targets", (mex_cmd_handler_t) Cmd_TgtInit, target_table};
|
|
|
|
|
|
extern const mex_cmd_descriptor_t cmd_tgtset =
|
|
{ "tgtset", "Manipulate targets", (mex_cmd_handler_t) Cmd_TgtSet, target_table};
|
|
|
|
|
|
extern const mex_cmd_descriptor_t cmd_tgtturn =
|
|
{ "tgtturn", "Manipulate targets", (mex_cmd_handler_t) Cmd_TgtTurn, target_turn_table};
|