Skip to content
GitLab
Commit fb7d8413 authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Merge branch 'new_state_machine_modes' of https://github.com/DrTon/qgroundcontrol into config

parents c5bed07a 8541598f
Hide whitespace changes
Inline Side-by-side
src/uas/UAS.cc
View file @ fb7d8413
...@@ -57,9 +57,9 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), ...@@ -57,9 +57,9 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
airframe(QGC_AIRFRAME_GENERIC), airframe(QGC_AIRFRAME_GENERIC),
autopilot(-1), autopilot(-1),
systemIsArmed(false), systemIsArmed(false),
mode(-1), base_mode(0),
custom_mode(0),
// custom_mode not initialized // custom_mode not initialized
navMode(-1),
status(-1), status(-1),
// shortModeText not initialized // shortModeText not initialized
// shortStateText not initialized // shortStateText not initialized
...@@ -337,7 +337,7 @@ void UAS::updateState() ...@@ -337,7 +337,7 @@ void UAS::updateState()
} }
else else
{ {
if (((mode&MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (mode&MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock) if (((base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock)
{ {
GAudioOutput::instance()->notifyNegative(); GAudioOutput::instance()->notifyNegative();
} }
...@@ -568,24 +568,18 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -568,24 +568,18 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
stateAudio = uasState; stateAudio = uasState;
} }
if (this->mode != static_cast<int>(state.base_mode)) if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode)
{ {
modechanged = true; modechanged = true;
this->mode = static_cast<int>(state.base_mode); this->base_mode = state.base_mode;
shortModeText = getShortModeTextFor(this->mode); this->custom_mode = state.custom_mode;
shortModeText = getShortModeTextFor(this->base_mode, this->custom_mode, this->autopilot);
emit modeChanged(this->getUASID(), shortModeText, ""); emit modeChanged(this->getUASID(), shortModeText, "");
modeAudio = " is now in " + audiomodeText; modeAudio = " is now in " + audiomodeText;
} }
if (navMode != state.custom_mode)
{
emit navModeChanged(uasId, state.custom_mode, getNavModeText(state.custom_mode));
navMode = state.custom_mode;
//navModeAudio = tr(" changed nav mode to ") + tr("FIXME");
}
// AUDIO // AUDIO
if (modechanged && statechanged) if (modechanged && statechanged)
{ {
...@@ -595,7 +589,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -595,7 +589,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
else if (modechanged || statechanged) else if (modechanged || statechanged)
{ {
// Output the one message // Output the one message
audiostring += modeAudio + stateAudio + navModeAudio; audiostring += modeAudio + stateAudio;
} }
if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY)) if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY))
...@@ -1826,25 +1820,24 @@ QList<int> UAS::getComponentIds() ...@@ -1826,25 +1820,24 @@ QList<int> UAS::getComponentIds()
/** /**
* @param mode that UAS is to be set to. * @param mode that UAS is to be set to.
*/ */
void UAS::setMode(int mode) void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode)
{ {
//this->mode = mode; //no call assignament, update receive message from UAS //this->mode = mode; //no call assignament, update receive message from UAS
// Strip armed / disarmed call, this is not relevant for setting the mode // Strip armed / disarmed call, this is not relevant for setting the mode
uint8_t newMode = mode; newBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
newMode &= (~(uint8_t)MAV_MODE_FLAG_SAFETY_ARMED);
// Now set current state (request no change) // Now set current state (request no change)
newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_SAFETY_ARMED); newBaseMode |= this->base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
// Strip HIL part, replace it with current system state // Strip HIL part, replace it with current system state
newMode &= (~(uint8_t)MAV_MODE_FLAG_HIL_ENABLED); newBaseMode &= (~MAV_MODE_FLAG_HIL_ENABLED);
// Now set current state (request no change) // Now set current state (request no change)
newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_HIL_ENABLED); newBaseMode |= this->base_mode & MAV_MODE_FLAG_HIL_ENABLED;
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newMode, (uint16_t)navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode);
sendMessage(msg); sendMessage(msg);
qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newMode; qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newBaseMode;
} }
/** /**
...@@ -1945,35 +1938,6 @@ float UAS::filterVoltage(float value) const ...@@ -1945,35 +1938,6 @@ float UAS::filterVoltage(float value) const
return lpVoltage * 0.7f + value * 0.3f; return lpVoltage * 0.7f + value * 0.3f;
} }
/**
* The mode can be preflight or unknown.
* @Return the mode of the autopilot
*/
QString UAS::getNavModeText(int mode)
{
if (autopilot == MAV_AUTOPILOT_PIXHAWK)
{
switch (mode)
{
case 0:
return QString("PREFLIGHT");
break;
default:
return QString("UNKNOWN");
}
}
else if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA)
{
return QString("UNKNOWN");
}
else if (autopilot == MAV_AUTOPILOT_OPENPILOT)
{
return QString("UNKNOWN");
}
// If nothing matches, return unknown
return QString("UNKNOWN");
}
/** /**
* Get the status of the code and a description of the status. * Get the status of the code and a description of the status.
* Status can be unitialized, booting up, calibrating sensors, active * Status can be unitialized, booting up, calibrating sensors, active
...@@ -2750,7 +2714,7 @@ void UAS::launch() ...@@ -2750,7 +2714,7 @@ void UAS::launch()
void UAS::armSystem() void UAS::armSystem()
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_SAFETY_ARMED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
sendMessage(msg); sendMessage(msg);
} }
...@@ -2761,35 +2725,35 @@ void UAS::armSystem() ...@@ -2761,35 +2725,35 @@ void UAS::armSystem()
void UAS::disarmSystem() void UAS::disarmSystem()
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & ~MAV_MODE_FLAG_SAFETY_ARMED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode & ~MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
sendMessage(msg); sendMessage(msg);
} }
void UAS::toggleArmedState() void UAS::toggleArmedState()
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode ^ MAV_MODE_FLAG_SAFETY_ARMED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode ^ MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
sendMessage(msg); sendMessage(msg);
} }
void UAS::goAutonomous() void UAS::goAutonomous()
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & MAV_MODE_FLAG_AUTO_ENABLED & ~MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_MODE_FLAG_AUTO_ENABLED, 0);
sendMessage(msg); sendMessage(msg);
} }
void UAS::goManual() void UAS::goManual()
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & ~MAV_MODE_FLAG_AUTO_ENABLED & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
sendMessage(msg); sendMessage(msg);
} }
void UAS::toggleAutonomy() void UAS::toggleAutonomy()
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
sendMessage(msg); sendMessage(msg);
} }
...@@ -2813,7 +2777,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double ...@@ -2813,7 +2777,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
manualThrust = thrust * thrustScaling; manualThrust = thrust * thrustScaling;
// If system has manual inputs enabled and is armed // If system has manual inputs enabled and is armed
if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED)) if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
{ {
mavlink_message_t message; mavlink_message_t message;
mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualPitchAngle, (float)manualRollAngle, (float)manualThrust, (float)manualYawAngle, buttons); mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualPitchAngle, (float)manualRollAngle, (float)manualThrust, (float)manualYawAngle, buttons);
...@@ -2831,7 +2795,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double ...@@ -2831,7 +2795,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw) void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{ {
// If system has manual inputs enabled and is armed // If system has manual inputs enabled and is armed
if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED)) if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
{ {
mavlink_message_t message; mavlink_message_t message;
mavlink_msg_setpoint_6dof_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)x, (float)y, (float)z, (float)roll, (float)pitch, (float)yaw); mavlink_msg_setpoint_6dof_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)x, (float)y, (float)z, (float)roll, (float)pitch, (float)yaw);
...@@ -3110,7 +3074,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa ...@@ -3110,7 +3074,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
float pitchspeed, float yawspeed, double lat, double lon, double alt, float pitchspeed, float yawspeed, double lat, double lon, double alt,
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
{ {
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{ {
float q[4]; float q[4];
...@@ -3139,7 +3103,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa ...@@ -3139,7 +3103,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg); sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
} }
...@@ -3148,7 +3112,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa ...@@ -3148,7 +3112,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
{ {
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
...@@ -3162,7 +3126,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl ...@@ -3162,7 +3126,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg); sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
} }
...@@ -3174,7 +3138,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi ...@@ -3174,7 +3138,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100) if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
return; return;
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{ {
float course = cog; float course = cog;
// map to 0..2pi // map to 0..2pi
...@@ -3193,7 +3157,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi ...@@ -3193,7 +3157,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg); sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
} }
...@@ -3209,7 +3173,7 @@ void UAS::startHil() ...@@ -3209,7 +3173,7 @@ void UAS::startHil()
hilEnabled = true; hilEnabled = true;
sensorHil = false; sensorHil = false;
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg); sendMessage(msg);
// Connect HIL simulation link // Connect HIL simulation link
simulation->connectSimulation(); simulation->connectSimulation();
...@@ -3222,7 +3186,7 @@ void UAS::stopHil() ...@@ -3222,7 +3186,7 @@ void UAS::stopHil()
{ {
if (simulation) simulation->disconnectSimulation(); if (simulation) simulation->disconnectSimulation();
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode); mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode & !MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg); sendMessage(msg);
hilEnabled = false; hilEnabled = false;
sensorHil = false; sensorHil = false;
...@@ -3350,51 +3314,57 @@ QString UAS::getAudioModeTextFor(int id) ...@@ -3350,51 +3314,57 @@ QString UAS::getAudioModeTextFor(int id)
* The mode returned can be auto, stabilized, test, manual, preflight or unknown. * The mode returned can be auto, stabilized, test, manual, preflight or unknown.
* @return the short text of the mode for the id given. * @return the short text of the mode for the id given.
*/ */
QString UAS::getShortModeTextFor(int id) QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot)
{ {
QString mode = ""; QString mode = "";
uint8_t modeid = id;
// BASE MODE DECODING enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
if (modeid == 0) PX4_CUSTOM_MODE_SEATBELT,
{ PX4_CUSTOM_MODE_EASY,
mode = "|PREFLIGHT"; PX4_CUSTOM_MODE_AUTO
} };
else {
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO){ if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
mode += "|AUTO"; // use custom_mode - autopilot-specific
if (autopilot == MAV_AUTOPILOT_PX4) {
if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
mode += "|MANUAL";
} else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
mode += "|SEATBELT";
} else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
mode += "|EASY";
} else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
mode += "|AUTO";
}
} }
}
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL){ // fallback to using base_mode
if (mode.length() == 0) {
// use base_mode - not autopilot-specific
if (base_mode == 0) {
mode += "|PREFLIGHT";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) {
mode += "|AUTO";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) {
mode += "|MANUAL"; mode += "|MANUAL";
if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED) {
mode += "|GUIDED";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) {
mode += "|STABILIZED";
}
} }
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED){
mode += "|VECTOR";
}
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE){
mode += "|STABILIZED";
}
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_TEST){
mode += "|TEST";
}
} }
if (mode.length() == 0) if (mode.length() == 0)
{ {
mode = "|UNKNOWN"; mode = "|UNKNOWN";
qDebug() << __FILE__ << __LINE__ << " Unknown modeid: " << modeid; qDebug() << __FILE__ << __LINE__ << " Unknown mode: base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot;
} }
// ARMED STATE DECODING // ARMED STATE DECODING
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
{ {
mode.prepend("A"); mode.prepend("A");
} }
...@@ -3404,12 +3374,12 @@ QString UAS::getShortModeTextFor(int id) ...@@ -3404,12 +3374,12 @@ QString UAS::getShortModeTextFor(int id)
} }
// HARDWARE IN THE LOOP DECODING // HARDWARE IN THE LOOP DECODING
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL) if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)
{ {
mode.prepend("HIL:"); mode.prepend("HIL:");
} }
qDebug() << "MODE: " << modeid << " " << mode; //qDebug() << "base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot << ": " << mode;
return mode; return mode;
} }
......
src/uas/UAS.h
View file @ fb7d8413
...@@ -69,7 +69,7 @@ public: ...@@ -69,7 +69,7 @@ public:
/** @brief Get short mode */ /** @brief Get short mode */
const QString& getShortMode() const; const QString& getShortMode() const;
/** @brief Translate from mode id to text */ /** @brief Translate from mode id to text */
static QString getShortModeTextFor(int id); static QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot);
/** @brief Translate from mode id to audio text */ /** @brief Translate from mode id to audio text */
static QString getAudioModeTextFor(int id); static QString getAudioModeTextFor(int id);
/** @brief Get the unique system id */ /** @brief Get the unique system id */
...@@ -362,9 +362,8 @@ protected: //COMMENTS FOR TEST UNIT ...@@ -362,9 +362,8 @@ protected: //COMMENTS FOR TEST UNIT
int airframe; ///< The airframe type int airframe; ///< The airframe type
int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
bool systemIsArmed; ///< If the system is armed bool systemIsArmed; ///< If the system is armed
uint8_t mode; ///< The current mode of the MAV uint8_t base_mode; ///< The current mode of the MAV
uint32_t custom_mode; ///< The current mode of the MAV uint32_t custom_mode; ///< The current mode of the MAV
uint32_t navMode; ///< The current navigation mode of the MAV
int status; ///< The current status of the MAV int status; ///< The current status of the MAV
QString shortModeText; ///< Short textual mode description QString shortModeText; ///< Short textual mode description
QString shortStateText; ///< Short textual state description QString shortStateText; ///< Short textual state description
...@@ -506,8 +505,6 @@ public: ...@@ -506,8 +505,6 @@ public:
float getChargeLevel(); float getChargeLevel();
/** @brief Get the human-readable status message for this code */ /** @brief Get the human-readable status message for this code */
void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription);
/** @brief Get the human-readable navigation mode translation for this mode */
QString getNavModeText(int mode);
/** @brief Check if vehicle is in autonomous mode */ /** @brief Check if vehicle is in autonomous mode */
bool isAuto(); bool isAuto();
/** @brief Check if vehicle is armed */ /** @brief Check if vehicle is armed */
...@@ -834,7 +831,7 @@ public slots: ...@@ -834,7 +831,7 @@ public slots:
void setSelected(); void setSelected();
/** @brief Set current mode of operation, e.g. auto or manual */ /** @brief Set current mode of operation, e.g. auto or manual */
void setMode(int mode); void setMode(uint8_t newBaseMode, uint32_t newCustomMode);
/** @brief Request all parameters */ /** @brief Request all parameters */
void requestParameters(); void requestParameters();
......
src/uas/UASInterface.h
View file @ fb7d8413
...@@ -295,7 +295,7 @@ public slots: ...@@ -295,7 +295,7 @@ public slots:
/** @brief Start/continue the current robot action */ /** @brief Start/continue the current robot action */
virtual void go() = 0; virtual void go() = 0;
/** @brief Set the current mode of operation */ /** @brief Set the current mode of operation */
virtual void setMode(int mode) = 0; virtual void setMode(uint8_t newBaseMode, uint32_t newCustomMode) = 0;
/** Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/ /** Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/
virtual void emergencySTOP() = 0; virtual void emergencySTOP() = 0;
/** Kills the robot. All systems are immediately shut down (e.g. the main power line is cut). This might lead to a crash **/ /** Kills the robot. All systems are immediately shut down (e.g. the main power line is cut). This might lead to a crash **/
......
src/ui/uas/UASControlWidget.cc
View file @ fb7d8413
...@@ -33,7 +33,6 @@ This file is part of the PIXHAWK project ...@@ -33,7 +33,6 @@ This file is part of the PIXHAWK project
#include <QTimer> #include <QTimer>
#include <QLabel> #include <QLabel>
#include <QFileDialog> #include <QFileDialog>
#include <QDebug>
#include <QProcess> #include <QProcess>
#include <QPalette> #include <QPalette>
...@@ -42,71 +41,111 @@ This file is part of the PIXHAWK project ...@@ -42,71 +41,111 @@ This file is part of the PIXHAWK project
#include <UAS.h> #include <UAS.h>
#include "QGC.h" #include "QGC.h"
static struct full_mode_s modes_list_common[] = {
{ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
0 },
{ (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
0 },
{ (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
0 },
{ (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
0 },
};
static struct full_mode_s modes_list_px4[] = {
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
PX4_CUSTOM_MODE_MANUAL },
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
PX4_CUSTOM_MODE_SEATBELT },
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
PX4_CUSTOM_MODE_EASY },
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
PX4_CUSTOM_MODE_AUTO },
};
UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
uas(0), uasID(-1),
uasMode(0), modesList(NULL),
engineOn(false) modesNum(0),
modeIdx(0),
armed(false)
{ {
ui.setupUi(this); ui.setupUi(this);
this->setUAS(UASManager::instance()->getActiveUAS());
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
ui.modeComboBox->clear();
int modes[] = {
MAV_MODE_PREFLIGHT,
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED,
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED,
};
for (int i = 0; i < sizeof(modes) / sizeof(int); i++) {
int mode = modes[i];
ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode).remove(0, 2), mode);
}
connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int))); connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));
uasMode = ui.modeComboBox->itemData(ui.modeComboBox->currentIndex()).toInt(); ui.gridLayout->setAlignment(Qt::AlignTop);
}
ui.modeComboBox->setCurrentIndex(0); void UASControlWidget::updateModesList()
{
// Detect autopilot type
int autopilot = 0;
if (this->uasID >= 0) {
UASInterface *uas = UASManager::instance()->getUASForId(this->uasID);
if (uas) {
autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType();
}
}
ui.gridLayout->setAlignment(Qt::AlignTop); // Use corresponding modes list
if (autopilot == MAV_AUTOPILOT_PX4) {
modesList = modes_list_px4;
modesNum = sizeof(modes_list_px4) / sizeof(struct full_mode_s);
} else {
modesList = modes_list_common;
modesNum = sizeof(modes_list_common) / sizeof(struct full_mode_s);
}
// Set combobox items
ui.modeComboBox->clear();
for (int i = 0; i < modesNum; i++) {
struct full_mode_s mode = modesList[i];
ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode.baseMode, mode.customMode, autopilot).remove(0, 2), i);
}
// Select first mode in list
modeIdx = 0;
ui.modeComboBox->setCurrentIndex(modeIdx);
ui.modeComboBox->update();
} }
void UASControlWidget::setUAS(UASInterface* uas) void UASControlWidget::setUAS(UASInterface* uas)
{ {
if (this->uas) if (this->uasID) {
{ UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uasID);
UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas);
disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem())); disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem()));
disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch())); disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch()));
disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home())); disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home()));
disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown())); disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown()));
//connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int, QString, QString)));
disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
} }
// Connect user interface controls // Connect user interface controls
if (uas) if (uas) {
{ connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton()));
connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton())); connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home()));
connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home())); connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
//connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); connect(uas, SIGNAL(modeChanged(int, QString, QString)), this, SLOT(updateMode(int, QString, QString)));
connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());
ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());
this->uasID = uas->getUASID();
this->uas = uas->getUASID(); setBackgroundColor(uas->getColor());
setBackgroundColor(uas->getColor()); } else {
} this->uasID = -1;
else }
{ this->updateModesList();
this->uas = -1; this->updateArmText();
}
} }
UASControlWidget::~UASControlWidget() UASControlWidget::~UASControlWidget()
...@@ -114,15 +153,11 @@ UASControlWidget::~UASControlWidget() ...@@ -114,15 +153,11 @@ UASControlWidget::~UASControlWidget()
} }
void UASControlWidget::updateStatemachine() void UASControlWidget::updateArmText()
{ {
if (armed) {
if (engineOn)
{
ui.controlButton->setText(tr("DISARM SYSTEM")); ui.controlButton->setText(tr("DISARM SYSTEM"));
} } else {
else
{
ui.controlButton->setText(tr("ARM SYSTEM")); ui.controlButton->setText(tr("ARM SYSTEM"));
} }
} }
...@@ -138,7 +173,7 @@ void UASControlWidget::setBackgroundColor(QColor color) ...@@ -138,7 +173,7 @@ void UASControlWidget::setBackgroundColor(QColor color)
QString colorstyle; QString colorstyle;
QString borderColor = "#4A4A4F"; QString borderColor = "#4A4A4F";
borderColor = "#FA4A4F"; borderColor = "#FA4A4F";
uasColor = uasColor.darker(900); uasColor = uasColor.darker(400);
colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }", colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }",
uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str()); uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str());
setStyleSheet(colorstyle); setStyleSheet(colorstyle);
...@@ -149,7 +184,7 @@ void UASControlWidget::setBackgroundColor(QColor color) ...@@ -149,7 +184,7 @@ void UASControlWidget::setBackgroundColor(QColor color)
} }
void UASControlWidget::updateMode(int uas,QString mode,QString description) void UASControlWidget::updateMode(int uas, QString mode, QString description)
{ {
Q_UNUSED(uas); Q_UNUSED(uas);
Q_UNUSED(mode); Q_UNUSED(mode);
...@@ -158,17 +193,15 @@ void UASControlWidget::updateMode(int uas,QString mode,QString description) ...@@ -158,17 +193,15 @@ void UASControlWidget::updateMode(int uas,QString mode,QString description)
void UASControlWidget::updateState(int state) void UASControlWidget::updateState(int state)
{ {
switch (state) switch (state) {
{
case (int)MAV_STATE_ACTIVE: case (int)MAV_STATE_ACTIVE:
engineOn = true; armed = true;
ui.controlButton->setText(tr("DISARM SYSTEM"));
break; break;
case (int)MAV_STATE_STANDBY: case (int)MAV_STATE_STANDBY:
engineOn = false; armed = false;
ui.controlButton->setText(tr("ARM SYSTEM"));
break; break;
} }
this->updateArmText();
} }
/** /**
...@@ -177,7 +210,7 @@ void UASControlWidget::updateState(int state) ...@@ -177,7 +210,7 @@ void UASControlWidget::updateState(int state)
void UASControlWidget::setMode(int mode) void UASControlWidget::setMode(int mode)
{ {
// Adapt context button mode // Adapt context button mode
uasMode = ui.modeComboBox->itemData(mode).toInt(); modeIdx = mode;
ui.modeComboBox->blockSignals(true); ui.modeComboBox->blockSignals(true);
ui.modeComboBox->setCurrentIndex(mode); ui.modeComboBox->setCurrentIndex(mode);
ui.modeComboBox->blockSignals(false); ui.modeComboBox->blockSignals(false);
...@@ -187,43 +220,36 @@ void UASControlWidget::setMode(int mode) ...@@ -187,43 +220,36 @@ void UASControlWidget::setMode(int mode)
void UASControlWidget::transmitMode() void UASControlWidget::transmitMode()
{ {
UASInterface* mav = UASManager::instance()->getUASForId(this->uas); UASInterface* uas = UASManager::instance()->getUASForId(this->uasID);
if (mav) if (uas) {
{ if (modeIdx >= 0 && modeIdx < modesNum) {
// include armed state struct full_mode_s mode = modesList[modeIdx];
if (engineOn) // include armed state
uasMode |= MAV_MODE_FLAG_SAFETY_ARMED; if (armed) {
else mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
uasMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } else {
mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
mav->setMode(uasMode); }
QString mode = ui.modeComboBox->currentText();
uas->setMode(mode.baseMode, mode.customMode);
ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(mode).arg(mav->getUASName())); QString modeText = ui.modeComboBox->currentText();
ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName()));
}
} }
} }
void UASControlWidget::cycleContextButton() void UASControlWidget::cycleContextButton()
{ {
UAS* mav = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uas)); UAS* uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uasID));
if (mav) if (uas) {
{ if (!armed) {
uas->armSystem();
if (!engineOn) ui.lastActionLabel->setText(QString("Arm %1").arg(uas->getUASName()));
{
mav->armSystem();
ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName()));
} else { } else {
mav->disarmSystem(); uas->disarmSystem();
ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName())); ui.lastActionLabel->setText(QString("Disarm %1").arg(uas->getUASName()));
} }
// Update state now and in several intervals when MAV might have changed state
updateStatemachine();
QTimer::singleShot(50, this, SLOT(updateStatemachine()));
QTimer::singleShot(200, this, SLOT(updateStatemachine()));
} }
} }
src/ui/uas/UASControlWidget.h
View file @ fb7d8413
...@@ -39,6 +39,18 @@ This file is part of the QGROUNDCONTROL project ...@@ -39,6 +39,18 @@ This file is part of the QGROUNDCONTROL project
#include <ui_UASControl.h> #include <ui_UASControl.h>
#include <UASInterface.h> #include <UASInterface.h>
enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
PX4_CUSTOM_MODE_SEATBELT,
PX4_CUSTOM_MODE_EASY,
PX4_CUSTOM_MODE_AUTO
};
struct full_mode_s {
uint8_t baseMode;
uint32_t customMode;
};
/** /**
* @brief Widget controlling one MAV * @brief Widget controlling one MAV
*/ */
...@@ -51,8 +63,10 @@ public: ...@@ -51,8 +63,10 @@ public:
~UASControlWidget(); ~UASControlWidget();
public slots: public slots:
/** @brief Update modes list for selected system */
void updateModesList();
/** @brief Set the system this widget controls */ /** @brief Set the system this widget controls */
void setUAS(UASInterface* uas); void setUAS(UASInterface* uasID);
/** @brief Trigger next context action */ /** @brief Trigger next context action */
void cycleContextButton(); void cycleContextButton();
/** @brief Set the operation mode of the MAV */ /** @brief Set the operation mode of the MAV */
...@@ -60,11 +74,11 @@ public slots: ...@@ -60,11 +74,11 @@ public slots:
/** @brief Transmit the operation mode */ /** @brief Transmit the operation mode */
void transmitMode(); void transmitMode();
/** @brief Update the mode */ /** @brief Update the mode */
void updateMode(int uas,QString mode,QString description); void updateMode(int uasID, QString mode, QString description);
/** @brief Update state */ /** @brief Update state */
void updateState(int state); void updateState(int state);
/** @brief Update internal state machine */ /** @brief Update internal state machine */
void updateStatemachine(); void updateArmText();
signals: signals:
void changedMode(int); void changedMode(int);
...@@ -75,9 +89,11 @@ protected slots: ...@@ -75,9 +89,11 @@ protected slots:
void setBackgroundColor(QColor color); void setBackgroundColor(QColor color);
protected: protected:
int uas; ///< Reference to the current uas int uasID; ///< Reference to the current uas
unsigned int uasMode; ///< Current uas mode struct full_mode_s *modesList; ///< Modes list for the current UAS
bool engineOn; ///< Engine state int modesNum; ///< Number of modes in list for the current UAS
int modeIdx; ///< Current uas mode index
bool armed; ///< Engine state
private: private:
Ui::uasControl ui; Ui::uasControl ui;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment