diff --git a/media/face1.png b/media/face1.png deleted file mode 100644 index 7c9c97d82774f7084839da868af8c8b6d152fefa..0000000000000000000000000000000000000000 Binary files a/media/face1.png and /dev/null differ diff --git a/media/face2.png b/media/face2.png deleted file mode 100644 index 63a87d5cfee4d00e8e3ba5c1a6f2a95bf34d3515..0000000000000000000000000000000000000000 Binary files a/media/face2.png and /dev/null differ diff --git a/media/face3.png b/media/face3.png deleted file mode 100644 index 1c61d580271d2553ef5f17da08fd5963d601e50f..0000000000000000000000000000000000000000 Binary files a/media/face3.png and /dev/null differ diff --git a/media/face4.png b/media/face4.png deleted file mode 100644 index b18858f7c3635f4cccd20afd7b38ca346327bcc9..0000000000000000000000000000000000000000 Binary files a/media/face4.png and /dev/null differ diff --git a/media/face5.png b/media/face5.png deleted file mode 100644 index b42231e5da8b46f492e58f22f24fe2e96ca55bd7..0000000000000000000000000000000000000000 Binary files a/media/face5.png and /dev/null differ diff --git a/media/letterB.png b/media/letterB.png deleted file mode 100644 index de8dec9cf8d4b7ff09031ab77b19a5d637b65549..0000000000000000000000000000000000000000 Binary files a/media/letterB.png and /dev/null differ diff --git a/media/letterD.png b/media/letterD.png deleted file mode 100644 index f96b3e8fe696a9e3164923779125d37101ca57bf..0000000000000000000000000000000000000000 Binary files a/media/letterD.png and /dev/null differ diff --git a/media/letterP.png b/media/letterP.png deleted file mode 100644 index 4ee1a56ad59ebb9de8cc7ef7429d867858ce7419..0000000000000000000000000000000000000000 Binary files a/media/letterP.png and /dev/null differ diff --git a/media/letterR.png b/media/letterR.png deleted file mode 100644 index 2329f1464f23bd6ecd2d3b39f73c6392266c53ba..0000000000000000000000000000000000000000 Binary files a/media/letterR.png and /dev/null differ diff --git a/media/letterS.png b/media/letterS.png deleted file mode 100644 index 4cb14b1b69909e6d943e471d3a0c6ae4086706b0..0000000000000000000000000000000000000000 Binary files a/media/letterS.png and /dev/null differ diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 71348cee4c5a553bd59b5376bcc3892ae80ec648..539b6e6cfae5347ef121721564cb0ef1d0e2a225 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -86,6 +86,19 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) positionLock = true; } break; + case MAVLINK_MSG_ID_AUX_STATUS: + { + mavlink_aux_status_t status; + mavlink_msg_aux_status_decode(&message, &status); + emit loadChanged(this, status.load/10.0f); + emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count); + emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count); + emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count); + emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count); + emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count); + emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); + } + break; default: // Do nothing break; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 983e24023faf40f11365b6d0a871a8e3315142e3..1417c97df2e7f303fe02aab1fdf54687b25b3505 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -250,19 +250,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } } break; - case MAVLINK_MSG_ID_AUX_STATUS: - { - mavlink_aux_status_t status; - mavlink_msg_aux_status_decode(&message, &status); - emit loadChanged(this, status.load/10.0f); - emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count); - emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count); - emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count); - emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count); - emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count); - emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); - } - break; case MAVLINK_MSG_ID_RAW_IMU: { mavlink_raw_imu_t raw; @@ -351,13 +338,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "lat", pos.lat, time); emit valueChanged(uasId, "lon", pos.lon, time); emit valueChanged(uasId, "alt", pos.alt, time); - emit valueChanged(uasId, "g-vx", pos.vx, time); - emit valueChanged(uasId, "g-vy", pos.vy, time); - emit valueChanged(uasId, "g-vz", pos.vz, time); + emit valueChanged(uasId, "speed", pos.v, time); qDebug() << "GOT GPS RAW"; emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); } break; + case MAVLINK_MSG_ID_GPS_STATUS: + { + mavlink_gps_status_t pos; + mavlink_msg_gps_status_decode(&message, &pos); + for(int i = 0; i < pos.satellites_visible && i < sizeof(pos.satellite_used); i++) + { + emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast(pos.satellite_used[i])); + } + } + break; case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t value; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 5599f0b6e9267017955e89aadb995692eba0f143..4fe7572f1d86237fb314efc4022ca2c6232e8cd9 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -304,6 +304,8 @@ signals: void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec); + /** @brief Update the status of one satellite used for localization */ + void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used); void speedChanged(UASInterface*, double x, double y, double z, quint64 usec); void imageStarted(int imgid, int width, int height, int depth, int channels); void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 88af81f977525a118ec51ab5ca57b214696aede2..eb4ba2d1c1f998e4fe67ba36c93bc65493e28652 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -39,11 +39,22 @@ This file is part of the PIXHAWK project #include HSIDisplay::HSIDisplay(QWidget *parent) : - HDDisplay(NULL, parent) + HDDisplay(NULL, parent), + gpsSatellites() { } +void HSIDisplay::paintEvent(QPaintEvent * event) +{ + Q_UNUSED(event); + //paintGL(); + static quint64 interval = 0; + qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; + interval = MG::TIME::getGroundTimeNow(); + paintDisplay(); +} + void HSIDisplay::paintDisplay() { quint64 refreshInterval = 100; @@ -69,7 +80,20 @@ void HSIDisplay::paintDisplay() const int columns = 3; const float spacing = 0.4f; // 40% of width const float gaugeWidth = vwidth / (((float)columns) + (((float)columns+1) * spacing + spacing * 0.1f)); - const QColor ringColor = QColor(200, 200, 200); + const QColor ringColor = QColor(200, 250, 200); + + const int ringCount = 2; + const float margin = 0.1f; // 10% margin of total width on each side + + for (int i = 0; i < ringCount; i++) + { + float radius = (vwidth - vwidth * 2.0f * margin) / (2.0f * i+1) / 2.0f; + drawCircle(vwidth/2.0f, vheight/2.0f, radius, 0.1f, ringColor, &painter); + } + + drawGPS(); + + //drawSystemIndicator(10.0f-gaugeWidth/2.0f, 20.0f, 10.0f, 40.0f, 15.0f, &painter); //drawGauge(15.0f, 15.0f, gaugeWidth/2.0f, 0, 1.0f, "thrust", values.value("thrust", 0.0f), gaugeColor, &painter, qMakePair(0.45f, 0.8f), qMakePair(0.8f, 1.0f), true); //drawGauge(15.0f+gaugeWidth*1.7f, 15.0f, gaugeWidth/2.0f, 0, 10.0f, "altitude", values.value("altitude", 0.0f), gaugeColor, &painter, qMakePair(1.0f, 2.5f), qMakePair(0.0f, 0.5f), true); @@ -119,9 +143,64 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) //} } +void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used) +{ + Q_UNUSED(uasid); + // If slot is empty, insert object + if (gpsSatellites.at(satid) == NULL) + { + gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used)); + } + else + { + // Satellite exists, update it + gpsSatellites.at(satid)->update(satid, azimuth, direction, snr, used); + } +} + +QColor HSIDisplay::getColorForSNR(float snr) +{ + return QColor(200, 250, 200); +} + void HSIDisplay::drawGPS() { - + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::HighQualityAntialiasing, true); + + // Max satellite circle radius + + const float margin = 0.2f; // 20% margin of total width on each side + float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f; + + for (int i = 0; i < gpsSatellites.size(); i++) + { + GPSSatellite* sat = gpsSatellites.at(i); + if (sat) + { + // Draw satellite + QBrush brush; + QColor color = getColorForSNR(sat->snr); + brush.setColor(color); + if (sat->used) + { + brush.setStyle(Qt::SolidPattern); + } + else + { + brush.setStyle(Qt::NoBrush); + } + painter.setPen(Qt::SolidLine); + painter.setPen(color); + painter.setBrush(brush); + + float xPos = sin(sat->direction) * sat->azimuth * radius; + float yPos = cos(sat->direction) * sat->azimuth * radius; + + drawCircle(xPos, yPos, vwidth/10.0f, 1.0f, color, &painter); + } + } } void HSIDisplay::drawObjects() diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 69c257a632b86ec8b1c09c883d512aa01ea18309..838013daed55df642414caaf8c3327fec81f10df 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -45,19 +45,58 @@ class HSIDisplay : public HDDisplay { Q_OBJECT public: HSIDisplay(QWidget *parent = 0); - // ~HSIDisplay(); + // ~HSIDisplay(); public slots: void setActiveUAS(UASInterface* uas); + void paintEvent(QPaintEvent * event); protected slots: void paintDisplay(); - void drawGPS(); - void drawObjects(); - void drawBaseLines(float xRef, float yRef, float radius, float yaw, const QColor& color, QPainter* painter, bool solid); - + void drawGPS(); + void drawObjects(); + void drawBaseLines(float xRef, float yRef, float radius, float yaw, const QColor& color, QPainter* painter, bool solid); + protected: + void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used); + static QColor getColorForSNR(float snr); + + /** + * @brief Private data container class to be used within the HSI widget + */ + class GPSSatellite + { + public: + GPSSatellite(int id, float azimuth, float direction, float snr, bool used) : + id(id), + azimuth(azimuth), + direction(direction), + snr(snr), + used(used) + { + + } + + void update(int id, float azimuth, float direction, float snr, bool used) + { + this->id = id; + this->azimuth = azimuth; + this->direction = direction; + this->snr = snr; + this->used = used; + } + + int id; + float azimuth; + float direction; + float snr; + bool used; + + friend class HSIDisplay; + }; + + QVector gpsSatellites; private: }; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 11d49940fec25ced58a692d5d6261dbdb07a33c1..fcfab3cf514f1334224be302697f5db1eabbe21f 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -356,6 +356,7 @@ void MainWindow::clearView() linechart->setActive(false); headDown1->stop(); headDown2->stop(); + hsi->stop(); // Remove all dock widgets QList list = this->children(); @@ -426,9 +427,10 @@ void MainWindow::loadOperatorView() container5->setWidget(waypoints); addDockWidget(Qt::BottomDockWidgetArea, container5); - // DEBUG CONSOLE - QDockWidget* container7 = new QDockWidget(tr("Communication Console"), this); - container7->setWidget(debugConsole); + // HORIZONTAL SITUATION INDICATOR + QDockWidget* container7 = new QDockWidget(tr("Horizontal Situation Indicator"), this); + container7->setWidget(hsi); + hsi->start(); addDockWidget(Qt::BottomDockWidgetArea, container7); // OBJECT DETECTION diff --git a/test/generated/mavlink_message_attitude.h b/test/generated/mavlink_message_attitude.h deleted file mode 100644 index 5070d762af9b8f701ddbd8dad9b22479f299b5da..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_attitude.h +++ /dev/null @@ -1,56 +0,0 @@ -// MESSAGE ATTITUDE PACKING - -#define MESSAGE_ID_ATTITUDE 90 - -/** - * @brief Send a attitude message - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_attitude_pack(uint8_t system_id, CommMessage_t* msg, float roll, float pitch, float yaw) -{ - msg->msgid = MESSAGE_ID_ATTITUDE; - uint16_t i = 0; - - i += put_float_by_index(roll, i, msg->payload); //Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); //Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); //Yaw angle (rad) - - return finalize_message(msg, system_id, i); -} - -// MESSAGE ATTITUDE UNPACKING - -/** - * @brief Get field roll from attitude message - * - * @return Roll angle (rad) - */ -static inline float message_attitude_get_roll(CommMessage_t* msg) -{ - return *((float*) (void*)msg->payload); -} - -/** - * @brief Get field pitch from attitude message - * - * @return Pitch angle (rad) - */ -static inline float message_attitude_get_pitch(CommMessage_t* msg) -{ - return *((float*) (void*)msg->payload+sizeof(float)); -} - -/** - * @brief Get field yaw from attitude message - * - * @return Yaw angle (rad) - */ -static inline float message_attitude_get_yaw(CommMessage_t* msg) -{ - return *((float*) (void*)msg->payload+sizeof(float)+sizeof(float)); -} - diff --git a/test/generated/mavlink_message_attitude2.h b/test/generated/mavlink_message_attitude2.h deleted file mode 100644 index 2e2a9c48b6fc1a4af6c8db5390e43a81c17103d5..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_attitude2.h +++ /dev/null @@ -1,89 +0,0 @@ -// MESSAGE ATTITUDE2 PACKING - -#define MESSAGE_ID_ATTITUDE2 90 - -typedef struct __attitude2_t -{ - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - -} attitude2_t; - -/** - * @brief Send a attitude2 message - * - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_attitude2_pack(uint8_t system_id, CommMessage_t* msg, float roll, float pitch, float yaw) -{ - msg->msgid = MESSAGE_ID_ATTITUDE2; - uint16_t i = 0; - - i += put_float_by_index(roll, i, msg->payload); //Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); //Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); //Yaw angle (rad) - - return finalize_message(msg, system_id, i); -} - -static inline uint16_t message_attitude2_encode(uint8_t system_id, CommMessage_t* msg, const attitude2_t* attitude2) -{ - message_attitude2_pack(system_id, msg, attitude2->roll, attitude2->pitch, attitude2->yaw); -} -// MESSAGE ATTITUDE2 UNPACKING - -/** - * @brief Get field roll from attitude2 message - * - * @return Roll angle (rad) - */ -static inline float message_attitude2_get_roll(CommMessage_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; -} - -/** - * @brief Get field pitch from attitude2 message - * - * @return Pitch angle (rad) - */ -static inline float message_attitude2_get_pitch(CommMessage_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; -} - -/** - * @brief Get field yaw from attitude2 message - * - * @return Yaw angle (rad) - */ -static inline float message_attitude2_get_yaw(CommMessage_t* msg) -{ - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; -} - -static inline void message_attitude2_decode(CommMessage_t* msg, attitude2_t* attitude2) -{ - attitude2->roll = message_attitude2_get_roll(msg); - attitude2->pitch = message_attitude2_get_pitch(msg); - attitude2->yaw = message_attitude2_get_yaw(msg); -} diff --git a/test/generated/mavlink_message_boot.h b/test/generated/mavlink_message_boot.h deleted file mode 100644 index df60f49ebb043850fd4db9cb8626d6455b596aba..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_boot.h +++ /dev/null @@ -1,32 +0,0 @@ -// MESSAGE BOOT PACKING - -#define MESSAGE_ID_BOOT 1 - -/** - * @brief Send a boot message - * - * @param version The onboard software version - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_boot_pack(uint8_t system_id, CommMessage_t* msg, uint32 version) -{ - msg->msgid = MESSAGE_ID_BOOT; - uint16_t i = 0; - - i += put_uint32_by_index(version, i, msg->payload); //The onboard software version - - return finalize_message(msg, system_id, i); -} - -// MESSAGE BOOT UNPACKING - -/** - * @brief Get field version from boot message - * - * @return The onboard software version - */ -static inline uint32 message_boot_get_version(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload); -} - diff --git a/test/generated/mavlink_message_boot2.h b/test/generated/mavlink_message_boot2.h deleted file mode 100644 index 06e3f3b65d4b15bd71ab5523beeef2e6da729a2e..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_boot2.h +++ /dev/null @@ -1,46 +0,0 @@ -// MESSAGE BOOT2 PACKING - -#define MESSAGE_ID_BOOT2 1 - -typedef struct __boot2_t -{ - uint32 version; ///< The onboard software version - -} boot2_t; - -/** - * @brief Send a boot2 message - * - * @param version The onboard software version - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_boot2_pack(uint8_t system_id, CommMessage_t* msg, uint32 version) -{ - msg->msgid = MESSAGE_ID_BOOT2; - uint16_t i = 0; - - i += put_uint32_by_index(version, i, msg->payload); //The onboard software version - - return finalize_message(msg, system_id, i); -} - -static inline uint16_t message_boot2_encode(uint8_t system_id, CommMessage_t* msg, const boot2_t* boot2) -{ - message_boot2_pack(system_id, msg, boot2->version); -} -// MESSAGE BOOT2 UNPACKING - -/** - * @brief Get field version from boot2 message - * - * @return The onboard software version - */ -static inline uint32 message_boot2_get_version(CommMessage_t* msg) -{ - -} - -static inline void message_boot2_decode(CommMessage_t* msg, boot2_t* boot2) -{ - boot2->version = message_boot2_get_version(msg); -} diff --git a/test/generated/mavlink_message_heartbeat.h b/test/generated/mavlink_message_heartbeat.h deleted file mode 100644 index 5a2bf611fdba9ffe567e46aadf0464592414fc27..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_heartbeat.h +++ /dev/null @@ -1,32 +0,0 @@ -// MESSAGE HEARTBEAT PACKING - -#define MESSAGE_ID_HEARTBEAT 0 - -/** - * @brief Send a heartbeat message - * - * @param type Type of the MAV (quadrotor, helicopter, etc.) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_heartbeat_pack(uint8_t system_id, CommMessage_t* msg, uint8 type) -{ - msg->msgid = MESSAGE_ID_HEARTBEAT; - uint16_t i = 0; - - i += put_uint8_by_index(type, i, msg->payload); //Type of the MAV (quadrotor, helicopter, etc.) - - return finalize_message(msg, system_id, i); -} - -// MESSAGE HEARTBEAT UNPACKING - -/** - * @brief Get field type from heartbeat message - * - * @return Type of the MAV (quadrotor, helicopter, etc.) - */ -static inline uint8 message_heartbeat_get_type(CommMessage_t* msg) -{ - return *((uint8*) (void*)msg->payload); -} - diff --git a/test/generated/mavlink_message_heartbeat2.h b/test/generated/mavlink_message_heartbeat2.h deleted file mode 100644 index 70233dad9eee76ccfdd047aa9c5635d22b1344a2..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_heartbeat2.h +++ /dev/null @@ -1,46 +0,0 @@ -// MESSAGE HEARTBEAT2 PACKING - -#define MESSAGE_ID_HEARTBEAT2 0 - -typedef struct __heartbeat2_t -{ - uint8 type; ///< Type of the MAV (quadrotor, helicopter, etc.) - -} heartbeat2_t; - -/** - * @brief Send a heartbeat2 message - * - * @param type Type of the MAV (quadrotor, helicopter, etc.) - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_heartbeat2_pack(uint8_t system_id, CommMessage_t* msg, uint8 type) -{ - msg->msgid = MESSAGE_ID_HEARTBEAT2; - uint16_t i = 0; - - i += put_uint8_by_index(type, i, msg->payload); //Type of the MAV (quadrotor, helicopter, etc.) - - return finalize_message(msg, system_id, i); -} - -static inline uint16_t message_heartbeat2_encode(uint8_t system_id, CommMessage_t* msg, const heartbeat2_t* heartbeat2) -{ - message_heartbeat2_pack(system_id, msg, heartbeat2->type); -} -// MESSAGE HEARTBEAT2 UNPACKING - -/** - * @brief Get field type from heartbeat2 message - * - * @return Type of the MAV (quadrotor, helicopter, etc.) - */ -static inline uint8 message_heartbeat2_get_type(CommMessage_t* msg) -{ - -} - -static inline void message_heartbeat2_decode(CommMessage_t* msg, heartbeat2_t* heartbeat2) -{ - heartbeat2->type = message_heartbeat2_get_type(msg); -} diff --git a/test/generated/mavlink_message_sensraw.h b/test/generated/mavlink_message_sensraw.h deleted file mode 100644 index 814d8a1438f129c30d078639b0c04eb83722ed0e..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_sensraw.h +++ /dev/null @@ -1,164 +0,0 @@ -// MESSAGE SENSRAW PACKING - -#define MESSAGE_ID_SENSRAW 100 - -/** - * @brief Send a sensraw message - * - * @param xacc X acceleration (mg raw) - * @param yacc Y acceleration (mg raw) - * @param zacc Z acceleration (mg raw) - * @param xgyro Angular speed around X axis (adc units) - * @param ygyro Angular speed around Y axis (adc units) - * @param zgyro Angular speed around Z axis (adc units) - * @param xmag - * @param ymag - * @param zmag - * @param baro - * @param gdist - * @param temp - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_sensraw_pack(uint8_t system_id, CommMessage_t* msg, uint32 xacc, uint32 yacc, uint32 zacc, uint32 xgyro, uint32 ygyro, uint32 zgyro, uint32 xmag, uint32 ymag, uint32 zmag, int32 baro, uint32 gdist, int32 temp) -{ - msg->msgid = MESSAGE_ID_SENSRAW; - uint16_t i = 0; - - i += put_uint32_by_index(xacc, i, msg->payload); //X acceleration (mg raw) - i += put_uint32_by_index(yacc, i, msg->payload); //Y acceleration (mg raw) - i += put_uint32_by_index(zacc, i, msg->payload); //Z acceleration (mg raw) - i += put_uint32_by_index(xgyro, i, msg->payload); //Angular speed around X axis (adc units) - i += put_uint32_by_index(ygyro, i, msg->payload); //Angular speed around Y axis (adc units) - i += put_uint32_by_index(zgyro, i, msg->payload); //Angular speed around Z axis (adc units) - i += put_uint32_by_index(xmag, i, msg->payload); // - i += put_uint32_by_index(ymag, i, msg->payload); // - i += put_uint32_by_index(zmag, i, msg->payload); // - i += put_int32_by_index(baro, i, msg->payload); // - i += put_uint32_by_index(gdist, i, msg->payload); // - i += put_int32_by_index(temp, i, msg->payload); // - - return finalize_message(msg, system_id, i); -} - -// MESSAGE SENSRAW UNPACKING - -/** - * @brief Get field xacc from sensraw message - * - * @return X acceleration (mg raw) - */ -static inline uint32 message_sensraw_get_xacc(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload); -} - -/** - * @brief Get field yacc from sensraw message - * - * @return Y acceleration (mg raw) - */ -static inline uint32 message_sensraw_get_yacc(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)); -} - -/** - * @brief Get field zacc from sensraw message - * - * @return Z acceleration (mg raw) - */ -static inline uint32 message_sensraw_get_zacc(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field xgyro from sensraw message - * - * @return Angular speed around X axis (adc units) - */ -static inline uint32 message_sensraw_get_xgyro(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field ygyro from sensraw message - * - * @return Angular speed around Y axis (adc units) - */ -static inline uint32 message_sensraw_get_ygyro(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field zgyro from sensraw message - * - * @return Angular speed around Z axis (adc units) - */ -static inline uint32 message_sensraw_get_zgyro(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field xmag from sensraw message - * - * @return - */ -static inline uint32 message_sensraw_get_xmag(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field ymag from sensraw message - * - * @return - */ -static inline uint32 message_sensraw_get_ymag(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field zmag from sensraw message - * - * @return - */ -static inline uint32 message_sensraw_get_zmag(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field baro from sensraw message - * - * @return - */ -static inline int32 message_sensraw_get_baro(CommMessage_t* msg) -{ - return *((int32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)); -} - -/** - * @brief Get field gdist from sensraw message - * - * @return - */ -static inline uint32 message_sensraw_get_gdist(CommMessage_t* msg) -{ - return *((uint32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(int32)); -} - -/** - * @brief Get field temp from sensraw message - * - * @return - */ -static inline int32 message_sensraw_get_temp(CommMessage_t* msg) -{ - return *((int32*) (void*)msg->payload+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(uint32)+sizeof(int32)+sizeof(uint32)); -} - diff --git a/test/generated/mavlink_message_sensraw2.h b/test/generated/mavlink_message_sensraw2.h deleted file mode 100644 index d130047cc9a1a6093c957a7331ac4cd5b031177e..0000000000000000000000000000000000000000 --- a/test/generated/mavlink_message_sensraw2.h +++ /dev/null @@ -1,200 +0,0 @@ -// MESSAGE SENSRAW2 PACKING - -#define MESSAGE_ID_SENSRAW2 100 - -typedef struct __sensraw2_t -{ - uint32 xacc; ///< X acceleration (mg raw) - uint32 yacc; ///< Y acceleration (mg raw) - uint32 zacc; ///< Z acceleration (mg raw) - uint32 xgyro; ///< Angular speed around X axis (adc units) - uint32 ygyro; ///< Angular speed around Y axis (adc units) - uint32 zgyro; ///< Angular speed around Z axis (adc units) - uint32 xmag; ///< - uint32 ymag; ///< - uint32 zmag; ///< - int32 baro; ///< - uint32 gdist; ///< - int32 temp; ///< - -} sensraw2_t; - -/** - * @brief Send a sensraw2 message - * - * @param xacc X acceleration (mg raw) - * @param yacc Y acceleration (mg raw) - * @param zacc Z acceleration (mg raw) - * @param xgyro Angular speed around X axis (adc units) - * @param ygyro Angular speed around Y axis (adc units) - * @param zgyro Angular speed around Z axis (adc units) - * @param xmag - * @param ymag - * @param zmag - * @param baro - * @param gdist - * @param temp - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t message_sensraw2_pack(uint8_t system_id, CommMessage_t* msg, uint32 xacc, uint32 yacc, uint32 zacc, uint32 xgyro, uint32 ygyro, uint32 zgyro, uint32 xmag, uint32 ymag, uint32 zmag, int32 baro, uint32 gdist, int32 temp) -{ - msg->msgid = MESSAGE_ID_SENSRAW2; - uint16_t i = 0; - - i += put_uint32_by_index(xacc, i, msg->payload); //X acceleration (mg raw) - i += put_uint32_by_index(yacc, i, msg->payload); //Y acceleration (mg raw) - i += put_uint32_by_index(zacc, i, msg->payload); //Z acceleration (mg raw) - i += put_uint32_by_index(xgyro, i, msg->payload); //Angular speed around X axis (adc units) - i += put_uint32_by_index(ygyro, i, msg->payload); //Angular speed around Y axis (adc units) - i += put_uint32_by_index(zgyro, i, msg->payload); //Angular speed around Z axis (adc units) - i += put_uint32_by_index(xmag, i, msg->payload); // - i += put_uint32_by_index(ymag, i, msg->payload); // - i += put_uint32_by_index(zmag, i, msg->payload); // - i += put_int32_by_index(baro, i, msg->payload); // - i += put_uint32_by_index(gdist, i, msg->payload); // - i += put_int32_by_index(temp, i, msg->payload); // - - return finalize_message(msg, system_id, i); -} - -static inline uint16_t message_sensraw2_encode(uint8_t system_id, CommMessage_t* msg, const sensraw2_t* sensraw2) -{ - message_sensraw2_pack(system_id, msg, sensraw2->xacc, sensraw2->yacc, sensraw2->zacc, sensraw2->xgyro, sensraw2->ygyro, sensraw2->zgyro, sensraw2->xmag, sensraw2->ymag, sensraw2->zmag, sensraw2->baro, sensraw2->gdist, sensraw2->temp); -} -// MESSAGE SENSRAW2 UNPACKING - -/** - * @brief Get field xacc from sensraw2 message - * - * @return X acceleration (mg raw) - */ -static inline uint32 message_sensraw2_get_xacc(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field yacc from sensraw2 message - * - * @return Y acceleration (mg raw) - */ -static inline uint32 message_sensraw2_get_yacc(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field zacc from sensraw2 message - * - * @return Z acceleration (mg raw) - */ -static inline uint32 message_sensraw2_get_zacc(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field xgyro from sensraw2 message - * - * @return Angular speed around X axis (adc units) - */ -static inline uint32 message_sensraw2_get_xgyro(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field ygyro from sensraw2 message - * - * @return Angular speed around Y axis (adc units) - */ -static inline uint32 message_sensraw2_get_ygyro(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field zgyro from sensraw2 message - * - * @return Angular speed around Z axis (adc units) - */ -static inline uint32 message_sensraw2_get_zgyro(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field xmag from sensraw2 message - * - * @return - */ -static inline uint32 message_sensraw2_get_xmag(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field ymag from sensraw2 message - * - * @return - */ -static inline uint32 message_sensraw2_get_ymag(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field zmag from sensraw2 message - * - * @return - */ -static inline uint32 message_sensraw2_get_zmag(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field baro from sensraw2 message - * - * @return - */ -static inline int32 message_sensraw2_get_baro(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field gdist from sensraw2 message - * - * @return - */ -static inline uint32 message_sensraw2_get_gdist(CommMessage_t* msg) -{ - -} - -/** - * @brief Get field temp from sensraw2 message - * - * @return - */ -static inline int32 message_sensraw2_get_temp(CommMessage_t* msg) -{ - -} - -static inline void message_sensraw2_decode(CommMessage_t* msg, sensraw2_t* sensraw2) -{ - sensraw2->xacc = message_sensraw2_get_xacc(msg); - sensraw2->yacc = message_sensraw2_get_yacc(msg); - sensraw2->zacc = message_sensraw2_get_zacc(msg); - sensraw2->xgyro = message_sensraw2_get_xgyro(msg); - sensraw2->ygyro = message_sensraw2_get_ygyro(msg); - sensraw2->zgyro = message_sensraw2_get_zgyro(msg); - sensraw2->xmag = message_sensraw2_get_xmag(msg); - sensraw2->ymag = message_sensraw2_get_ymag(msg); - sensraw2->zmag = message_sensraw2_get_zmag(msg); - sensraw2->baro = message_sensraw2_get_baro(msg); - sensraw2->gdist = message_sensraw2_get_gdist(msg); - sensraw2->temp = message_sensraw2_get_temp(msg); -} diff --git a/test/mavlink.h b/test/mavlink.h deleted file mode 100644 index 2fd7063d99507aaad1f7a51a099fc466362f0f5e..0000000000000000000000000000000000000000 --- a/test/mavlink.h +++ /dev/null @@ -1,19 +0,0 @@ -/** @file - * @brief MAVLink comm protocol. - * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sonntag, März 21 2010, 14:05 UTC - */ -#ifndef MAVLINK_H -#define MAVLINK_H - -#include "protocol.h" - -// TO BE REMOVED: -#include "generated/messages.h" - - -#include "generated/mavlink_message_heartbeat2.h" -#include "generated/mavlink_message_boot2.h" -#include "generated/mavlink_message_sensraw2.h" -#include "generated/mavlink_message_attitude2.h" -#endif \ No newline at end of file diff --git a/test/mavlink.lcm b/test/mavlink.lcm deleted file mode 100644 index 30d76427baeefe1a32808fe270b63cee616863a3..0000000000000000000000000000000000000000 --- a/test/mavlink.lcm +++ /dev/null @@ -1,37 +0,0 @@ - -typedef struct __heartbeat2_t -{ - uint8 type; ///< Type of the MAV (quadrotor, helicopter, etc.) - -} heartbeat2_t; - -typedef struct __boot2_t -{ - uint32 version; ///< The onboard software version - -} boot2_t; - -typedef struct __sensraw2_t -{ - uint32 xacc; ///< X acceleration (mg raw) - uint32 yacc; ///< Y acceleration (mg raw) - uint32 zacc; ///< Z acceleration (mg raw) - uint32 xgyro; ///< Angular speed around X axis (adc units) - uint32 ygyro; ///< Angular speed around Y axis (adc units) - uint32 zgyro; ///< Angular speed around Z axis (adc units) - uint32 xmag; ///< - uint32 ymag; ///< - uint32 zmag; ///< - int32 baro; ///< - uint32 gdist; ///< - int32 temp; ///< - -} sensraw2_t; - -typedef struct __attitude2_t -{ - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - -} attitude2_t; diff --git a/test/mavlink.xml b/test/mavlink.xml deleted file mode 100644 index bc20e784979f095db41748c990113a7e43a110b3..0000000000000000000000000000000000000000 --- a/test/mavlink.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - Type of the MAV (quadrotor, helicopter, etc.) - - - - The onboard software version - - - - X acceleration (mg raw) - Y acceleration (mg raw) - Z acceleration (mg raw) - Angular speed around X axis (adc units) - Angular speed around Y axis (adc units) - Angular speed around Z axis (adc units) - X Magnetic field (milli tesla) - Y Magnetic field (milli tesla) - Z Magnetic field (milli tesla) - Barometric pressure (hecto Pascal) - Ground distance (meters) - Temperature (degrees celcius) - - - - Roll angle (rad) - Pitch angle (rad) - Yaw angle (rad) - Roll angular speed (rad/s) - Pitch angular speed (rad/s) - Yaw angular speed (rad/s) - - - \ No newline at end of file diff --git a/test/media/face1.png b/test/media/face1.png deleted file mode 100644 index 2656b50e83aa28dd7c33c6df7d0a99487e239e9b..0000000000000000000000000000000000000000 Binary files a/test/media/face1.png and /dev/null differ diff --git a/test/media/face2.png b/test/media/face2.png deleted file mode 100644 index 63a87d5cfee4d00e8e3ba5c1a6f2a95bf34d3515..0000000000000000000000000000000000000000 Binary files a/test/media/face2.png and /dev/null differ diff --git a/test/media/face3.png b/test/media/face3.png deleted file mode 100644 index 1c61d580271d2553ef5f17da08fd5963d601e50f..0000000000000000000000000000000000000000 Binary files a/test/media/face3.png and /dev/null differ diff --git a/test/media/face4.png b/test/media/face4.png deleted file mode 100644 index b18858f7c3635f4cccd20afd7b38ca346327bcc9..0000000000000000000000000000000000000000 Binary files a/test/media/face4.png and /dev/null differ diff --git a/test/media/face5.png b/test/media/face5.png deleted file mode 100644 index b42231e5da8b46f492e58f22f24fe2e96ca55bd7..0000000000000000000000000000000000000000 Binary files a/test/media/face5.png and /dev/null differ diff --git a/test/media/letterB.png b/test/media/letterB.png deleted file mode 100644 index de8dec9cf8d4b7ff09031ab77b19a5d637b65549..0000000000000000000000000000000000000000 Binary files a/test/media/letterB.png and /dev/null differ diff --git a/test/media/letterD.png b/test/media/letterD.png deleted file mode 100644 index f96b3e8fe696a9e3164923779125d37101ca57bf..0000000000000000000000000000000000000000 Binary files a/test/media/letterD.png and /dev/null differ diff --git a/test/media/letterP.png b/test/media/letterP.png deleted file mode 100644 index 4ee1a56ad59ebb9de8cc7ef7429d867858ce7419..0000000000000000000000000000000000000000 Binary files a/test/media/letterP.png and /dev/null differ diff --git a/test/media/letterR.png b/test/media/letterR.png deleted file mode 100644 index 2329f1464f23bd6ecd2d3b39f73c6392266c53ba..0000000000000000000000000000000000000000 Binary files a/test/media/letterR.png and /dev/null differ diff --git a/test/media/letterS.png b/test/media/letterS.png deleted file mode 100644 index 4cb14b1b69909e6d943e471d3a0c6ae4086706b0..0000000000000000000000000000000000000000 Binary files a/test/media/letterS.png and /dev/null differ