diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 70529131c431c3dd9aa76043f1dbfbb7a2ad1d76..daceae14987ca24e7c3df35c07970d5337a2bbf3 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -18,9 +18,10 @@ TARGET = qgroundcontrol BASEDIR = . BUILDDIR = build LANGUAGE = C++ -CONFIG += debug_and_release \ - #console -OBJECTS_DIR = $$BUILDDIR/obj +CONFIG += debug_and_release \ # console + OBJECTS_DIR \ + = \ + $$BUILDDIR/obj MOC_DIR = $$BUILDDIR/moc UI_HEADERS_DIR = src/ui/generated @@ -161,6 +162,7 @@ HEADERS += src/MG.h \ src/ui/QGCRemoteControlView.h \ src/WaypointGlobal.h \ src/ui/WaypointGlobalView.h \ + src/ui/RadioCalibration/RadioCalibrationData.h \ src/ui/RadioCalibration/RadioCalibrationWindow.h \ src/ui/RadioCalibration/AirfoilServoCalibrator.h \ src/ui/RadioCalibration/SwitchCalibrator.h \ @@ -236,7 +238,8 @@ SOURCES += src/main.cc \ src/ui/RadioCalibration/AirfoilServoCalibrator.cc \ src/ui/RadioCalibration/SwitchCalibrator.cc \ src/ui/RadioCalibration/CurveCalibrator.cc \ - src/ui/RadioCalibration/AbstractCalibrator.cc + src/ui/RadioCalibration/AbstractCalibrator.cc \ + src/ui/RadioCalibration/RadioCalibrationData.cc RESOURCES = mavground.qrc # Include RT-LAB Library diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc index e650f4001fa83e5ed984a3c8646fe5f156c3e3e9..454a3ad7ea77eac53156c684e19c945e1c19ff76 100644 --- a/src/comm/OpalLink.cc +++ b/src/comm/OpalLink.cc @@ -142,6 +142,16 @@ void OpalLink::writeBytes(const char *bytes, qint64 length) this->sendRCValues = static_cast(rc.enabled); } break; +#ifdef MAVLINK_ENABLE_UALBERTA_MESSAGES + case MAVLINK_MSG_ID_RADIO_CALIBRATION: + { + mavlink_radio_calibration_t radio; + mavlink_msg_radio_calibration_decode(&msg, &radio); + qDebug() << "RADIO CALIBRATION RECEIVED"; + qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2]; + } + break; +#endif default: { qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet"; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 08eb8889157b1cf0ef27a4b2c2fe7a45a3d03b2c..5605ba9c1271b60ffb4f03c3b0befeaf0c33292e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -552,6 +552,41 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time); } break; + case MAVLINK_MSG_ID_RADIO_CALIBRATION: + { + mavlink_radio_calibration_t radioMsg; + mavlink_msg_radio_calibration_decode(&message, &radioMsg); + QVector aileron; + QVector elevator; + QVector rudder; + QVector gyro; + QVector pitch; + QVector throttle; + + for (int i=0; i radioData = new RadioCalibrationData(aileron, + elevator, + rudder, + gyro, + pitch, + throttle); + emit radioCalibrationReceived(radioData); + delete radioData; + } + break; + #endif default: { diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index fc5dd526b36c32f4dd85a8bfe388b5bba42f333d..f58d55146f0218d73fcfda58eb0ccc7169952e5e 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -36,10 +36,12 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include "LinkInterface.h" #include "ProtocolInterface.h" #include "UASWaypointManager.h" +#include "RadioCalibration/RadioCalibrationData.h" /** * @brief Interface for all robots. @@ -339,6 +341,8 @@ signals: void remoteControlChannelChanged(int channelId, float raw, float normalized); /** @brief Remote control RSSI changed */ void remoteControlRSSIChanged(float rssi); + /** @brief Radio Calibration Data has been received from the MAV*/ + void radioCalibrationReceived(const QPointer&); /** * @brief Localization quality changed diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc index 8b28f800e7cb7b59d0df92afc65eaf4a1c875f96..52ac01e745a68c90c2107735bad24aba7df60b49 100644 --- a/src/ui/QGCRemoteControlView.cc +++ b/src/ui/QGCRemoteControlView.cc @@ -98,6 +98,8 @@ void QGCRemoteControlView::setUASId(int id) // The UAS exists, disconnect any existing connections disconnect(uas, SIGNAL(remoteControlChannelChanged(int,float,float)), this, SLOT(setChannel(int,float,float))); disconnect(uas, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); + disconnect(uas, SIGNAL(radioCalibrationReceived(const QPointer)), calibrationWindow, SLOT(receive(const QPointer&))); + disconnect(uas, SIGNAL(remoteControlChannelChanged(int,float,float)), calibrationWindow, SLOT(setChannel(int,float,float))); } } @@ -108,6 +110,7 @@ void QGCRemoteControlView::setUASId(int id) // New UAS exists, connect nameLabel->setText(QString("RC Input of %1").arg(newUAS->getUASName())); calibrationWindow->setUASId(id); + connect(newUAS, SIGNAL(radioCalibrationReceived(const QPointer)), calibrationWindow, SLOT(receive(const QPointer&))); connect(newUAS, SIGNAL(remoteControlChannelChanged(int,float,float)), this, SLOT(setChannel(int,float,float))); connect(newUAS, SIGNAL(remoteControlChannelChanged(int,float,float)), calibrationWindow, SLOT(setChannel(int,float,float))); connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); diff --git a/src/ui/RadioCalibration/AbstractCalibrator.h b/src/ui/RadioCalibration/AbstractCalibrator.h index dc0f97f9b608c6e4b3f1d5bbd881c65276f8fa8b..fa56810f7085041a2053d3ef1e4e8503d79d78c0 100644 --- a/src/ui/RadioCalibration/AbstractCalibrator.h +++ b/src/ui/RadioCalibration/AbstractCalibrator.h @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -14,6 +15,8 @@ public: explicit AbstractCalibrator(QWidget *parent = 0); ~AbstractCalibrator(); + virtual void set(const QVector& data)=0; + public slots: void channelChanged(float raw); diff --git a/src/ui/RadioCalibration/AirfoilServoCalibrator.cc b/src/ui/RadioCalibration/AirfoilServoCalibrator.cc index 472aa4101c999b4aa27ac30d3f75b50124ecb083..7b0f9d8abaf9980153fd4bb479a743041a37eb29 100644 --- a/src/ui/RadioCalibration/AirfoilServoCalibrator.cc +++ b/src/ui/RadioCalibration/AirfoilServoCalibrator.cc @@ -105,3 +105,13 @@ void AirfoilServoCalibrator::setLow() lowPulseWidth->setText(QString::number(static_cast(logExtrema()))); emit lowSetpointChanged(logExtrema()); } + +void AirfoilServoCalibrator::set(const QVector &data) +{ + if (data.size() == 3) + { + lowPulseWidth->setText(QString::number(data[0])); + centerPulseWidth->setText(QString::number(data[1])); + highPulseWidth->setText(QString::number(data[2])); + } +} diff --git a/src/ui/RadioCalibration/AirfoilServoCalibrator.h b/src/ui/RadioCalibration/AirfoilServoCalibrator.h index 234a13dc993f1773f664c5e95990abfcfe8147c7..34a2e9b10aa89775d507042e870854ce793da3d8 100644 --- a/src/ui/RadioCalibration/AirfoilServoCalibrator.h +++ b/src/ui/RadioCalibration/AirfoilServoCalibrator.h @@ -23,6 +23,9 @@ public: explicit AirfoilServoCalibrator(AirfoilType type = AILERON, QWidget *parent = 0); + /** @param data must have exaclty 3 elemets. they are assumed to be low center high */ + void set(const QVector& data); + signals: void highSetpointChanged(float); void centerSetpointChanged(float); @@ -38,9 +41,9 @@ protected: QLabel *centerPulseWidth; QLabel *lowPulseWidth; - float high; - float center; - float low; +// float high; +// float center; +// float low; }; #endif // AIRFOILSERVOCALIBRATOR_H diff --git a/src/ui/RadioCalibration/CurveCalibrator.cc b/src/ui/RadioCalibration/CurveCalibrator.cc index e27a7dde0175a1656f568190f5926c23e0178241..e0458bdaa411cddbadcaae5ea2fef9b46e063915 100644 --- a/src/ui/RadioCalibration/CurveCalibrator.cc +++ b/src/ui/RadioCalibration/CurveCalibrator.cc @@ -87,3 +87,17 @@ void CurveCalibrator::setSetpoint(int setpoint) emit setpointChanged(setpoint, static_cast(setpoints->value(setpoint))); } + +void CurveCalibrator::set(const QVector &data) +{ + if (data.size() == 5) + { + delete setpoints; + QVector dataDouble; + for (int i=0; i(data[i]); + setpoints = new QVector(dataDouble); + curve->setData(*positions, *setpoints); + plot->replot(); + } +} diff --git a/src/ui/RadioCalibration/CurveCalibrator.h b/src/ui/RadioCalibration/CurveCalibrator.h index d6ccc5933f739f8d26e26ccbfb9c9502094056dc..4dd1ddac661df39c8ccd4ac7e11b513fc240d995 100644 --- a/src/ui/RadioCalibration/CurveCalibrator.h +++ b/src/ui/RadioCalibration/CurveCalibrator.h @@ -23,6 +23,8 @@ Q_OBJECT public: explicit CurveCalibrator(QString title = QString(), QWidget *parent = 0); ~CurveCalibrator(); + + void set(const QVector &data); signals: void setpointChanged(int setpoint, float raw); diff --git a/src/ui/RadioCalibration/RadioCalibrationData.cc b/src/ui/RadioCalibration/RadioCalibrationData.cc new file mode 100644 index 0000000000000000000000000000000000000000..c7139b37546648e23c13a412d22a49858906b214 --- /dev/null +++ b/src/ui/RadioCalibration/RadioCalibrationData.cc @@ -0,0 +1,49 @@ +#include "RadioCalibrationData.h" + +RadioCalibrationData::RadioCalibrationData() +{ + data = new QVector >(); +} + +RadioCalibrationData::RadioCalibrationData(const QVector &aileron, + const QVector &elevator, + const QVector &rudder, + const QVector &gyro, + const QVector &pitch, + const QVector &throttle) +{ + data = new QVector >(); + (*data) << aileron + << elevator + << rudder + << gyro + << pitch + << throttle; +} + +RadioCalibrationData::RadioCalibrationData(const RadioCalibrationData &other) + :QObject() +{ + data = new QVector >(*other.data); +} + +const float* RadioCalibrationData::operator [](int i) const +{ + if (i < data->size()) + { + return (*data)[i].constData(); + } + + return NULL; +} + +const QVector& RadioCalibrationData::operator ()(int i) const +{ + if (i < data->size()) + { + return (*data)[i]; + } + + // This is not good. If it is ever used after being returned it will cause a crash + return QVector(); +} diff --git a/src/ui/RadioCalibration/RadioCalibrationData.h b/src/ui/RadioCalibration/RadioCalibrationData.h new file mode 100644 index 0000000000000000000000000000000000000000..726d01f2615e3c68673770d184aa6d2792d6e72a --- /dev/null +++ b/src/ui/RadioCalibration/RadioCalibrationData.h @@ -0,0 +1,53 @@ +#ifndef RADIOCALIBRATIONDATA_H +#define RADIOCALIBRATIONDATA_H + +#include +#include +#include + + +class RadioCalibrationData : public QObject +{ +Q_OBJECT + +public: + explicit RadioCalibrationData(); + RadioCalibrationData(const RadioCalibrationData&); + RadioCalibrationData(const QVector& aileron, + const QVector& elevator, + const QVector& rudder, + const QVector& gyro, + const QVector& pitch, + const QVector& throttle); + + enum RadioElement + { + AILERON=0, + ELEVATOR, + RUDDER, + GYRO, + PITCH, + THROTTLE + }; + + void loadFile(); + void saveFile(); + void send(); + void receive(); + const float* operator[](int i) const; + const QVector& operator()(int i) const; + +protected: + QVector > *data; + + + void init(const QVector& aileron, + const QVector& elevator, + const QVector& rudder, + const QVector& gyro, + const QVector& pitch, + const QVector& throttle); + +}; + +#endif // RADIOCALIBRATIONDATA_H diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.cc b/src/ui/RadioCalibration/RadioCalibrationWindow.cc index 3d98eb90d9d0a625da3c3359f365947821b06a2e..3efc756b72da0630540ff00f0d450835afe9539d 100644 --- a/src/ui/RadioCalibration/RadioCalibrationWindow.cc +++ b/src/ui/RadioCalibration/RadioCalibrationWindow.cc @@ -41,7 +41,8 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) : connect(load, SIGNAL(clicked()), this, SLOT(loadFile())); connect(save, SIGNAL(clicked()), this, SLOT(saveFile())); connect(transmit, SIGNAL(clicked()), this, SLOT(send())); - connect(get, SIGNAL(clicked()), this, SLOT(receive())); + connect(get, SIGNAL(clicked()), this, SLOT(request())); + setUASId(0); } @@ -76,53 +77,18 @@ void RadioCalibrationWindow::setChannel(int ch, float raw, float normalized) } } - -/* - ** RadioCalibrationData Function Definitions ** -*/ - -RadioCalibrationWindow::RadioCalibrationData::RadioCalibrationData(RadioCalibrationWindow *parent) - :parent(parent) -{ - data = new QVector >(); -} - -RadioCalibrationWindow::RadioCalibrationData::RadioCalibrationData(const QVector &aileron, - const QVector &elevator, - const QVector &rudder, - const QVector &gyro, - const QVector &pitch, - const QVector &throttle, - RadioCalibrationWindow *parent) - :parent(parent) -{ - data = new QVector >(); - (*data) << aileron - << elevator - << rudder - << gyro - << pitch - << throttle; -} - -RadioCalibrationWindow::RadioCalibrationData::RadioCalibrationData(RadioCalibrationData &other) - :parent(other.parent) -{ - data = new QVector >(*other.data); -} - -void RadioCalibrationWindow::RadioCalibrationData::saveFile() +void RadioCalibrationWindow::saveFile() { qDebug() << __FILE__ << __LINE__ << "SAVE TO FILE"; } -void RadioCalibrationWindow::RadioCalibrationData::loadFile() +void RadioCalibrationWindow::loadFile() { qDebug() << __FILE__ << __LINE__ << "LOAD FROM FILE"; } -void RadioCalibrationWindow::RadioCalibrationData::send() -{ +void RadioCalibrationWindow::send() +{ qDebug() << __FILE__ << __LINE__ << "uasId = " << uasId; #ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId)); @@ -131,24 +97,42 @@ void RadioCalibrationWindow::RadioCalibrationData::send() qDebug()<< "we have a uas"; mavlink_message_t msg; mavlink_msg_radio_calibration_pack(uasId, 0, &msg, - (*data)[AILERON].constData(), - (*data)[ELEVATOR].constData(), - (*data)[RUDDER].constData(), - (*data)[GYRO].constData(), - (*data)[PITCH].constData(), - (*data)[THROTTLE].constData()); + (*radio)[RadioCalibrationData::AILERON], + (*radio)[RadioCalibrationData::ELEVATOR], + (*radio)[RadioCalibrationData::RUDDER], + (*radio)[RadioCalibrationData::GYRO], + (*radio)[RadioCalibrationData::PITCH], + (*radio)[RadioCalibrationData::THROTTLE]); uas->sendMessage(msg); - } #endif } -void RadioCalibrationWindow::RadioCalibrationData::receive() +void RadioCalibrationWindow::request() { qDebug() << __FILE__ << __LINE__ << "READ FROM UAV"; + UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId)); + if (uas) + { + mavlink_message_t msg; + mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC); + uas->sendMessage(msg); + } } -void RadioCalibrationWindow::RadioCalibrationData::setUASId(int id) +void RadioCalibrationWindow::receive(const QPointer& radio) { - this->uasId = id; + if (radio) + { + if (this->radio) + delete this->radio; + this->radio = new RadioCalibrationData(*radio); + + aileron->set((*radio)(RadioCalibrationData::AILERON)); + elevator->set((*radio)(RadioCalibrationData::ELEVATOR)); + rudder->set((*radio)(RadioCalibrationData::RUDDER)); + gyro->set((*radio)(RadioCalibrationData::GYRO)); + pitch->set((*radio)(RadioCalibrationData::PITCH)); + throttle->set((*radio)(RadioCalibrationData::THROTTLE)); + } } diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.h b/src/ui/RadioCalibration/RadioCalibrationWindow.h index 983259968704031979f44ab1af629c541a40d9a7..af98e987f292eb3677d25b4972ba4fa998ea45fa 100644 --- a/src/ui/RadioCalibration/RadioCalibrationWindow.h +++ b/src/ui/RadioCalibration/RadioCalibrationWindow.h @@ -9,15 +9,18 @@ #include #include #include +#include #include "AirfoilServoCalibrator.h" #include "SwitchCalibrator.h" #include "CurveCalibrator.h" -#include "UASManager.h" -#include "UASInterface.h" -#include "UAS.h" #include "mavlink.h" +#include "mavlink_types.h" +#include "UAS.h" +#include "UASManager.h" +#include "RadioCalibrationData.h" + class RadioCalibrationWindow : public QWidget { @@ -30,11 +33,12 @@ signals: public slots: void setChannel(int ch, float raw, float normalized); - void loadFile() {this->radio->loadFile();} - void saveFile() {this->radio->saveFile();} - void send() {this->radio->send();} - void receive() {this->radio->receive();} - void setUASId(int id) {this->radio->setUASId(id);} + void loadFile(); + void saveFile(); + void send(); + void request(); + void receive(const QPointer& radio); + void setUASId(int id) {this->uasId = id;} protected: @@ -44,54 +48,8 @@ protected: SwitchCalibrator *gyro; CurveCalibrator *pitch; CurveCalibrator *throttle; - - class RadioCalibrationData - { - public: - explicit RadioCalibrationData(RadioCalibrationWindow *parent=0); - RadioCalibrationData(RadioCalibrationData&); - RadioCalibrationData(const QVector& aileron, - const QVector& elevator, - const QVector& rudder, - const QVector& gyro, - const QVector& pitch, - const QVector& throttle, - RadioCalibrationWindow *parent=0); - - enum RadioElement - { - AILERON=0, - ELEVATOR, - RUDDER, - GYRO, - PITCH, - THROTTLE - }; - - void loadFile(); - void saveFile(); - void send(); - void receive(); - void setUASId(int id); - - protected: - QVector > *data; - int uasId; - - void init(const QVector& aileron, - const QVector& elevator, - const QVector& rudder, - const QVector& gyro, - const QVector& pitch, - const QVector& throttle); - - private: - RadioCalibrationWindow *parent; - - - }; - - RadioCalibrationData *radio; + int uasId; + QPointer radio; }; #endif // RADIOCALIBRATIONWINDOW_H diff --git a/src/ui/RadioCalibration/SwitchCalibrator.cc b/src/ui/RadioCalibration/SwitchCalibrator.cc index 2062d985219920cd0d9a8a35ada5aa420e3b8833..d09fee3f38bc3c0e6e9a1b18ab8a43c58b1bf10b 100644 --- a/src/ui/RadioCalibration/SwitchCalibrator.cc +++ b/src/ui/RadioCalibration/SwitchCalibrator.cc @@ -47,3 +47,12 @@ void SwitchCalibrator::setToggled() toggledPulseWidth->setText(QString::number(static_cast(logExtrema()))); emit toggledSetpointChanged(logExtrema()); } + +void SwitchCalibrator::set(const QVector &data) +{ + if (data.size() == 2) + { + defaultPulseWidth->setText(QString::number(data[0])); + toggledPulseWidth->setText(QString::number(data[1])); + } +} diff --git a/src/ui/RadioCalibration/SwitchCalibrator.h b/src/ui/RadioCalibration/SwitchCalibrator.h index 430c1d121a588a7996ffb5cec646885a381d1e3c..6950f03a4acd8efe5c7b809f31c0ed051380911a 100644 --- a/src/ui/RadioCalibration/SwitchCalibrator.h +++ b/src/ui/RadioCalibration/SwitchCalibrator.h @@ -16,6 +16,7 @@ Q_OBJECT public: explicit SwitchCalibrator(QString title=QString(), QWidget *parent = 0); + void set(const QVector &data); signals: void defaultSetpointChanged(float); void toggledSetpointChanged(float);