Commit 81e44f00 authored by Gus Grubba's avatar Gus Grubba

Merge branch 'master' of https://github.com/mavlink/qgroundcontrol into foo

# Conflicts:
#	src/QmlControls/PIDTuning.qml
#	src/ui/MainWindow.cc
parents 0e7f614d df6e1a51
...@@ -6,6 +6,8 @@ Note: This file only contains high level features or important fixes. ...@@ -6,6 +6,8 @@ Note: This file only contains high level features or important fixes.
### 3.6.0 - Daily Build ### 3.6.0 - Daily Build
* ArduPilot: Copter - Add suppor for Simple and Super Simple flight modes
* ArduPilot: Flight Mode setup - Switch Options were not showing up for all firmware revs
* ArduCopter: Add PID Tuning page to Tuning Setup * ArduCopter: Add PID Tuning page to Tuning Setup
* ArduPilot: Copter - Advanced Tuning support * ArduPilot: Copter - Advanced Tuning support
* ArduPilot: Rover - Frame setup support * ArduPilot: Rover - Frame setup support
......
...@@ -76,6 +76,7 @@ public class QGCActivity extends QtActivity ...@@ -76,6 +76,7 @@ public class QGCActivity extends QtActivity
private static final String ACTION_USB_PERMISSION = "org.mavlink.qgroundcontrol.action.USB_PERMISSION"; private static final String ACTION_USB_PERMISSION = "org.mavlink.qgroundcontrol.action.USB_PERMISSION";
private static PendingIntent _usbPermissionIntent = null; private static PendingIntent _usbPermissionIntent = null;
private TaiSync taiSync = null; private TaiSync taiSync = null;
private Timer probeAccessoriesTimer = null;
public static Context m_context; public static Context m_context;
...@@ -225,7 +226,15 @@ public class QGCActivity extends QtActivity ...@@ -225,7 +226,15 @@ public class QGCActivity extends QtActivity
IntentFilter accessoryFilter = new IntentFilter(ACTION_USB_PERMISSION); IntentFilter accessoryFilter = new IntentFilter(ACTION_USB_PERMISSION);
filter.addAction(UsbManager.ACTION_USB_ACCESSORY_DETACHED); filter.addAction(UsbManager.ACTION_USB_ACCESSORY_DETACHED);
registerReceiver(mOpenAccessoryReceiver, accessoryFilter); registerReceiver(mOpenAccessoryReceiver, accessoryFilter);
probeAccessories();
probeAccessoriesTimer = new Timer();
probeAccessoriesTimer.schedule(new TimerTask() {
@Override
public void run()
{
probeAccessories();
}
}, 0, 3000);
} catch(Exception e) { } catch(Exception e) {
Log.e(TAG, "Exception: " + e); Log.e(TAG, "Exception: " + e);
} }
...@@ -243,6 +252,9 @@ public class QGCActivity extends QtActivity ...@@ -243,6 +252,9 @@ public class QGCActivity extends QtActivity
@Override @Override
protected void onDestroy() protected void onDestroy()
{ {
if (probeAccessoriesTimer != null) {
probeAccessoriesTimer.cancel();
}
unregisterReceiver(mOpenAccessoryReceiver); unregisterReceiver(mOpenAccessoryReceiver);
try { try {
if(_wakeLock != null) { if(_wakeLock != null) {
...@@ -706,22 +718,26 @@ public class QGCActivity extends QtActivity ...@@ -706,22 +718,26 @@ public class QGCActivity extends QtActivity
} }
} }
Object probeAccessoriesLock = new Object();
private void probeAccessories() private void probeAccessories()
{ {
final PendingIntent pendingIntent = PendingIntent.getBroadcast(this, 0, new Intent(ACTION_USB_PERMISSION), 0); final PendingIntent pendingIntent = PendingIntent.getBroadcast(this, 0, new Intent(ACTION_USB_PERMISSION), 0);
new Thread(new Runnable() { new Thread(new Runnable() {
public void run() { public void run() {
Log.i(TAG, "probeAccessories"); synchronized(openAccessoryLock) {
UsbAccessory[] accessories = _usbManager.getAccessoryList(); Log.i(TAG, "probeAccessories");
if (accessories != null) { UsbAccessory[] accessories = _usbManager.getAccessoryList();
for (UsbAccessory usbAccessory : accessories) { if (accessories != null) {
if (_usbManager.hasPermission(usbAccessory)) { for (UsbAccessory usbAccessory : accessories) {
openAccessory(usbAccessory); if (_usbManager.hasPermission(usbAccessory)) {
} else { openAccessory(usbAccessory);
Log.i(TAG, "requestPermission"); } else {
_usbManager.requestPermission(usbAccessory, pendingIntent); Log.i(TAG, "requestPermission");
_usbManager.requestPermission(usbAccessory, pendingIntent);
}
} }
} }
} }
} }
}).start(); }).start();
......
...@@ -201,7 +201,7 @@ void APMAutoPilotPlugin::_checkForBadCubeBlack(void) ...@@ -201,7 +201,7 @@ void APMAutoPilotPlugin::_checkForBadCubeBlack(void)
if (paramMgr->parameterExists(-1, paramAcc3) && paramMgr->getParameter(-1, paramAcc3)->rawValue().toInt() == 0 && if (paramMgr->parameterExists(-1, paramAcc3) && paramMgr->getParameter(-1, paramAcc3)->rawValue().toInt() == 0 &&
paramMgr->parameterExists(-1, paramGyr3) && paramMgr->getParameter(-1, paramGyr3)->rawValue().toInt() == 0 && paramMgr->parameterExists(-1, paramGyr3) && paramMgr->getParameter(-1, paramGyr3)->rawValue().toInt() == 0 &&
paramMgr->parameterExists(-1, paramEnableMask) && paramMgr->getParameter(-1, paramEnableMask)->rawValue().toInt() >= 7) { paramMgr->parameterExists(-1, paramEnableMask) && paramMgr->getParameter(-1, paramEnableMask)->rawValue().toInt() >= 7) {
qgcApp()->showMessage(tr("WARNING: The flight board you are using has a critical service bulletin against ti which advise against flying. https://discuss.cubepilot.org/t/sb-0000002-critical-service-bulletin-for-cubes-purchased-between-january-2019-to-present-do-not-fly/406")); qgcApp()->showMessage(tr("WARNING: The flight board you are using has a critical service bulletin against it which advises against flying. For details see: https://discuss.cubepilot.org/t/sb-0000002-critical-service-bulletin-for-cubes-purchased-between-january-2019-to-present-do-not-fly/406"));
} }
} }
......
...@@ -14,13 +14,50 @@ ...@@ -14,13 +14,50 @@
#include <QVariant> #include <QVariant>
#include <QQmlProperty> #include <QQmlProperty>
bool APMFlightModesComponentController::_typeRegistered = false;
const char* APMFlightModesComponentController::_simpleParamName = "SIMPLE";
const char* APMFlightModesComponentController::_superSimpleParamName = "SUPER_SIMPLE";
APMFlightModesComponentController::APMFlightModesComponentController(void) APMFlightModesComponentController::APMFlightModesComponentController(void)
: _activeFlightMode(0) : _activeFlightMode (0)
, _channelCount(Vehicle::cMaxRcChannels) , _channelCount (Vehicle::cMaxRcChannels)
, _simpleMode (SimpleModeStandard)
, _simpleModeFact (parameterExists(-1, _simpleParamName) ? getParameterFact(-1, _simpleParamName) : nullptr)
, _superSimpleModeFact (parameterExists(-1, _superSimpleParamName) ? getParameterFact(-1, _superSimpleParamName) : nullptr)
, _simpleModesSupported (_simpleModeFact && _superSimpleModeFact)
{ {
if (!_typeRegistered) {
qmlRegisterUncreatableType<APMFlightModesComponentController>("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController", "Reference only");
}
_modeParamPrefix = _vehicle->rover() ? QStringLiteral("MODE") : QStringLiteral("FLTMODE"); _modeParamPrefix = _vehicle->rover() ? QStringLiteral("MODE") : QStringLiteral("FLTMODE");
_modeChannelParam = _vehicle->rover() ? QStringLiteral("MODE_CH") : QStringLiteral("FLTMODE_CH"); _modeChannelParam = _vehicle->rover() ? QStringLiteral("MODE_CH") : QStringLiteral("FLTMODE_CH");
_simpleModeNames << tr("Off") << tr("Simple") << tr("Super-Simple") << tr("Custom");
for (int i=0; i<_cFltModes; i++) {
_simpleModeEnabled.append(QVariant(false));
_superSimpleModeEnabled.append(QVariant(false));
}
if (_simpleModesSupported) {
_setupSimpleModeEnabled();
uint8_t simpleModeValue = static_cast<uint8_t>(_simpleModeFact->rawValue().toUInt());
uint8_t superSimpleModeValue = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toUInt());
if (simpleModeValue == 0 && superSimpleModeValue == 0) {
_simpleMode = SimpleModeStandard;
} else if (simpleModeValue == _allSimpleBits && superSimpleModeValue == 0) {
_simpleMode = SimpleModeSimple;
} else if (simpleModeValue == 0 && superSimpleModeValue == _allSimpleBits) {
_simpleMode = SimpleModeSuperSimple;
} else {
_simpleMode = SimpleModeCustom;
}
connect(this, &APMFlightModesComponentController::simpleModeChanged, this, &APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode);
}
QStringList usedParams; QStringList usedParams;
for (int i=1; i<7; i++) { for (int i=1; i<7; i++) {
usedParams << QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i); usedParams << QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i);
...@@ -29,8 +66,10 @@ APMFlightModesComponentController::APMFlightModesComponentController(void) ...@@ -29,8 +66,10 @@ APMFlightModesComponentController::APMFlightModesComponentController(void)
return; return;
} }
_rgChannelOptionEnabled << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false); for (int i=0; i<_cChannelOptions; i++) {
_rgChannelOptionEnabled.append(QVariant(false));
}
connect(_vehicle, &Vehicle::rcChannelsChanged, this, &APMFlightModesComponentController::_rcChannelsChanged); connect(_vehicle, &Vehicle::rcChannelsChanged, this, &APMFlightModesComponentController::_rcChannelsChanged);
} }
...@@ -65,7 +104,7 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int ...@@ -65,7 +104,7 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int
} }
emit activeFlightModeChanged(_activeFlightMode); emit activeFlightModeChanged(_activeFlightMode);
for (int i=0; i<6; i++) { for (int i=0; i<_cChannelOptions; i++) {
_rgChannelOptionEnabled[i] = QVariant(false); _rgChannelOptionEnabled[i] = QVariant(false);
channelValue = pwmValues[i+6]; channelValue = pwmValues[i+6];
if (channelValue > 1800) { if (channelValue > 1800) {
...@@ -74,3 +113,66 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int ...@@ -74,3 +113,66 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int
} }
emit channelOptionEnabledChanged(); emit channelOptionEnabledChanged();
} }
void APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode(void)
{
int newSimpleModeValue = 0;
int newSuperSimpleModeValue = 0;
if (_simpleMode == SimpleModeSimple) {
newSimpleModeValue = _allSimpleBits;
} else if (_simpleMode == SimpleModeSuperSimple) {
newSuperSimpleModeValue = _allSimpleBits;
}
for (int i=0; i<_cFltModes; i++) {
_simpleModeEnabled[i] = false;
_superSimpleModeEnabled[i] = false;
}
emit simpleModeEnabledChanged();
emit superSimpleModeEnabledChanged();
_simpleModeFact->setRawValue(newSimpleModeValue);
_superSimpleModeFact->setRawValue(newSuperSimpleModeValue);
}
void APMFlightModesComponentController::setSimpleMode(int fltModeIndex, bool enabled)
{
if (fltModeIndex < _cFltModes) {
uint8_t mode = static_cast<uint8_t>(_simpleModeFact->rawValue().toInt());
if (enabled) {
mode |= 1 << fltModeIndex;
} else {
mode &= ~(1 << fltModeIndex);
}
_simpleModeFact->setRawValue(mode);
}
}
void APMFlightModesComponentController::setSuperSimpleMode(int fltModeIndex, bool enabled)
{
if (fltModeIndex < _cFltModes) {
uint8_t mode = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toInt());
if (enabled) {
mode |= 1 << fltModeIndex;
} else {
mode &= ~(1 << fltModeIndex);
}
_superSimpleModeFact->setRawValue(mode);
}
}
void APMFlightModesComponentController::_setupSimpleModeEnabled(void)
{
uint8_t simpleMode = static_cast<uint8_t>(_simpleModeFact->rawValue().toUInt());
uint8_t superSimpleMode = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toUInt());
for (int i=0; i<_cFltModes; i++) {
uint8_t bitSet = static_cast<uint8_t>(1 << i);
_simpleModeEnabled[i] = !!(simpleMode & bitSet);
_superSimpleModeEnabled[i] = !!(superSimpleMode & bitSet);
}
emit simpleModeEnabledChanged();
emit superSimpleModeEnabledChanged();
}
...@@ -27,30 +27,68 @@ class APMFlightModesComponentController : public FactPanelController ...@@ -27,30 +27,68 @@ class APMFlightModesComponentController : public FactPanelController
Q_OBJECT Q_OBJECT
public: public:
enum SimpleModeValues {
SimpleModeStandard = 0,
SimpleModeSimple,
SimpleModeSuperSimple,
SimpleModeCustom
};
Q_ENUM(SimpleModeValues)
APMFlightModesComponentController(void); APMFlightModesComponentController(void);
Q_PROPERTY(QString modeParamPrefix MEMBER _modeParamPrefix CONSTANT) Q_PROPERTY(QString modeParamPrefix MEMBER _modeParamPrefix CONSTANT)
Q_PROPERTY(QString modeChannelParam MEMBER _modeChannelParam CONSTANT) Q_PROPERTY(QString modeChannelParam MEMBER _modeChannelParam CONSTANT)
Q_PROPERTY(int activeFlightMode READ activeFlightMode NOTIFY activeFlightModeChanged) Q_PROPERTY(int activeFlightMode READ activeFlightMode NOTIFY activeFlightModeChanged)
Q_PROPERTY(int channelCount MEMBER _channelCount CONSTANT) Q_PROPERTY(int channelCount MEMBER _channelCount CONSTANT)
Q_PROPERTY(QVariantList channelOptionEnabled READ channelOptionEnabled NOTIFY channelOptionEnabledChanged) Q_PROPERTY(QVariantList channelOptionEnabled READ channelOptionEnabled NOTIFY channelOptionEnabledChanged)
Q_PROPERTY(bool simpleModesSupported MEMBER _simpleModesSupported CONSTANT)
Q_PROPERTY(QStringList simpleModeNames MEMBER _simpleModeNames CONSTANT)
Q_PROPERTY(int simpleMode MEMBER _simpleMode NOTIFY simpleModeChanged)
Q_PROPERTY(QVariantList simpleModeEnabled MEMBER _simpleModeEnabled NOTIFY simpleModeEnabledChanged)
Q_PROPERTY(QVariantList superSimpleModeEnabled MEMBER _superSimpleModeEnabled NOTIFY superSimpleModeEnabledChanged)
Q_INVOKABLE void setSimpleMode(int fltModeIndex, bool enabled);
Q_INVOKABLE void setSuperSimpleMode(int fltModeIndex, bool enabled);
int activeFlightMode(void) const { return _activeFlightMode; } int activeFlightMode(void) const { return _activeFlightMode; }
QVariantList channelOptionEnabled(void) const { return _rgChannelOptionEnabled; } QVariantList channelOptionEnabled(void) const { return _rgChannelOptionEnabled; }
signals: signals:
void activeFlightModeChanged(int activeFlightMode); void activeFlightModeChanged (int activeFlightMode);
void channelOptionEnabledChanged(void); void channelOptionEnabledChanged (void);
void simpleModeChanged (int simpleMode);
void simpleModeEnabledChanged (void);
void superSimpleModeEnabledChanged (void);
private slots: private slots:
void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]); void _rcChannelsChanged (int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);
void _updateSimpleParamsFromSimpleMode (void);
void _setupSimpleModeEnabled (void);
private: private:
QString _modeParamPrefix; QString _modeParamPrefix;
QString _modeChannelParam; QString _modeChannelParam;
int _activeFlightMode; int _activeFlightMode;
int _channelCount; int _channelCount;
QVariantList _rgChannelOptionEnabled; QVariantList _rgChannelOptionEnabled;
QStringList _simpleModeNames;
int _simpleMode;
Fact* _simpleModeFact;
Fact* _superSimpleModeFact;
bool _simpleModesSupported;
QVariantList _simpleModeEnabled;
QVariantList _superSimpleModeEnabled;
static const uint8_t _allSimpleBits = 0x3F;
static const int _cChannelOptions = 10;
static const int _cSimpleModeBits = 8;
static const int _cFltModes = 6;
static const char* _simpleParamName;
static const char* _superSimpleParamName;
static bool _typeRegistered;
}; };
#endif #endif
...@@ -515,7 +515,7 @@ SetupPage { ...@@ -515,7 +515,7 @@ SetupPage {
QGCLabel { QGCLabel {
width: parent.width width: parent.width
wrapMode: Text.WordWrap wrapMode: Text.WordWrap
text: qsTr("Measure battery voltage using an external voltmeter and enter the value below. Click Calculate to set the new voltage multiplier.") text: qsTr("Measure battery voltage using an external voltmeter and enter the value below. Click Calculate to set the new adjusted voltage multiplier.")
} }
Grid { Grid {
...@@ -536,7 +536,7 @@ SetupPage { ...@@ -536,7 +536,7 @@ SetupPage {
} }
QGCButton { QGCButton {
text: "Calculate" text: qsTr("Calculate And Set")
onClicked: { onClicked: {
var measuredVoltageValue = parseFloat(measuredVoltage.text) var measuredVoltageValue = parseFloat(measuredVoltage.text)
...@@ -598,7 +598,7 @@ SetupPage { ...@@ -598,7 +598,7 @@ SetupPage {
} }
QGCButton { QGCButton {
text: "Calculate" text: qsTr("Calculate And Set")
onClicked: { onClicked: {
var measuredCurrentValue = parseFloat(measuredCurrent.text) var measuredCurrentValue = parseFloat(measuredCurrent.text)
......
...@@ -692,16 +692,25 @@ SetupPage { ...@@ -692,16 +692,25 @@ SetupPage {
calValid: controller.orientationCalDownSideDone calValid: controller.orientationCalDownSideDone
calInProgress: controller.orientationCalDownSideInProgress calInProgress: controller.orientationCalDownSideInProgress
calInProgressText: controller.orientationCalDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalDownSideRotate ? "qrc:///qmlimages/VehicleDownRotate.png" : "qrc:///qmlimages/VehicleDown.png" imageSource: "qrc:///qmlimages/VehicleDown.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
height: parent.indicatorHeight height: parent.indicatorHeight
visible: controller.orientationCalUpsideDownSideVisible visible: controller.orientationCalLeftSideVisible
calValid: controller.orientationCalUpsideDownSideDone calValid: controller.orientationCalLeftSideDone
calInProgress: controller.orientationCalUpsideDownSideInProgress calInProgress: controller.orientationCalLeftSideInProgress
calInProgressText: controller.orientationCalUpsideDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalLeftSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalUpsideDownSideRotate ? "qrc:///qmlimages/VehicleUpsideDownRotate.png" : "qrc:///qmlimages/VehicleUpsideDown.png" imageSource: "qrc:///qmlimages/VehicleLeft.png"
}
VehicleRotationCal {
width: parent.indicatorWidth
height: parent.indicatorHeight
visible: controller.orientationCalRightSideVisible
calValid: controller.orientationCalRightSideDone
calInProgress: controller.orientationCalRightSideInProgress
calInProgressText: controller.orientationCalRightSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: "qrc:///qmlimages/VehicleRight.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
...@@ -710,7 +719,7 @@ SetupPage { ...@@ -710,7 +719,7 @@ SetupPage {
calValid: controller.orientationCalNoseDownSideDone calValid: controller.orientationCalNoseDownSideDone
calInProgress: controller.orientationCalNoseDownSideInProgress calInProgress: controller.orientationCalNoseDownSideInProgress
calInProgressText: controller.orientationCalNoseDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalNoseDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalNoseDownSideRotate ? "qrc:///qmlimages/VehicleNoseDownRotate.png" : "qrc:///qmlimages/VehicleNoseDown.png" imageSource: "qrc:///qmlimages/VehicleNoseDown.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
...@@ -719,25 +728,16 @@ SetupPage { ...@@ -719,25 +728,16 @@ SetupPage {
calValid: controller.orientationCalTailDownSideDone calValid: controller.orientationCalTailDownSideDone
calInProgress: controller.orientationCalTailDownSideInProgress calInProgress: controller.orientationCalTailDownSideInProgress
calInProgressText: controller.orientationCalTailDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalTailDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalTailDownSideRotate ? "qrc:///qmlimages/VehicleTailDownRotate.png" : "qrc:///qmlimages/VehicleTailDown.png" imageSource: "qrc:///qmlimages/VehicleTailDown.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
height: parent.indicatorHeight height: parent.indicatorHeight
visible: controller.orientationCalLeftSideVisible visible: controller.orientationCalUpsideDownSideVisible
calValid: controller.orientationCalLeftSideDone calValid: controller.orientationCalUpsideDownSideDone
calInProgress: controller.orientationCalLeftSideInProgress calInProgress: controller.orientationCalUpsideDownSideInProgress
calInProgressText: controller.orientationCalLeftSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalUpsideDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalLeftSideRotate ? "qrc:///qmlimages/VehicleLeftRotate.png" : "qrc:///qmlimages/VehicleLeft.png" imageSource: "qrc:///qmlimages/VehicleUpsideDown.png"
}
VehicleRotationCal {
width: parent.indicatorWidth
height: parent.indicatorHeight
visible: controller.orientationCalRightSideVisible
calValid: controller.orientationCalRightSideDone
calInProgress: controller.orientationCalRightSideInProgress
calInProgressText: controller.orientationCalRightSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalRightSideRotate ? "qrc:///qmlimages/VehicleRightRotate.png" : "qrc:///qmlimages/VehicleRight.png"
} }
} }
} }
......
...@@ -227,7 +227,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone ...@@ -227,7 +227,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_startLogCalibration(); _startLogCalibration();
uint8_t compassBits = 0; uint8_t compassBits = 0;
if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID"))->rawValue().toInt() > 0 && if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID"))->rawValue().toInt() > 0 &&
getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE"))->rawValue().toBool()) { getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE"))->rawValue().toBool()) {
compassBits |= 1 << 0; compassBits |= 1 << 0;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1"; qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1";
} else { } else {
...@@ -236,7 +236,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone ...@@ -236,7 +236,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_rgCompassCalFitness[0] = 0; _rgCompassCalFitness[0] = 0;
} }
if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID2"))->rawValue().toInt() > 0 && if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID2"))->rawValue().toInt() > 0 &&
getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE2"))->rawValue().toBool()) { getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE2"))->rawValue().toBool()) {
compassBits |= 1 << 1; compassBits |= 1 << 1;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2"; qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2";
} else { } else {
...@@ -245,7 +245,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone ...@@ -245,7 +245,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_rgCompassCalFitness[1] = 0; _rgCompassCalFitness[1] = 0;
} }
if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID3"))->rawValue().toInt() > 0 && if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID3"))->rawValue().toInt() > 0 &&
getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE3"))->rawValue().toBool()) { getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE3"))->rawValue().toBool()) {
compassBits |= 1 << 2; compassBits |= 1 << 2;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3"; qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3";
} else { } else {
...@@ -335,11 +335,118 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, ...@@ -335,11 +335,118 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
return; return;
} }
if (text.startsWith(QLatin1Literal("PreArm:")) || text.startsWith(QLatin1Literal("EKF")) QString originalMessageText = text;
|| text.startsWith(QLatin1Literal("Arm")) || text.startsWith(QLatin1Literal("Initialising"))) { text = text.toLower();
QStringList hidePrefixList = { QStringLiteral("prearm:"), QStringLiteral("ekf"), QStringLiteral("arm"), QStringLiteral("initialising") };
for (const QString& hidePrefix: hidePrefixList) {
if (text.startsWith(hidePrefix)) {
return;
}
}
if (_calTypeInProgress == CalTypeAccel) {
if (text == QStringLiteral("place vehicle level and press any key.")) {
_startVisualCalibration();
_cancelButton->setEnabled(false);
// Reset all progress indication
_orientationCalDownSideDone = false;
_orientationCalUpsideDownSideDone = false;
_orientationCalLeftSideDone = false;
_orientationCalRightSideDone = false;
_orientationCalTailDownSideDone = false;
_orientationCalNoseDownSideDone = false;
_orientationCalDownSideInProgress = false;
_orientationCalUpsideDownSideInProgress = false;
_orientationCalLeftSideInProgress = false;
_orientationCalRightSideInProgress = false;
_orientationCalNoseDownSideInProgress = false;
_orientationCalTailDownSideInProgress = false;
// Reset all visibility
_orientationCalDownSideVisible = false;
_orientationCalUpsideDownSideVisible = false;
_orientationCalLeftSideVisible = false;
_orientationCalRightSideVisible = false;
_orientationCalTailDownSideVisible = false;
_orientationCalNoseDownSideVisible = false;
_calTypeInProgress = CalTypeAccel;
_orientationCalDownSideVisible = true;
_orientationCalUpsideDownSideVisible = true;
_orientationCalLeftSideVisible = true;
_orientationCalRightSideVisible = true;
_orientationCalTailDownSideVisible = true;
_orientationCalNoseDownSideVisible = true;
emit orientationCalSidesDoneChanged();
emit orientationCalSidesVisibleChanged();
emit orientationCalSidesInProgressChanged();
_updateAndEmitShowOrientationCalArea(true);
}
QString placeVehicle("place vehicle ");
if (_calTypeInProgress == CalTypeAccel && text.startsWith(placeVehicle)) {
text = text.right(text.length() - placeVehicle.length());
if (text.startsWith("level")) {
_orientationCalDownSideInProgress = true;
_nextButton->setEnabled(true);
} else if (text.startsWith("on its left")) {
_orientationCalDownSideDone = true;
_orientationCalDownSideInProgress = false;
_orientationCalLeftSideInProgress = true;
_progressBar->setProperty("value", (qreal)(17 / 100.0));
} else if (text.startsWith("on its right")) {
_orientationCalLeftSideDone = true;
_orientationCalLeftSideInProgress = false;
_orientationCalRightSideInProgress = true;
_progressBar->setProperty("value", (qreal)(34 / 100.0));
} else if (text.startsWith("nose down")) {
_orientationCalRightSideDone = true;
_orientationCalRightSideInProgress = false;
_orientationCalNoseDownSideInProgress = true;
_progressBar->setProperty("value", (qreal)(51 / 100.0));
} else if (text.startsWith("nose up")) {
_orientationCalNoseDownSideDone = true;
_orientationCalNoseDownSideInProgress = false;
_orientationCalTailDownSideInProgress = true;
_progressBar->setProperty("value", (qreal)(68 / 100.0));
} else if (text.startsWith("on its back")) {
_orientationCalTailDownSideDone = true;
_orientationCalTailDownSideInProgress = false;
_orientationCalUpsideDownSideInProgress = true;
_progressBar->setProperty("value", (qreal)(85 / 100.0));
}
_orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation and press Next when ready"));
emit orientationCalSidesDoneChanged();
emit orientationCalSidesInProgressChanged();
emit orientationCalSidesRotateChanged();
}
}
_appendStatusLog(originalMessageText);
qCDebug(APMSensorsComponentControllerLog) << originalMessageText << severity;
if (text.contains(QLatin1String("calibration successful"))) {
_stopCalibration(StopCalibrationSuccess);
return; return;
} }
if (text.startsWith(QStringLiteral("calibration cancelled"))) {
_stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
return;
}
if (text.startsWith(QStringLiteral("calibration failed"))) {
_stopCalibration(StopCalibrationFailed);
return;
}
#if 0
if (text.contains(QLatin1Literal("progress <"))) { if (text.contains(QLatin1Literal("progress <"))) {