Commit 4224b476 authored by DonLakeFlyer's avatar DonLakeFlyer

parent 64eebbef
......@@ -6,6 +6,8 @@ Note: This file only contains high level features or important fixes.
### 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
* ArduPilot: Copter - Advanced Tuning support
* ArduPilot: Rover - Frame setup support
* ArduPilot: Copter - Update support to 3.5+
......
......@@ -14,13 +14,56 @@
#include <QVariant>
#include <QQmlProperty>
bool APMFlightModesComponentController::_typeRegistered = false;
APMFlightModesComponentController::APMFlightModesComponentController(void)
: _activeFlightMode(0)
, _channelCount(Vehicle::cMaxRcChannels)
: _rover (_vehicle->rover())
, _activeFlightMode (0)
, _channelCount (Vehicle::cMaxRcChannels)
, _simpleMode (SimpleModeStandard)
, _simpleModeFact (_rover ? nullptr : getParameterFact(-1, "SIMPLE"))
, _superSimpleModeFact (_rover ? nullptr : getParameterFact(-1, "SUPER_SIMPLE"))
, _simpleModesSupported (_simpleModeFact && _superSimpleModeFact)
{
if (!_typeRegistered) {
qmlRegisterUncreatableType<APMFlightModesComponentController>("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController", "Reference only");
}
_modeParamPrefix = _vehicle->rover() ? QStringLiteral("MODE") : QStringLiteral("FLTMODE");
_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 (!_rover) {
for (int i=0; i<_cFltModes; i++) {
Fact* fltModeFact = getParameterFact(-1, QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i+1));
_rgFltModeFacts.append(fltModeFact);
connect(fltModeFact, &Fact::rawValueChanged, this, &APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams);
}
_adjustSimpleModeEnabledFromParams();
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);
connect(_simpleModeFact, &Fact::rawValueChanged, this, &APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams);
connect(_superSimpleModeFact, &Fact::rawValueChanged, this, &APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams);
}
QStringList usedParams;
for (int i=1; i<7; i++) {
usedParams << QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i);
......@@ -29,8 +72,10 @@ APMFlightModesComponentController::APMFlightModesComponentController(void)
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);
}
......@@ -65,7 +110,7 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int
}
emit activeFlightModeChanged(_activeFlightMode);
for (int i=0; i<6; i++) {
for (int i=0; i<_cChannelOptions; i++) {
_rgChannelOptionEnabled[i] = QVariant(false);
channelValue = pwmValues[i+6];
if (channelValue > 1800) {
......@@ -74,3 +119,79 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int
}
emit channelOptionEnabledChanged();
}
void APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode(void)
{
int newSimpleModeValue = 0;
int newSuperSimpleModeValue = 0;
if (_simpleMode == SimpleModeSimple) {
newSimpleModeValue = _allSimpleBits;
} else if (_simpleMode == SimpleModeSuperSimple) {
newSuperSimpleModeValue = _allSimpleBits;
}
_simpleModeFact->setRawValue(newSimpleModeValue);
_superSimpleModeFact->setRawValue(newSuperSimpleModeValue);
}
void APMFlightModesComponentController::setSimpleMode(int fltModeIndex, bool enabled)
{
if (fltModeIndex < _cFltModes) {
int fltMode = _rgFltModeFacts[fltModeIndex]->rawValue().toInt();
if (fltMode < _cSimpleModeBits) {
uint8_t mode = static_cast<uint8_t>(_simpleModeFact->rawValue().toInt());
if (enabled) {
mode |= 1 << fltMode;
} else {
mode &= ~(1 << fltMode);
}
_simpleModeFact->setRawValue(mode);
}
}
}
void APMFlightModesComponentController::setSuperSimpleMode(int fltModeIndex, bool enabled)
{
if (fltModeIndex < _cFltModes) {
int fltMode = _rgFltModeFacts[fltModeIndex]->rawValue().toInt();
if (fltMode < _cSimpleModeBits) {
uint8_t mode = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toInt());
if (enabled) {
mode |= 1 << fltMode;
} else {
mode &= ~(1 << fltMode);
}
_superSimpleModeFact->setRawValue(mode);
}
}
}
void APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams(void)
{
uint8_t simpleMode = static_cast<uint8_t>(_simpleModeFact->rawValue().toUInt());
uint8_t superSimpleMode = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toUInt());
for (int fltModeIndex=0; fltModeIndex<_cFltModes; fltModeIndex++) {
_simpleModeEnabled[fltModeIndex] = false;
_superSimpleModeEnabled[fltModeIndex] = false;
}
for (int fltModeBit=0; fltModeBit<_cSimpleModeBits; fltModeBit++) {
for (int fltModeIndex=0; fltModeIndex<_cFltModes; fltModeIndex++) {
int fltModeValue = _rgFltModeFacts[fltModeIndex]->rawValue().toInt();
uint8_t bitSet = 1 << fltModeBit;
bool simpleModeBitEnabled = !!(simpleMode & bitSet) && fltModeValue == fltModeBit;
bool superSimpleModeBitEnabled = !!(superSimpleMode & bitSet) && fltModeValue == fltModeBit;
if (simpleModeBitEnabled) {
_simpleModeEnabled[fltModeIndex] = true;
}
if (superSimpleModeBitEnabled) {
_superSimpleModeEnabled[fltModeIndex] = true;
}
}
}
emit simpleModeEnabledChanged();
emit superSimpleModeEnabledChanged();
}
......@@ -27,30 +27,67 @@ class APMFlightModesComponentController : public FactPanelController
Q_OBJECT
public:
enum SimpleModeValues {
SimpleModeStandard = 0,
SimpleModeSimple,
SimpleModeSuperSimple,
SimpleModeCustom
};
Q_ENUM(SimpleModeValues)
APMFlightModesComponentController(void);
Q_PROPERTY(QString modeParamPrefix MEMBER _modeParamPrefix CONSTANT)
Q_PROPERTY(QString modeChannelParam MEMBER _modeChannelParam CONSTANT)
Q_PROPERTY(int activeFlightMode READ activeFlightMode NOTIFY activeFlightModeChanged)
Q_PROPERTY(int channelCount MEMBER _channelCount CONSTANT)
Q_PROPERTY(QVariantList channelOptionEnabled READ channelOptionEnabled NOTIFY channelOptionEnabledChanged)
Q_PROPERTY(QString modeParamPrefix MEMBER _modeParamPrefix CONSTANT)
Q_PROPERTY(QString modeChannelParam MEMBER _modeChannelParam CONSTANT)
Q_PROPERTY(int activeFlightMode READ activeFlightMode NOTIFY activeFlightModeChanged)
Q_PROPERTY(int channelCount MEMBER _channelCount CONSTANT)
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; }
QVariantList channelOptionEnabled(void) const { return _rgChannelOptionEnabled; }
signals:
void activeFlightModeChanged(int activeFlightMode);
void channelOptionEnabledChanged(void);
void activeFlightModeChanged (int activeFlightMode);
void channelOptionEnabledChanged (void);
void simpleModeChanged (int simpleMode);
void simpleModeEnabledChanged (void);
void superSimpleModeEnabledChanged (void);
private slots:
void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);
void _rcChannelsChanged (int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);
void _updateSimpleParamsFromSimpleMode (void);
void _adjustSimpleModeEnabledFromParams (void);
private:
bool _rover;
QString _modeParamPrefix;
QString _modeChannelParam;
int _activeFlightMode;
int _channelCount;
QVariantList _rgChannelOptionEnabled;
QStringList _simpleModeNames;
int _simpleMode;
QList<Fact*> _rgFltModeFacts;
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 bool _typeRegistered;
};
#endif
......@@ -516,7 +516,7 @@ SetupPage {
QGCLabel {
width: parent.width
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 {
......@@ -537,7 +537,7 @@ SetupPage {
}
QGCButton {
text: "Calculate"
text: qsTr("Calculate And Set")
onClicked: {
var measuredVoltageValue = parseFloat(measuredVoltage.text)
......@@ -599,7 +599,7 @@ SetupPage {
}
QGCButton {
text: "Calculate"
text: qsTr("Calculate And Set")
onClicked: {
var measuredCurrentValue = parseFloat(measuredCurrent.text)
......
......@@ -3399,7 +3399,23 @@ QString Vehicle::firmwareVersionTypeString(void) const
void Vehicle::rebootVehicle()
{
_autoDisconnect = true;
sendMavCommand(_defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f);
mavlink_message_t msg;
mavlink_command_long_t cmd;
memset(&cmd, 0, sizeof(cmd));
cmd.target_system = _id;
cmd.target_component = _defaultComponentId;
cmd.command = MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN;
cmd.confirmation = 0;
cmd.param1 = 1;
cmd.param2 = cmd.param3 = cmd.param4 = cmd.param5 = cmd.param6 = cmd.param7 = 0;
mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
_mavlink->getComponentId(),
priorityLink()->mavlinkChannel(),
&msg,
&cmd);
sendMessageOnLink(priorityLink(), msg);
}
void Vehicle::setSoloFirmware(bool soloFirmware)
......
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