hm/sil
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
af18523dc992b37d71e94ec63fbe8a31a1d3a4d1
sil/mainwindow.cpp
T

634 lines
20 KiB
C++
Raw Blame History

#include "mainwindow.h"
#include "./ui_mainwindow.h"
#include <QLibrary>
#include <QHostAddress>
#include <QFileDialog>
#include <QDateTime>
extern "C"{
#include "SIL_capi.h"
}
void MainWindow::scan_param()
{
const rtwCAPI_ModelMappingStaticInfo *sm = (*getmap_func)();
if (sm)
{
//unsigned int n = rtwCAPI_GetNumBlockParametersFromStaticMap(sm);
unsigned int m = rtwCAPI_GetNumModelParametersFromStaticMap(sm);
ui->tableWidget->setRowCount(512);
QStringList hdr;
hdr.push_back(tr("Parameter"));
hdr.push_back(tr("Value"));
ui->tableWidget->setHorizontalHeaderLabels(hdr);
int cur = 0;
rtwCAPI_ModelParameters const *prm = rtwCAPI_GetModelParametersFromStaticMap(sm);
rtwCAPI_DataTypeMap const *dtm = rtwCAPI_GetDataTypeMapFromStaticMap(sm);
rtwCAPI_DimensionMap const *dmm = rtwCAPI_GetDimensionMapFromStaticMap(sm);
uint_T const *dam = rtwCAPI_GetDimensionArrayFromStaticMap(sm);
rtwCAPI_ModelMappingInfo *MMI = &(rtmGetDataMapInfo(this->m).mmi);
void **da = rtwCAPI_GetDataAddressMap(MMI);
for (unsigned int i = 0; i < m; ++i)
{
const char *name = rtwCAPI_GetModelParameterName(prm, i);
int idx = rtwCAPI_GetModelParameterAddrIdx(prm, i);
uint16_t dti = rtwCAPI_GetModelParameterDataTypeIdx(prm, i);
uint16_t dmi = rtwCAPI_GetModelParameterDimensionIdx(prm, i);
uint8_t ss = rtwCAPI_GetDataTypeSLId(dtm, dti);
uint16_t dai = rtwCAPI_GetDimArrayIndex(dmm, dmi);
uint8_t nd = rtwCAPI_GetNumDims(dmm, dmi);
uint16_t n = 1;
for (int j = 0; j < nd; ++j)
{
n *= dam[dai + j];
}
for (int j = 0; j < n; ++j)
{
QString param_name = QString::fromLatin1(name);
if (j>0)
{
param_name = param_name+QString("(%1)").arg(j);
}
QString param_value;
switch (ss)
{
case SS_DOUBLE:
{
double *d = (double *)da[idx];
param_value = QString("%1").arg(d[j]);
}
break;
case SS_SINGLE:
{
float *s = (float *)da[idx];
param_value = QString("%1").arg(s[j]);
}
break;
case SS_INT8:
{
int8_T *c = (int8_T *)da[idx];
param_value = QString("%1").arg(c[j]);
}
break;
case SS_INT16:
{
int16_T *h = (int16_T *)da[idx];
param_value = QString("%1").arg(h[j]);
}
break;
case SS_INT32:
{
int32_T *I = (int32_T *)da[idx];
param_value = QString("%1").arg(I[j]);
}
break;
case SS_UINT8:
{
uint8_T *c = (uint8_T *)da[idx];
param_value = QString("%1").arg(c[j]);
}
break;
case SS_BOOLEAN:
{
boolean_T *c = (boolean_T *)da[idx];
param_value = QString("%1").arg(c[j]);
}
break;
case SS_UINT16:
{
uint16_T *h = (uint16_T *)da[idx];
param_value = QString("%1").arg(h[j]);
}
break;
case SS_UINT32:
{
uint32_T *I = (uint32_T *)da[idx];
param_value = QString("%1").arg(I[j]);
}
break;
}
QTableWidgetItem *newItem = new QTableWidgetItem(param_name);
ui->tableWidget->setItem(cur, 0, newItem);
newItem = new QTableWidgetItem(param_value);
ui->tableWidget->setItem(cur++, 1, newItem);
if (param_name == "X0")
{
X0[0] = param_value.toDouble();
}
else if (param_name == "X0(1)")
{
X0[1] = param_value.toDouble();
}
else if (param_name == "X0(2)")
{
X0[2] = param_value.toDouble();
}
else if (param_name == "Att0")
{
Att0[0] = param_value.toDouble();
}
else if (param_name == "Att0(1)")
{
Att0[1] = param_value.toDouble();
}
else if (param_name == "Att0(2)")
{
Att0[2] = param_value.toDouble();
}
else if (param_name == "V0")
{
V0[0] = param_value.toDouble();
}
else if (param_name == "V0(1)")
{
V0[1] = param_value.toDouble();
}
else if (param_name == "V0(2)")
{
V0[2] = param_value.toDouble();
}
else if (param_name == "hground")
{
hground = param_value.toDouble();
}
}
}
ui->tableWidget->setRowCount(cur);
}
}
void MainWindow::setup_param(const QVector<QString> &head, const QVector<double> &onecase)
{
const rtwCAPI_ModelMappingStaticInfo *sm = (*getmap_func)();
if (sm)
{
//unsigned int n = rtwCAPI_GetNumBlockParametersFromStaticMap(sm);
unsigned int m = rtwCAPI_GetNumModelParametersFromStaticMap(sm);
rtwCAPI_ModelParameters const *prm = rtwCAPI_GetModelParametersFromStaticMap(sm);
rtwCAPI_DataTypeMap const *dtm = rtwCAPI_GetDataTypeMapFromStaticMap(sm);
rtwCAPI_DimensionMap const *dmm = rtwCAPI_GetDimensionMapFromStaticMap(sm);
uint_T const *dam = rtwCAPI_GetDimensionArrayFromStaticMap(sm);
rtwCAPI_ModelMappingInfo *MMI = &(rtmGetDataMapInfo(this->m).mmi);
void **da = rtwCAPI_GetDataAddressMap(MMI);
for (unsigned int i = 0; i < m; ++i)
{
const char *name = rtwCAPI_GetModelParameterName(prm, i);
int idx = rtwCAPI_GetModelParameterAddrIdx(prm, i);
uint16_t dti = rtwCAPI_GetModelParameterDataTypeIdx(prm, i);
uint16_t dmi = rtwCAPI_GetModelParameterDimensionIdx(prm, i);
uint8_t ss = rtwCAPI_GetDataTypeSLId(dtm, dti);
uint16_t dai = rtwCAPI_GetDimArrayIndex(dmm, dmi);
uint8_t nd = rtwCAPI_GetNumDims(dmm, dmi);
uint16_t n = 1;
for (int j = 0; j < nd; ++j)
{
n *= dam[dai + j];
}
int j = 0;
for (auto i = head.begin(); i != head.end(); ++i, ++j)
{
QStringList s = i->split(QRegExp("[()]"));
if (*i == name || ((s.size() >= 2) && (s.value(0) == name)))
{
int target_idx = 0;
double target_value = onecase[j];
if (s.size() >= 2)
{
target_idx = s.value(1).toInt();
}
switch (ss)
{
case SS_DOUBLE:
{
double *d = (double *)da[idx];
d[target_idx] = target_value;
}
break;
case SS_SINGLE:
{
float *s = (float *)da[idx];
s[target_idx] = target_value;
}
break;
case SS_INT8:
{
int8_T *c = (int8_T *)da[idx];
c[target_idx] = target_value;
}
break;
case SS_INT16:
{
int16_T *h = (int16_T *)da[idx];
h[target_idx] = target_value;
}
break;
case SS_INT32:
{
int32_T *I = (int32_T *)da[idx];
I[target_idx] = target_value;
}
break;
case SS_UINT8:
{
uint8_T *c = (uint8_T *)da[idx];
c[target_idx] = target_value;
}
break;
case SS_BOOLEAN:
{
bool *c = (bool *)da[idx];
c[target_idx] = target_value >=0.5?true:false;
}
break;
case SS_UINT16:
{
uint16_T *h = (uint16_T *)da[idx];
h[target_idx] = target_value;
}
break;
case SS_UINT32:
{
uint32_T *I = (uint32_T *)da[idx];
I[target_idx] = target_value;
}
break;
}
}
}
}
}
}
MainWindow::MainWindow(QWidget *parent)
: QMainWindow(parent), ui(new Ui::MainWindow), times(1), idx(0)
{
ui->setupUi(this);
QLibrary *mdl = new QLibrary("SIL_win64.dll");
if (mdl->isLoaded())
{
init_func = static_cast<SIL_initialize_ptr>(mdl->resolve("SIL_initialize"));
step_func = static_cast<SIL_step_ptr>(mdl->resolve("SIL_step"));
getmap_func = (SIL_GetCAPIStaticMap_ptr)(mdl->resolve("SIL_GetCAPIStaticMap"));
u = (ExtU_SIL_T *)mdl->resolve("SIL_U");
y = (ExtY_SIL_T *)mdl->resolve("SIL_Y");
V0 = (double *)mdl->resolve("V0");
m = (RT_MODEL_SIL_T *)mdl->resolve("SIL_M_");
}
else
{
init_func = &SIL_initialize;
step_func = &SIL_step;
getmap_func = &SIL_GetCAPIStaticMap;
u = &SIL_U;
y = &SIL_Y;
V0 = ::V0;
m = (RT_MODEL_SIL_T *)SIL_M;
}
ui->pushButton_start->setText(QString("Fail"));
stage = 0;
if (init_func && step_func && u && y && getmap_func)
{
SIL_InitializeDataMapInfo();
scan_param();
ui->lineEditLat->setText(QString("%1").arg(X0[0]));
ui->lineEditLon->setText(QString("%1").arg(X0[1]));
ui->lineEditAlt->setText(QString("%1").arg(hground));
ui->lineEditHdg->setText(QString("%1").arg(Att0[2]*57.3));
ui->lineEditSpeed->setText(QString("%1").arg(V0[0]));
gear_height = X0[2] - hground;
ui->lineEditAGL->setText(QString("%1").arg(gear_height));
qgc_sock = new QUdpSocket(this);
if (qgc_sock->bind(QHostAddress::LocalHost, 6061))
{
ui->pushButton_start->setText(QString("Start"));
stage = 1;
}
}
}
MainWindow::~MainWindow()
{
delete ui;
}
void MainWindow::update200Hz(void)
{
for (int j=0;j<times;++j)
{
u->data_len = 0u;
if (qgc_sock->hasPendingDatagrams())
{
qint64 len = qgc_sock->readDatagram((char *)u->data_in, 255);
if (len > 0)
{
u->data_len = static_cast<uint16_t>(len);
}
}
for (int i = 0; i < 1; ++i)
{
(*step_func)();
}
if (y->seq != seq)
{
seq = y->seq;
qgc_sock->writeDatagram((char *)y->buff, y->buff_len, QHostAddress::LocalHost, 6060);
}
if (y->rec_seq != seq_rec)
{
seq_rec = y->rec_seq;
rec_file->write((const char *)y->rec_buff, y->rec_len);
}
}
if (++idx >= 20)
{
idx = 0;
ui->lineEditMass->setText(QString("%1").arg(y->mass,6,'f',2));
ui->lineEditRPM->setText(QString("%1").arg(y->RPM,5,'f',0));
ui->lineEditDel->setText(QString("%1").arg(y->de*57.3,6,'f',2));
ui->lineEditDer->setText(QString("%1").arg(y->de*57.3,6,'f',2));
ui->lineEditDal->setText(QString("%1").arg(y->da*-57.3,6,'f',2));
ui->lineEditDar->setText(QString("%1").arg(y->da*57.3,6,'f',2));
ui->lineEditDr->setText(QString("%1").arg(y->dr*57.3,6,'f',2));
if (y->RPM > 44500 && u->launch_trigger == 0)
{
u->launch_trigger = 1.0;
ui->pushButton_start->setText(QString("Flying"));
ui->pushButtonLaunch->setText(QString("Flying"));
ui->pushButton_start->setEnabled(false);
ui->pushButtonLaunch->setEnabled(false);
stage = 3;
ui->tabWidget->setCurrentIndex(1);
}
if (ui->checkBoxTurbine->checkState() == Qt::Checked)
{
u->fault |= 1;
}
else
{
u->fault &= 0xFFFFFFFE;
}
if (ui->checkBoxDel->checkState() == Qt::Checked)
{
u->fault |= 2;
}
else
{
u->fault &= 0xFFFFFFFD;
}
if (ui->checkBoxDer->checkState() == Qt::Checked)
{
u->fault |= 4;
}
else
{
u->fault &= 0xFFFFFFFB;
}
if (ui->checkBoxDr->checkState() == Qt::Checked)
{
u->fault |= 8;
}
else
{
u->fault &= 0xFFFFFFF7;
}
if (ui->checkBoxDal->checkState() == Qt::Checked)
{
u->fault |= 0x10;
}
else
{
u->fault &= 0xFFFFFFEF;
}
if (ui->checkBoxDar->checkState() == Qt::Checked)
{
u->fault |= 0x20;
}
else
{
u->fault &= 0xFFFFFFDF;
}
}
}
void MainWindow::onFire()
{
if (stage == 1)
{
QVector<QString> head;
QVector<double> onecase;
int n = ui->tableWidget->rowCount();
for (int i=0;i<n;++i)
{
head.push_back(ui->tableWidget->item(i,0)->text());
onecase.push_back(ui->tableWidget->item(i,1)->text().toDouble());
}
double val;
bool ok;
val = ui->lineEditLat->text().toDouble(&ok);
if (ok)
{
head.push_back("X0");
onecase.push_back(val);
}
val = ui->lineEditLon->text().toDouble(&ok);
if (ok)
{
head.push_back("X0(1)");
onecase.push_back(val);
}
double hground;
val = ui->lineEditAlt->text().toDouble(&ok);
if (ok)
{
hground = val;
head.push_back("hground");
onecase.push_back(hground);
if (ui->radioButtonOnGround->isChecked())
{
head.push_back("X0(2)");
onecase.push_back(hground+gear_height);
}
}
val = ui->lineEditHdg->text().toDouble(&ok);
if (ok)
{
head.push_back("Att0(2)");
onecase.push_back(val/57.3);
}
if (ui->radioButtonInAir->isChecked())
{
head.push_back("Att0(1)");
onecase.push_back(0);
val = ui->lineEditSpeed->text().toDouble(&ok);
if (ok)
{
head.push_back("V0");
onecase.push_back(val);
}
val = ui->lineEditAGL->text().toDouble(&ok);
if (ok)
{
head.push_back("X0(2)");
onecase.push_back(hground+val);
}
head.push_back("force_sysauto_level");
onecase.push_back(0);
}
else
{
head.push_back("force_sysauto_level");
onecase.push_back(1);
}
setup_param(head, onecase);
(*init_func)();
QString filename = QString("sil")
+QDateTime::currentDateTime().toString("yyyyMMddhhmmss")
+".DAT";
rec_file = new QFile(filename, this);
rec_file->open(QFile::WriteOnly);
seq = 0;
seq_rec = 0;
timer200Hz = new QTimer(this);
connect(timer200Hz, SIGNAL(timeout()), this, SLOT(update200Hz()));
timer200Hz->start(5);
if (ui->radioButtonOnGround->isChecked())
{
stage = 2;
ui->pushButton_start->setText(QString("Launch"));
ui->pushButtonLaunch->setEnabled(true);
ui->tabWidget->setCurrentIndex(1);
u->fault = 0x0;
}
if (ui->radioButtonInAir->isChecked())
{
stage = 3;
ui->pushButton_start->setText(QString("Flying"));
ui->pushButtonLaunch->setText(QString("Flying"));
ui->pushButton_start->setEnabled(false);
ui->pushButtonLaunch->setEnabled(false);
ui->tabWidget->setCurrentIndex(1);
u->fault = 0x40;
}
}
else if (stage == 2)
{
u->launch_trigger = 1.0;
ui->pushButton_start->setText(QString("Flying"));
ui->pushButtonLaunch->setText(QString("Flying"));
ui->pushButton_start->setEnabled(false);
ui->pushButtonLaunch->setEnabled(false);
stage = 3;
ui->tabWidget->setCurrentIndex(1);
}
}
void MainWindow::onExport()
{
QString filename = QFileDialog::getSaveFileName(this, tr("Save to"), QString(), tr("Text File (*.txt)"));
if (!filename.isEmpty())
{
QFile data(filename);
if (data.open(QFile::WriteOnly | QFile::Truncate)) {
QTextStream out(&data);
int n = ui->tableWidget->rowCount();
for (int i=0;i<n;++i)
{
out << ui->tableWidget->item(i,0)->text() << ',';
out << ui->tableWidget->item(i,1)->text() << '\n';
}
out << QString("X0") << ',' << ui->lineEditLat->text() << '\n';
out << QString("X0(1)") << ',' << ui->lineEditLon->text() << '\n';
out << QString("hground") << ',' << ui->lineEditAlt->text() << '\n';
out << QString("Att0(2)") << ',' << ui->lineEditHdg->text().toDouble()/57.3 << '\n';
}
}
}
void MainWindow::onImport()
{
QString filename = QFileDialog::getOpenFileName(this, tr("Save to"), QString(), tr("Text File (*.txt)"));
if (!filename.isEmpty())
{
QFile data(filename);
if (data.open(QFile::ReadOnly)) {
QTextStream stream(&data);
QString line;
while (stream.readLineInto(&line)) {
QStringList items = line.split(QString(","));
int n = ui->tableWidget->rowCount();
for (int i=0;i<n;++i)
{
if (ui->tableWidget->item(i,0)->text() == items[0])
{
ui->tableWidget->item(i,1)->setText(items[1]);
break;
}
}
}
{
int n = ui->tableWidget->rowCount();
for (int i=0;i<n;++i)
{
if (ui->tableWidget->item(i,0)->text() == "X0")
{
ui->lineEditLat->setText(ui->tableWidget->item(i,1)->text());
}
else if (ui->tableWidget->item(i,0)->text() == "X0(1)")
{
ui->lineEditLon->setText(ui->tableWidget->item(i,1)->text());
}
else if (ui->tableWidget->item(i,0)->text() == "hground")
{
ui->lineEditAlt->setText(ui->tableWidget->item(i,1)->text());
}
else if (ui->tableWidget->item(i,0)->text() == "Att0(2)")
{
ui->lineEditHdg->setText(QString("%1").arg(ui->tableWidget->item(i,1)->text().toDouble()*57.3));
}
}
}
}
}
}
void MainWindow::onTimes(int i)
{
times = i;
}
void MainWindow::onAG()
{
if (ui->radioButtonOnGround->isChecked())
{
ui->lineEditSpeed->setEnabled(false);
ui->lineEditAGL->setEnabled(false);
}
if (ui->radioButtonInAir->isChecked())
{
ui->lineEditSpeed->setEnabled(true);
ui->lineEditAGL->setEnabled(true);
}
}
Reference in New Issue View Git Blame Copy Permalink