Commit f40d2066 authored by pixhawk's avatar pixhawk

Flight fixes

parent c2fe813e
......@@ -1459,6 +1459,7 @@ void UAS::writeParametersToStorage()
{
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
qDebug() << "SENT COMMAND" << MAV_CMD_PREFLIGHT_STORAGE;
sendMessage(msg);
}
......@@ -1492,7 +1493,6 @@ void UAS::enableAllDataTransmission(int rate)
mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
}
void UAS::enableRawSensorDataTransmission(int rate)
......@@ -1514,7 +1514,6 @@ void UAS::enableRawSensorDataTransmission(int rate)
mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
}
void UAS::enableExtendedSystemStatusTransmission(int rate)
......@@ -1536,7 +1535,6 @@ void UAS::enableExtendedSystemStatusTransmission(int rate)
mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
}
void UAS::enableRCChannelDataTransmission(int rate)
......@@ -1562,7 +1560,6 @@ void UAS::enableRCChannelDataTransmission(int rate)
mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
#endif
}
......@@ -1585,7 +1582,6 @@ void UAS::enableRawControllerDataTransmission(int rate)
mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
}
//void UAS::enableRawSensorFusionTransmission(int rate)
......@@ -1629,7 +1625,6 @@ void UAS::enablePositionTransmission(int rate)
mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
}
void UAS::enableExtra1Transmission(int rate)
......@@ -2122,7 +2117,7 @@ void UAS::shutdown()
{
// 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, 0, 2, 0, 0, 0, 0, 0, 0);
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;
}
......@@ -2165,33 +2160,34 @@ QString UAS::getShortModeTextFor(int id)
qDebug() << "MODE:" << modeid;
// BASE MODE DECODING
if (modeid & MAV_MODE_FLAG_DECODE_POSITION_AUTO)
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO)
{
mode = "AUTO";
mode += "AUTO";
}
else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_GUIDED)
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED)
{
mode = "GUIDED";
mode += "GUIDED";
}
else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE)
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE)
{
mode = "STABILIZED";
mode += "STABILIZED";
}
else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_TEST)
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_TEST)
{
mode = "TEST";
mode += "TEST";
}
else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_MANUAL)
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL)
{
mode = "MANUAL";
mode += "MANUAL";
}
else
if (modeid == 0)
{
mode = "PREFLIGHT";
}
// ARMED STATE DECODING
if (modeid & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
{
mode.prepend("A|");
}
......@@ -2201,7 +2197,7 @@ QString UAS::getShortModeTextFor(int id)
}
// HARDWARE IN THE LOOP DECODING
if (modeid & MAV_MODE_FLAG_DECODE_POSITION_HIL)
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL)
{
mode.prepend("HIL:");
}
......
......@@ -707,15 +707,15 @@ void QGCParamWidget::saveParameters()
{
case QVariant::Int:
paramValue = paramValue.arg(j.value().toInt());
paramType.arg(MAVLINK_TYPE_INT32_T);
paramType = paramType.arg(MAVLINK_TYPE_INT32_T);
break;
case QVariant::UInt:
paramValue = paramValue.arg(j.value().toUInt());
paramType.arg(MAVLINK_TYPE_UINT32_T);
paramType = paramType.arg(MAVLINK_TYPE_UINT32_T);
break;
case QMetaType::Float:
paramValue = paramValue.arg(j.value().toDouble(), 25, 'g', 12);
paramType.arg(MAVLINK_TYPE_FLOAT);
paramType = paramType.arg(MAVLINK_TYPE_FLOAT);
break;
default:
qCritical() << "ABORTED PARAM WRITE TO FILE, NO VALID QVARIANT TYPE" << j.value();
......
......@@ -50,12 +50,12 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
ui.modeComboBox->clear();
ui.modeComboBox->insertItem(MAV_MODE_PREFLIGHT, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT));
ui.modeComboBox->insertItem(MAV_MODE_STABILIZE_ARMED, UAS::getShortModeTextFor(MAV_MODE_STABILIZE_ARMED));
ui.modeComboBox->insertItem(MAV_MODE_MANUAL_ARMED, UAS::getShortModeTextFor(MAV_MODE_MANUAL_ARMED));
ui.modeComboBox->insertItem(MAV_MODE_GUIDED_DISARMED, UAS::getShortModeTextFor(MAV_MODE_GUIDED_ARMED));
ui.modeComboBox->insertItem(MAV_MODE_AUTO_ARMED, UAS::getShortModeTextFor(MAV_MODE_AUTO_ARMED));
ui.modeComboBox->insertItem(MAV_MODE_TEST_ARMED, UAS::getShortModeTextFor(MAV_MODE_TEST_ARMED));
ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT), MAV_MODE_PREFLIGHT);
ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED));
ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED);
ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED));
ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED));
ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED));
connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));
......@@ -157,7 +157,7 @@ void UASControlWidget::updateState(int state)
void UASControlWidget::setMode(int mode)
{
// Adapt context button mode
uasMode = mode;
uasMode = ui.modeComboBox->itemData(mode).toInt();
ui.modeComboBox->blockSignals(true);
ui.modeComboBox->setCurrentIndex(mode);
ui.modeComboBox->blockSignals(false);
......
Markdown is supported
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