diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 75fb54d..a0c75d8 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -249,9 +249,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) switch (message.compid) { - case MAV_COMP_ID_IMU_2: + case MAV_COMP_ID_IMU: // Prefer IMU 2 over IMU 1 (FIXME) - componentID[message.msgid] = MAV_COMP_ID_IMU_2; + componentID[message.msgid] = MAV_COMP_ID_IMU; break; default: // Do nothing @@ -2236,7 +2236,7 @@ void UAS::go() sendMessage(msg); } -/** Order the robot to return home / to land on the runway **/ +/** Order the robot to return home **/ void UAS::home() { mavlink_message_t msg; @@ -2250,6 +2250,15 @@ void UAS::home() sendMessage(msg); } +/** Order the robot to land on the runway **/ +void UAS::land() +{ + mavlink_message_t msg; + + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_LAND, 1, 0, 0, 0, 0, 0, 0, 0); + sendMessage(msg); +} + /** * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation * and might differ between systems. diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d9e7fe2..bf6ad12 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -511,8 +511,10 @@ public slots: //void setWaypoint(Waypoint* wp); FIXME tbd /** @brief Set currently active waypoint */ //void setWaypointActive(int id); FIXME tbd - /** @brief Order the robot to return home / to land on the runway **/ + /** @brief Order the robot to return home **/ void home(); + /** @brief Order the robot to land **/ + void land(); void halt(); void go(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index cb083ab..5b11944 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -237,6 +237,8 @@ public slots: //virtual void setWaypointActive(int wp) = 0; /** @brief Order the robot to return home / to land on the runway **/ virtual void home() = 0; + /** @brief Order the robot to land **/ + virtual void land() = 0; /** @brief Halt the system */ virtual void halt() = 0; /** @brief Start/continue the current robot action */ diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 680fa8b..530274a 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -109,7 +109,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(m_ui->liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); connect(m_ui->haltButton, SIGNAL(clicked()), uas, SLOT(halt())); connect(m_ui->continueButton, SIGNAL(clicked()), uas, SLOT(go())); - connect(m_ui->landButton, SIGNAL(clicked()), uas, SLOT(home())); + connect(m_ui->landButton, SIGNAL(clicked()), uas, SLOT(land())); connect(m_ui->abortButton, SIGNAL(clicked()), uas, SLOT(emergencySTOP())); connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL())); connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));