diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index 7be36f7f3773145bb7ea1762c05967599394e285..384066afd19a3a90b0c7f66935a73afac527f2f2 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -197,6 +197,18 @@ bool GAudioOutput::startEmergency() return true; } +/** + * The emergency sound will be played continously during the emergency. + * call stopEmergency() to disable it again. No speech synthesis or other + * audio output is available during the emergency. + * + * @return true if the emergency could be started, false else + */ +bool GAudioOutput::startEmergency(QString message) +{ + return startEmergency(); +} + /** * Stops the continous emergency sound. Use startEmergency() to start * the emergency sound. diff --git a/src/GAudioOutput.h b/src/GAudioOutput.h index 9f1c93db36d050832b591cffb3653daf97f70234..7e6c6cc781b44545067c311ba65971ec0ba6bb82 100644 --- a/src/GAudioOutput.h +++ b/src/GAudioOutput.h @@ -77,6 +77,8 @@ public slots: bool alert(QString text); /** @brief Start emergency sound */ bool startEmergency(); + /** @brief Start emergency sound */ + bool startEmergency(QString); /** @brief Stop emergency sound */ bool stopEmergency(); /** @brief Select female voice */ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 95114b508ce509521e498a1fef35fd575d519bcf..aaba0648cfe06ae5d729f6f53d1a5fbb22cc36d7 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -67,7 +67,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : manualThrust(0), receiveDropRate(0), sendDropRate(0), - unknownPackets() + unknownPackets(), + lowBattAlarm(false) { setBattery(LIPOLY, 3); mavlink = protocol; @@ -181,6 +182,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); emit voltageChanged(message.sysid, state.vbat/1000.0f); + // LOW BATTERY ALARM + float chargeLevel = getChargeLevel(); + if (chargeLevel <= 10.0f) + { + startLowBattAlarm(); + } + else + { + stopLowBattAlarm(); + } + // COMMUNICATIONS DROP RATE emit dropRateChanged(this->getUASID(), this->receiveDropRate, this->sendDropRate); @@ -806,11 +818,32 @@ int UAS::calculateTimeRemaining() return remaining; } +/** + * @return charge level in percent - 0 - 100 + */ double UAS::getChargeLevel() { return 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); } +void UAS::startLowBattAlarm() +{ + if (!lowBattAlarm) + { + GAudioOutput::instance()->startEmergency("BATTERY"); + lowBattAlarm = true; + } +} + +void UAS::stopLowBattAlarm() +{ + if (lowBattAlarm) + { + GAudioOutput::instance()->stopEmergency(); + lowBattAlarm = false; + } +} + void UAS::clearWaypointList() { mavlink_message_t message; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 89e56d6cf70648f78ab5aab4bad7e0247f23fcac..8b702dbc95856d3a2d48ae2e756b236c1ff80579 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -121,6 +121,7 @@ protected: double manualThrust; ///< Thrust set by human pilot (radians) float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS + bool lowBattAlarm; ///< Switch if battery is low /** @brief Set the current battery type */ void setBattery(BatteryType type, int cells); @@ -136,7 +137,9 @@ protected: public slots: /** @brief Launches the system **/ void launch(); + /** @brief Write this waypoint to the list of waypoints */ void setWaypoint(Waypoint* wp); + /** @brief Set currently active waypoint */ void setWaypointActive(int id); /** @brief Order the robot to return home / to land on the runway **/ void home(); @@ -151,6 +154,9 @@ public slots: /** @brief Shut the system cleanly down. Will shut down any onboard computers **/ void shutdown(); + void startLowBattAlarm(); + void stopLowBattAlarm(); + void requestWaypoints(); void requestParameters(); void clearWaypointList(); diff --git a/src/ui/ParameterInterface.cc b/src/ui/ParameterInterface.cc index 540aa6d87784325fc4f5c2f3e55d1ce32d1a8a72..850a23abd237349ab9c476ac7e6730a843389444 100644 --- a/src/ui/ParameterInterface.cc +++ b/src/ui/ParameterInterface.cc @@ -14,10 +14,6 @@ ParameterInterface::ParameterInterface(QWidget *parent) : m_ui->setupUi(this); connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); - // FIXME Testing TODO - - QString testData = "IMU\n ROLL_K_P\t0.527\n ROLL_K_I\t1.255\n PITCH_K_P\t0.621\n PITCH_K_I\t2.5545\n"; - tree = new ParamTreeModel(); treeView = new QTreeView(); @@ -76,11 +72,10 @@ void ParameterInterface::addComponent(UASInterface* uas, int component, QString void ParameterInterface::receiveParameter(int uas, int component, QString parameterName, float value) { Q_UNUSED(uas); - qDebug() << "RECEIVED PARAMETER" << component << parameterName << value; // Insert parameter into map tree->appendParam(component, parameterName, value); // Refresh view - treeView->setModel(tree); + treeView->update(); } /** diff --git a/src/ui/param/ParamTreeModel.cc b/src/ui/param/ParamTreeModel.cc index e86ae1daf61ab2c4dd0aa5ca421a2519814d35c7..797f85cb3170b59165e05f2863df32e0335dd7c7 100644 --- a/src/ui/param/ParamTreeModel.cc +++ b/src/ui/param/ParamTreeModel.cc @@ -40,7 +40,8 @@ ParamTreeModel::ParamTreeModel(QObject *parent) QList rootData; rootData << tr("Parameter") << tr("Value"); rootItem = new ParamTreeItem(rootData); - //setupModelData(data.split(QString("\n")), rootItem); + QString data = "IMU\n ROLL_K_I\t1.255\n PITCH_K_P\t0.621\n PITCH_K_I\t2.5545\n"; + setupModelData(data.split(QString("\n")), rootItem); } ParamTreeModel::ParamTreeModel(const QString &data, QObject *parent) @@ -153,20 +154,29 @@ void ParamTreeModel::appendComponent(int componentId, QString name) { if (!components.contains(componentId)) { - ParamTreeItem* item = new ParamTreeItem(name + QString("(") + QString::number(componentId) + QString(")"), 0, rootItem); + ParamTreeItem* item = new ParamTreeItem(name + QString("(#") + QString::number(componentId) + QString(")"), 0, rootItem); + components.insert(componentId, item); + } +} + +void ParamTreeModel::appendComponent(int componentId) +{ + if (!components.contains(componentId)) + { + ParamTreeItem* item = new ParamTreeItem(QString("Component #") + QString::number(componentId) + QString(""), 0, rootItem); components.insert(componentId, item); } } void ParamTreeModel::appendParam(int componentId, QString name, float value) { - ParamTreeItem* comp = components.value(componentId); + // If component does not exist yet - if (comp == NULL) + if (!components.contains(componentId)) { - appendComponent(componentId, name); - comp = components.value(componentId); + appendComponent(componentId); } + ParamTreeItem* comp = components.value(componentId); // FIXME Children may be double here comp->appendChild(new ParamTreeItem(name, value, comp)); qDebug() << __FILE__ << __LINE__ << "Added param" << name << value << "for component" << comp->getParamName(); diff --git a/src/ui/param/ParamTreeModel.h b/src/ui/param/ParamTreeModel.h index 572ea64921e94921a551f3bf7fd47ae7c47df431..b0a190287b3d9329aaa28fb56ab3514828127d7d 100644 --- a/src/ui/param/ParamTreeModel.h +++ b/src/ui/param/ParamTreeModel.h @@ -61,6 +61,8 @@ public slots: void appendParam(int componentId, QString name, float value); /** @brief Add a new component for this system */ void appendComponent(int componentId, QString name); + /** @brief Add a new component for this system */ + void appendComponent(int componentId); protected: ParamTreeItem* getNodeForComponentId(int id);