diff --git a/Control/Joystick.cpp b/Control/Joystick.cpp index ee3326a..67ae957 100644 --- a/Control/Joystick.cpp +++ b/Control/Joystick.cpp @@ -21,6 +21,7 @@ Joystick::Joystick(int updateTimeMs): ControlBase("Joystick") { void Joystick::updateDevice() { sf::Joystick::update(); + // qDebug() << "updateDevice"; for (unsigned int i = 0; i < sizeof(axis_table) / sizeof(axis_table[0]); i++) { sendAction(axis_table[i].action, diff --git a/ThrusterSettings/Thruster.cpp b/ThrusterSettings/Thruster.cpp index 6395dcf..789f599 100644 --- a/ThrusterSettings/Thruster.cpp +++ b/ThrusterSettings/Thruster.cpp @@ -9,7 +9,7 @@ Thruster::Thruster(QWidget *parent) : //change parameters connect(ui->CheckBox_ThrusterPower, SIGNAL(stateChanged(int)), this, SLOT(powerCheckBoxChanged(int))); - connect(ui->SpinBox_ThrusterId, SIGNAL(valueChanged(int)), this, SLOT(idChanged(int))); + connect(ui->SpinBox_ThrusterId, SIGNAL(valueChanged(int)), this, SLOT(slotChanged(int))); connect(ui->SpinBox_ThrusterSetSpeed, SIGNAL(valueChanged(int)), this, SLOT(speedChanged(int))); connect(ui->DoubleSpinBox_SetForwardK, SIGNAL(valueChanged(double)), this, SLOT(forwardKChanged(double))); connect(ui->DoubleSpinBox_SetBackwardK, SIGNAL(valueChanged(double)), this, SLOT(backwardKChanged(double))); @@ -87,14 +87,14 @@ void Thruster::powerCheckBoxChanged(int power) { emit parametorsChanged(ThrusterJson, ThisThruster); } -void Thruster::idChanged(int id) { +void Thruster::slotChanged(int id) { ui->SpinBox_ThrusterSetSpeed->setValue(0); // обнулям, чтобы не крутились не нужные движки ThisThruster.velocity = 0; emit parametorsChanged(ThrusterJson, ThisThruster); if (autoSave == true) ThrusterJson["id"] = id; - ThisThruster.id = id; + ThisThruster.slot = id; emit parametorsChanged(ThrusterJson, ThisThruster); } diff --git a/ThrusterSettings/Thruster.h b/ThrusterSettings/Thruster.h index 03e2696..b47bc41 100644 --- a/ThrusterSettings/Thruster.h +++ b/ThrusterSettings/Thruster.h @@ -50,7 +50,7 @@ public slots: private slots: void powerCheckBoxChanged(int power); - void idChanged(int id); + void slotChanged(int slot); void speedChanged(int speed); void forwardKChanged(double forwardK); void backwardKChanged(double backwardK); diff --git a/UV/iserverdata.cpp b/UV/iserverdata.cpp index b853074..d3d74e3 100644 --- a/UV/iserverdata.cpp +++ b/UV/iserverdata.cpp @@ -4,16 +4,18 @@ #include #include -uint16_t getCheckSumm16b(char* pcBlock, int len); -uint8_t isCheckSumm16bCorrect(char* pcBlock, int len); +uint16_t getCheckSumm16b(char *pcBlock, int len); +uint8_t isCheckSumm16bCorrect(char *pcBlock, int len); -void set_bit(uint8_t& byte, uint8_t bit, bool state); +void set_bit(uint8_t &byte, uint8_t bit, bool state); IServerData::IServerData() - : IBasicData() { + : IBasicData() +{ } -int IServerData::getThrusterAmount() { +int IServerData::getThrusterAmount() +{ int thrusterAmount; UVMutex.lock(); thrusterAmount = UVState.getThrusterAmount(); @@ -21,7 +23,8 @@ int IServerData::getThrusterAmount() { return thrusterAmount; } -QString IServerData::getUdpHostAddress() { +QString IServerData::getUdpHostAddress() +{ QString udpHostAddress; UVMutex.lock(); udpHostAddress = UVState.udpHostAddress; @@ -29,7 +32,8 @@ QString IServerData::getUdpHostAddress() { return udpHostAddress; } -quint16 IServerData::getUdpHostPort() { +quint16 IServerData::getUdpHostPort() +{ quint16 udpHostPort; UVMutex.lock(); udpHostPort = UVState.udpHostPort; @@ -37,7 +41,8 @@ quint16 IServerData::getUdpHostPort() { return udpHostPort; } -e_Countour IServerData::getCurrentControlContour() { +e_Countour IServerData::getCurrentControlContour() +{ e_Countour currentControlContour; UVMutex.lock(); @@ -47,10 +52,12 @@ e_Countour IServerData::getCurrentControlContour() { return currentControlContour; } -QByteArray IServerData::generateMessage() { +QByteArray IServerData::generateMessage() +{ QByteArray formed; formed.clear(); - switch (getCurrentPackageMode()) { + switch (getCurrentPackageMode()) + { case PACKAGE_NORMAL: formed = generateNormalMessage(); break; @@ -64,7 +71,8 @@ QByteArray IServerData::generateMessage() { return formed; } -e_packageMode IServerData::getCurrentPackageMode() { +e_packageMode IServerData::getCurrentPackageMode() +{ e_packageMode currentPackageMode; UVMutex.lock(); currentPackageMode = UVState.currentPackageMode; @@ -72,7 +80,8 @@ e_packageMode IServerData::getCurrentPackageMode() { return currentPackageMode; } -QByteArray IServerData::generateNormalMessage() { +QByteArray IServerData::generateNormalMessage() +{ QByteArray msg; msg.clear(); QDataStream stream(&msg, QIODeviceBase::Append); @@ -90,7 +99,8 @@ QByteArray IServerData::generateNormalMessage() { stream << req.roll; stream << req.pitch; stream << req.yaw; - for (int i = 0; i < 6; i++) { + for (int i = 0; i < 6; i++) + { stream << req.dev[i]; } @@ -99,7 +109,8 @@ QByteArray IServerData::generateNormalMessage() { return msg; } -void IServerData::fillStructure(RequestNormalMessage& req) { +void IServerData::fillStructure(RequestNormalMessage &req) +{ UVMutex.lock(); set_bit(req.flags, 0, UVState.stabRoll); @@ -116,14 +127,16 @@ void IServerData::fillStructure(RequestNormalMessage& req) { req.pitch = UVState.control.pitch; req.yaw = UVState.control.yaw; - for (int i = 0; i < 6; i++) { + for (int i = 0; i < 6; i++) + { req.dev[i] = UVState.device[i].velocity; } UVMutex.unlock(); } -QByteArray IServerData::generateConfigMessage() { +QByteArray IServerData::generateConfigMessage() +{ QByteArray msg; msg.clear(); QDataStream stream(&msg, QIODeviceBase::Append); @@ -166,7 +179,8 @@ QByteArray IServerData::generateConfigMessage() { return msg; } -void IServerData::fillStructure(RequestConfigMessage& req) { +void IServerData::fillStructure(RequestConfigMessage &req) +{ UVMutex.lock(); e_Countour currentControlContour = UVState.currentControlContour; @@ -194,7 +208,8 @@ void IServerData::fillStructure(RequestConfigMessage& req) { UVMutex.unlock(); } -QByteArray IServerData::generateDirectMessage() { +QByteArray IServerData::generateDirectMessage() +{ QByteArray msg; msg.clear(); QDataStream stream(&msg, QIODeviceBase::Append); @@ -219,16 +234,20 @@ QByteArray IServerData::generateDirectMessage() { return msg; } -void IServerData::fillStructure(RequestDirectMessage& req) { +void IServerData::fillStructure(RequestDirectMessage &req) +{ UVMutex.lock(); UVState.setThrusterNext(); req.id = UVState.currentThruster; req.slot = UVState.thruster[UVState.currentThruster].slot; - if (UVState.thruster[UVState.currentThruster].power == false) { + if (UVState.thruster[UVState.currentThruster].power == false) + { req.velocity = 0; - } else { + } + else + { req.velocity = UVState.thruster[UVState.currentThruster].velocity; } @@ -242,8 +261,10 @@ void IServerData::fillStructure(RequestDirectMessage& req) { UVMutex.unlock(); } -void IServerData::parseMessage(QByteArray message) { - switch (getCurrentPackageMode()) { +void IServerData::parseMessage(QByteArray message) +{ + switch (getCurrentPackageMode()) + { case PACKAGE_NORMAL: parseNormalMessage(message); break; @@ -258,7 +279,8 @@ void IServerData::parseMessage(QByteArray message) { } } -void IServerData::parseNormalMessage(QByteArray msg) { +void IServerData::parseNormalMessage(QByteArray msg) +{ ResponseNormalMessage res; uint16_t checksum_calc = getCheckSumm16b(msg.data(), msg.size() - 2); @@ -277,20 +299,21 @@ void IServerData::parseNormalMessage(QByteArray msg) { stream >> res.checksum; - if (res.checksum != checksum_calc) { - qDebug() << "Checksum NormalMessage is invalid"; - std::stringstream stream; - stream << "[ISERVERDATA] Checksum is invalid. Calculated: [" << - std::ios::hex << checksum_calc << "] " << - "Received: [" << - std::ios::hex << res.checksum << "]"; - throw std::invalid_argument(stream.str()); - } + // if (res.checksum != checksum_calc) { + // qDebug() << "Checksum NormalMessage is invalid"; + // std::stringstream stream; + // stream << "[ISERVERDATA] Checksum is invalid. Calculated: [" << + // std::ios::hex << checksum_calc << "] " << + // "Received: [" << + // std::ios::hex << res.checksum << "]"; + // throw std::invalid_argument(stream.str()); + // } pullFromStructure(res); } -void IServerData::pullFromStructure(ResponseNormalMessage res) { +void IServerData::pullFromStructure(ResponseNormalMessage res) +{ UVMutex.lock(); UVState.imu.roll = res.roll; @@ -305,7 +328,8 @@ void IServerData::pullFromStructure(ResponseNormalMessage res) { UVMutex.unlock(); } -void IServerData::parseConfigMessage(QByteArray msg) { +void IServerData::parseConfigMessage(QByteArray msg) +{ ResponseConfigMessage res; uint16_t checksum_calc = getCheckSumm16b(msg.data(), msg.size() - 2); @@ -341,20 +365,20 @@ void IServerData::parseConfigMessage(QByteArray msg) { stream >> res.checksum; - if (res.checksum != checksum_calc) { + if (res.checksum != checksum_calc) + { qDebug() << "Checksum ConfigMessage is invalid"; std::stringstream stream; - stream << "[ISERVERDATA] Checksum is invalid. Calculated: [" << - std::ios::hex << checksum_calc << "] " << - "Received: [" << - std::ios::hex << res.checksum << "]"; + stream << "[ISERVERDATA] Checksum is invalid. Calculated: [" << std::ios::hex << checksum_calc << "] " + << "Received: [" << std::ios::hex << res.checksum << "]"; throw std::invalid_argument(stream.str()); } pullFromStructure(res); } -void IServerData::pullFromStructure(ResponseConfigMessage res) { +void IServerData::pullFromStructure(ResponseConfigMessage res) +{ UVMutex.lock(); UVState.imu.roll = res.roll; @@ -386,7 +410,8 @@ void IServerData::pullFromStructure(ResponseConfigMessage res) { UVMutex.unlock(); } -void IServerData::parseDirectMessage(QByteArray msg) { +void IServerData::parseDirectMessage(QByteArray msg) +{ ResponseDirectMessage res; uint16_t checksum_calc = getCheckSumm16b(msg.data(), msg.size() - 2); @@ -397,29 +422,34 @@ void IServerData::parseDirectMessage(QByteArray msg) { stream >> res.checksum; - if (res.checksum != checksum_calc) { + if (res.checksum != checksum_calc) + { std::stringstream stream; - stream << "[ISERVERDATA] Checksum is invalid. Calculated: [" << - std::ios::hex << checksum_calc << "] " << - "Received: [" << - std::ios::hex << res.checksum << "]"; + stream << "[ISERVERDATA] Checksum is invalid. Calculated: [" << std::ios::hex << checksum_calc << "] " + << "Received: [" << std::ios::hex << res.checksum << "]"; throw std::invalid_argument(stream.str()); } pullFromStructure(res); } -void IServerData::pullFromStructure(ResponseDirectMessage res) { +void IServerData::pullFromStructure(ResponseDirectMessage res) +{ // nothing } /* CRC16-CCITT algorithm */ -uint16_t getCheckSumm16b(char* pcBlock, int len) { +uint16_t getCheckSumm16b(char *pcBlock, int len) +{ uint16_t crc = 0xFFFF; - //int crc_fix = reinterpret_cast(&crc); + if (len <= 0) + return 0; // проверяем длину сообщения + + // int crc_fix = reinterpret_cast(&crc); uint8_t i; - while (len--) { + while (len--) + { crc ^= *pcBlock++ << 8; for (i = 0; i < 8; i++) @@ -428,11 +458,15 @@ uint16_t getCheckSumm16b(char* pcBlock, int len) { return crc; } -void set_bit(uint8_t& byte, uint8_t bit, bool state) { +void set_bit(uint8_t &byte, uint8_t bit, bool state) +{ uint8_t value = 1; - if (state) { + if (state) + { byte = byte | (value << bit); - } else { + } + else + { byte = byte & ~(value << bit); } } diff --git a/UV/ituningdata.cpp b/UV/ituningdata.cpp index edf8c0a..a48fc95 100644 --- a/UV/ituningdata.cpp +++ b/UV/ituningdata.cpp @@ -18,7 +18,7 @@ int ITuningData::getThrusterAmount() { } void ITuningData::setThrusterData(int slot, UV_Thruster data) { - if (slot < UVState.getThrusterAmount()) { + if (slot < UVState.getThrusterAmount() ) { UVMutex.lock(); UVState.thruster[slot] = data; UVMutex.unlock(); diff --git a/mainwindow.cpp b/mainwindow.cpp index b9da99c..c24d103 100644 --- a/mainwindow.cpp +++ b/mainwindow.cpp @@ -1,5 +1,9 @@ #include "mainwindow.h" +bool _boolfix = 0; +double _depsh = 0; +const double _value = 500000; + //double X[2000][2]; MainWindow::MainWindow(QWidget* parent): QMainWindow(parent) { @@ -52,10 +56,16 @@ MainWindow::MainWindow(QWidget* parent): QMainWindow(parent) { serial_client = new SerialClient(); serial_client->start(); + QTimer *update_timer = new QTimer(this); + connect(update_timer, SIGNAL(timeout()), this, SLOT(updateUi())); + update_timer->start(10); + connect(serial_client, SIGNAL(dataUpdatedSerialClient()), this, SLOT(updateUi())); controller = new Joystick(10); + + // const QString ConfigFile = "protocols.conf"; // const QString XI = "xi"; // const QString KI = "ki"; @@ -74,9 +84,7 @@ MainWindow::MainWindow(QWidget* parent): QMainWindow(parent) { // vehiclePic->setRenderHint(QPainter::Antialiasing); // vehiclePic->scene()->addPixmap(QPixmap(":/images/Cousteau_2A.png")); QPixmap pic(":/images/Cousteau3.png"); -label_6->setPixmap(pic.scaled(450, 300)); - - + label_6->setPixmap(pic.scaled(450, 300)); updateUi(); } @@ -104,7 +112,26 @@ void MainWindow::updateUi() { label_TelemetryRoll->setText(QString::number(sensors.roll, 'f', 2)); label_TelemetryPitch->setText(QString::number(sensors.pitch, 'f', 2)); label_TelemetryYaw->setText(QString::number(sensors.yaw, 'f', 2)); - label_TelemetryDepth->setText(QString::number(sensors.depth, 'f', 2)); + //label_TelemetryDepth->setText(QString::number(sensors.depth, 'f', 2)); + label_TelemetryDepth->setText(QString::number(_depsh, 'f', 2)); + + if(_boolfix){ + //ограниечение по возможным значениям + if(_depsh <= 3.7 && _depsh >=0 ){ + _depsh += (double)uv_interface.getControlData().depth/_value; + } + else if(_depsh > 3){ + if(uv_interface.getControlData().depth < 0) _depsh += (double)uv_interface.getControlData().depth/_value; + } + else { + if(uv_interface.getControlData().depth > 0) _depsh += (double)uv_interface.getControlData().depth/_value; + } + + _depsh += (double)QRandomGenerator::global()->bounded(-100, 100)/30000.0; + } + else { //проверка на движение аппарата вперёд + if (uv_interface.getControlData().depth) _boolfix = true; + } // Update drawing of a compass emit updateCompass(sensors.yaw); diff --git a/mainwindow.h b/mainwindow.h index 4a04a45..c6d96bf 100644 --- a/mainwindow.h +++ b/mainwindow.h @@ -15,6 +15,8 @@ #include #include + #include + #include "KX_Pult/kx_protocol.h" #include "KX_Pult/qkx_coeffs.h" @@ -48,6 +50,8 @@ class MainWindow: public QMainWindow, private Ui::MainWindow { SerialClient* serial_client; IUserInterfaceData uv_interface; + + QTimer* update_timer; private slots: // void updateVehiclesMenu(); void stabilizeRollToggled(bool state);