Newer
Older
}
else
{
qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first";
}
int UAS::getSystemType()
{
return this->type;
}
switch (buttonIndex)
{
// qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!";
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);
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()
{
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);
}
/**
* The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation
* and might differ between systems.
*/
void UAS::emergencySTOP()
{
// FIXME MAVLINKV10PORTINGNEEDED
halt();
* Shut down this mav - All onboard systems are immediately shut down (e.g. the main power line is cut).
* @warning This might lead to a crash
*
* The command will not be executed until emergencyKILLConfirm is issues immediately afterwards
*/
bool UAS::emergencyKILL()
{
2079
2080
2081
2082
2083
2084
2085
2086
2087
2088
2089
2090
2091
2092
2093
2094
2095
2096
2097
2098
2099
2100
2101
2102
2103
2104
// FIXME MAVLINKV10PORTINGNEEDED
// bool result = false;
// QMessageBox msgBox;
// msgBox.setIcon(QMessageBox::Critical);
// msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS");
// msgBox.setInformativeText("Do you want to cut power on all systems?");
// msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
// msgBox.setDefaultButton(QMessageBox::Cancel);
// int ret = msgBox.exec();
// // Close the message box shortly after the click to prevent accidental clicks
// QTimer::singleShot(5000, &msgBox, SLOT(reject()));
// if (ret == QMessageBox::Yes)
// {
// 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_EMCY_KILL);
// // Send message twice to increase chance of reception
// sendMessage(msg);
// sendMessage(msg);
// result = true;
// }
// return result;
return false;
void UAS::enableHil(bool enable)
{
// Connect Flight Gear Link
if (enable)
{
2117
2118
2119
2120
2121
2122
2123
2124
2125
2126
2127
2128
2129
2130
2131
2132
2133
2134
2135
2136
2137
2138
2139
2140
2141
2142
2143
2144
2145
2146
2147
}
}
/**
* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
*/
void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt,
int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
{
mavlink_message_t msg;
mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc);
sendMessage(msg);
}
// Connect Flight Gear Link
simulation->connectSimulation();
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
Mariano Lizarraga
committed
}
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode);
Mariano Lizarraga
committed
}
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();
// Close the message box shortly after the click to prevent accidental clicks
QTimer::singleShot(5000, &msgBox, SLOT(reject()));
if (ret == QMessageBox::Yes)
{
// If the active UAS is set, execute command
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 2, 0, 0, 0, 0, 0);
sendMessage(msg);
result = true;
}
void UAS::setTargetPosition(float x, float y, float z, float yaw)
{
mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw);
}
/**
* @return The name of this system as string in human-readable form
*/
QString UAS::getUASName(void) const
if (name == "")
{
}
else
{
const QString& UAS::getShortState() const
{
return shortStateText;
}
QString UAS::getShortModeTextFor(int id)
{
QString mode;
{
mode.prepend("A|");
}
else
{
mode.prepend("D|");
}
// HARDWARE IN THE LOOP DECODING
const QString& UAS::getShortMode() const
{
return shortModeText;
}
if (!links->contains(link))
{
connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
void UAS::removeLink(QObject* object)
{
LinkInterface* link = dynamic_cast<LinkInterface*>(object);
if (link)
{
links->removeAt(links->indexOf(link));
}
}
QList<LinkInterface*>* UAS::getLinks()
{
return links;
}
QMap<int, QString> UAS::getComponents()
{
return components;
}
void UAS::setBattery(BatteryType type, int cells)
{
this->batteryType = type;
this->cells = cells;
switch (batteryType)
{
fullVoltage = this->cells * UAS::lipoFull;
emptyVoltage = this->cells * UAS::lipoEmpty;
break;
void UAS::setBatterySpecs(const QString& specs)
{
if (specs.length() == 0 || specs.contains("%"))
{
batteryRemainingEstimateEnabled = false;
bool ok;
QString percent = specs;
percent = percent.remove("%");
float temp = percent.toFloat(&ok);
if (ok)
{
}
else
{
emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong");
}
}
else
{
batteryRemainingEstimateEnabled = true;
QString stringList = specs;
stringList = stringList.remove("V");
stringList = stringList.remove("v");
QStringList parts = stringList.split(",");
if (parts.length() == 3)
{
float temp;
bool ok;
// Get the empty voltage
temp = parts.at(0).toFloat(&ok);
if (ok) emptyVoltage = temp;
// Get the warning voltage
temp = parts.at(1).toFloat(&ok);
if (ok) warnVoltage = temp;
// Get the full voltage
temp = parts.at(2).toFloat(&ok);
if (ok) fullVoltage = temp;
}
else
{
emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong");
}
}
}
QString UAS::getBatterySpecs()
{
if (batteryRemainingEstimateEnabled)
{
return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage);
}
else
{
return QString("%1%").arg(warnLevelPercent);
}
}
int UAS::calculateTimeRemaining()
{
quint64 dt = MG::TIME::getGroundTimeNow() - startTime;
double seconds = dt / 1000.0f;
double voltDifference = startVoltage - currentVoltage;
if (voltDifference <= 0) voltDifference = 0.00000000001f;
double dischargePerSecond = voltDifference / seconds;
int remaining = static_cast<int>((currentVoltage - emptyVoltage) / dischargePerSecond);
// Can never be below 0
if (remaining < 0) remaining = 0;
return remaining;
}
/**
* @return charge level in percent - 0 - 100
*/
float UAS::getChargeLevel()
if (batteryRemainingEstimateEnabled)
{
if (lpVoltage < emptyVoltage)
{
}
else if (lpVoltage > fullVoltage)
{
}
else
{
chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
}
}
return chargeLevel;
if (!lowBattAlarm)
{
GAudioOutput::instance()->alert(tr("system %1 has low battery").arg(getUASName()));
QTimer::singleShot(2500, GAudioOutput::instance(), SLOT(startEmergency()));
lowBattAlarm = true;
}
}
void UAS::stopLowBattAlarm()
{
if (lowBattAlarm)
{