diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 380ff1e3e13c340064f165093ffd03bd75c30107..f4f393e6077eadfec13914c775dd5c5dfe01504f 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -555,7 +555,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_param_value_t value; mavlink_msg_param_value_decode(&message, &value); - QByteArray bytes((char*)value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QByteArray bytes(value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); QString parameterName = QString(bytes); int component = message.compid; mavlink_param_union_t val; @@ -1964,36 +1964,30 @@ void UAS::receiveButton(int buttonIndex) void UAS::halt() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT); -// // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_HOLD, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0); + sendMessage(msg); } void UAS::go() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_CONTINUE); -// // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0); + sendMessage(msg); } /** Order the robot to return home / to land on the runway **/ void UAS::home() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_RETURN); -// // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); + mavlink_message_t msg; + + double latitude = UASManager::instance()->getHomeLatitude(); + double longitude = UASManager::instance()->getHomeLongitude(); + double altitude = UASManager::instance()->getHomeAltitude(); + int frame = UASManager::instance()->getHomeFrame(); + + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, frame, 0, latitude, longitude, altitude); + sendMessage(msg); } /** @@ -2002,13 +1996,8 @@ void UAS::home() */ void UAS::emergencySTOP() { -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// // FIXME MAVLINKV10PORTINGNEEDED -// //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); -// // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED + halt(); } /** @@ -2019,6 +2008,7 @@ void UAS::emergencySTOP() */ bool UAS::emergencyKILL() { + halt(); // FIXME MAVLINKV10PORTINGNEEDED // bool result = false; // QMessageBox msgBox; @@ -2108,41 +2098,33 @@ void UAS::stopHil() void UAS::shutdown() { - // FIXME MAVLINKV10PORTINGNEEDED -// bool result = false; -// QMessageBox msgBox; -// msgBox.setIcon(QMessageBox::Critical); -// msgBox.setText("Shutting down the UAS"); -// msgBox.setInformativeText("Do you want to shut down the onboard computer?"); - -// msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); -// msgBox.setDefaultButton(QMessageBox::Cancel); -// int ret = msgBox.exec(); + bool result = false; + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("Shutting down the UAS"); + msgBox.setInformativeText("Do you want to shut down the onboard computer?"); -// // Close the message box shortly after the click to prevent accidental clicks -// QTimer::singleShot(5000, &msgBox, SLOT(reject())); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Cancel); + int ret = msgBox.exec(); -// if (ret == QMessageBox::Yes) -// { -// // If the active UAS is set, execute command -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN); -// // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); + // Close the message box shortly after the click to prevent accidental clicks + QTimer::singleShot(5000, &msgBox, SLOT(reject())); -// result = true; -// } + if (ret == QMessageBox::Yes) + { + // If the active UAS is set, execute command + mavlink_message_t msg; + mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, 2, 0, 0, 0); + sendMessage(msg); + result = true; + } } void UAS::setTargetPosition(float x, float y, float z, float yaw) { mavlink_message_t msg; mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw); - - // Send message twice to increase chance of reception - sendMessage(msg); sendMessage(msg); } diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 36b4eeac470bd0f79f88a0c9a4a49525c5edfb8e..ad8d9fdce3d5e4c710e373f9aaec2209e70191c3 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -214,7 +214,8 @@ UASManager::UASManager() : activeUAS(NULL), homeLat(47.3769), homeLon(8.549444), - homeAlt(470.0) + homeAlt(470.0), + homeFrame(MAV_FRAME_GLOBAL) { loadSettings(); setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1); diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index 743fb195a1af2391566817c8fb2a4628beba830f..fc6bb0d51f9fd4de8e7b437db22dc5c98cca3e1a 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -84,6 +84,12 @@ public: return homeAlt; } + /** @brief Get the home position coordinate frame */ + int getHomeFrame() const + { + return homeFrame; + } + /** @brief Convert WGS84 coordinates to earth centric frame */ Eigen::Vector3d wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude); /** @brief Convert earth centric frame to EAST-NORTH-UP frame (x-y-z directions */ @@ -238,6 +244,7 @@ protected: double homeLat; double homeLon; double homeAlt; + int homeFrame; Eigen::Quaterniond ecef_ref_orientation_; Eigen::Vector3d ecef_ref_point_; Eigen::Vector3d nedSafetyLimitPosition1; diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index c19b094a7d49c40ee2288457b219bdef1a001bc8..f79ba30da68aef44bedcdaefc3d72a723b5efcf6 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -616,8 +616,6 @@ void QGCParamWidget::requestParameterList() // Request twice as mean of forward error correction mav->requestParameters(); - QGC::SLEEP::msleep(15); - mav->requestParameters(); } void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column)