Skip to content

Q
qgroundcontrol
Commit 4f293495 authored by Gus Grubba's avatar Gus Grubba
Browse files
Options

Initial work 

Add camera trigger
Add video start/stop/toggle recording
Add gimbal pitch and yaw
parent a732ca8b
Hide whitespace changes
Inline Side-by-side
Showing with 395 additions and 389 deletions
src/Joystick/Joystick.cc
View file @ 4f293495
......@@ -19,7 +19,7 @@
#include <QSettings>
QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog")
QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog")
QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog")
const char* Joystick::_settingsGroup = "Joysticks";
......@@ -48,56 +48,44 @@ const char* Joystick::_buttonActionNextStream = QT_TR_NOOP("Next Video S
const char* Joystick::_buttonActionPreviousStream = QT_TR_NOOP("Previous Video Stream");
const char* Joystick::_buttonActionNextCamera = QT_TR_NOOP("Next Camera");
const char* Joystick::_buttonActionPreviousCamera = QT_TR_NOOP("Previous Camera");
const char* Joystick::_buttonActionTriggerCamera = QT_TR_NOOP("Trigger Camera");
const char* Joystick::_buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video");
const char* Joystick::_buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video");
const char* Joystick::_buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video");
const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
"RollAxis",
"PitchAxis",
"YawAxis",
"ThrottleAxis"
"ThrottleAxis",
"GimbalPitchAxis",
"GimbalYawAxis"
};
int Joystick::_transmitterMode = 2;
Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager)
: _exitThread(false)
, _name(name)
: _name(name)
, _axisCount(axisCount)
, _buttonCount(buttonCount)
, _hatCount(hatCount)
, _hatButtonCount(4*hatCount)
, _hatButtonCount(4 * hatCount)
, _totalButtonCount(_buttonCount+_hatButtonCount)
, _calibrationMode(false)
, _rgAxisValues(nullptr)
, _rgCalibration(nullptr)
, _rgButtonValues(nullptr)
, _lastButtonBits(0)
, _throttleMode(ThrottleModeDownZero)
, _negativeThrust(false)
, _exponential(0)
, _accumulator(false)
, _deadband(false)
, _circleCorrection(true)
, _frequency(25.0f)
, _activeVehicle(nullptr)
, _pollingStartedForCalibration(false)
, _multiVehicleManager(multiVehicleManager)
{
_rgAxisValues = new int[_axisCount];
_rgCalibration = new Calibration_t[_axisCount];
_rgAxisValues = new int[_axisCount];
_rgCalibration = new Calibration_t[_axisCount];
_rgButtonValues = new bool[_totalButtonCount];
for (int i=0; i<_axisCount; i++) {
for (int i = 0; i < _axisCount; i++) {
_rgAxisValues[i] = 0;
}
for (int i=0; i<_totalButtonCount; i++) {
for (int i = 0; i < _totalButtonCount; i++) {
_rgButtonValues[i] = false;
}
_updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
_loadSettings();
connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
}
......@@ -135,13 +123,16 @@ void Joystick::_setDefaultCalibration(void) {
_rgFunctionAxis[yawFunction] = 0;
_rgFunctionAxis[throttleFunction] = 1;
_exponential = 0;
_accumulator = false;
_deadband = false;
_rgFunctionAxis[gimbalPitchFunction]= 4;
_rgFunctionAxis[gimbalYawFunction] = 5;
_exponential = 0;
_accumulator = false;
_deadband = false;
_frequency = 25.0f;
_throttleMode = ThrottleModeDownZero;
_calibrated = true;
_circleCorrection = false;
_frequency = 25.0f;
_throttleMode = ThrottleModeDownZero;
_calibrated = true;
_saveSettings();
}
......@@ -182,7 +173,7 @@ void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
}
}
void Joystick::_loadSettings(void)
void Joystick::_loadSettings()
{
QSettings settings;
......@@ -239,14 +230,13 @@ void Joystick::_loadSettings(void)
qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings;
}
for (int function=0; function<maxFunction; function++) {
for (int function = 0; function < maxFunction; function++) {
int functionAxis;
functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk);
badSettings |= !convertOk || (functionAxis == -1);
_rgFunctionAxis[function] = functionAxis;
badSettings |= !convertOk || (functionAxis == -1) || (functionAxis >= _axisCount);
if(functionAxis < _axisCount) {
_rgFunctionAxis[function] = functionAxis;
}
qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings;
}
......@@ -265,7 +255,7 @@ void Joystick::_loadSettings(void)
}
}
void Joystick::_saveSettings(void)
void Joystick::_saveSettings()
{
QSettings settings;
......@@ -429,7 +419,7 @@ void Joystick::run(void)
_update();
// Update axes
for (int axisIndex=0; axisIndex<_axisCount; axisIndex++) {
for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) {
int newAxisValue = _getAxis(axisIndex);
// Calibration code requires signal to be emitted even if value hasn't changed
_rgAxisValues[axisIndex] = newAxisValue;
......@@ -437,7 +427,7 @@ void Joystick::run(void)
}
// Update buttons
for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) {
for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) {
bool newButtonValue = _getButton(buttonIndex);
if (newButtonValue != _rgButtonValues[buttonIndex]) {
_rgButtonValues[buttonIndex] = newButtonValue;
......@@ -447,8 +437,8 @@ void Joystick::run(void)
// Update hat - append hat buttons to the end of the normal button list
int numHatButtons = 4;
for (int hatIndex=0; hatIndex<_hatCount; hatIndex++) {
for (int hatButtonIndex=0; hatButtonIndex<numHatButtons; hatButtonIndex++) {
for (int hatIndex = 0; hatIndex < _hatCount; hatIndex++) {
for (int hatButtonIndex = 0; hatButtonIndex<numHatButtons; hatButtonIndex++) {
// Create new index value that includes the normal button list
int rgButtonValueIndex = hatIndex*numHatButtons + hatButtonIndex + _buttonCount;
// Get hat value from joystick
......@@ -473,25 +463,29 @@ void Joystick::run(void)
axis = _rgFunctionAxis[throttleFunction];
float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband);
axis = _rgFunctionAxis[gimbalPitchFunction];
float gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
axis = _rgFunctionAxis[gimbalYawFunction];
float gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
if ( _accumulator ) {
static float throttle_accu = 0.f;
throttle_accu += throttle*(40/1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time)
throttle_accu = std::max(static_cast<float>(-1.f), std::min(throttle_accu, static_cast<float>(1.f)));
throttle = throttle_accu;
}
if ( _circleCorrection ) {
float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
float throttle_limited = std::max(static_cast<float>(-M_PI_4), std::min(throttle, static_cast<float>(M_PI_4)));
float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
float throttle_limited = std::max(static_cast<float>(-M_PI_4), std::min(throttle, static_cast<float>(M_PI_4)));
// Map from unit circle to linear range and limit
roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f));
pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f));
yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f));
roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f));
pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f));
yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f));
throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f));
}
......@@ -500,9 +494,9 @@ void Joystick::run(void)
//_exponential is set by a slider in joystickConfig.qml
// Calculate new RPY with exponential applied
roll = -_exponential*powf(roll,3) + (1+_exponential)*roll;
roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll;
pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch;
yaw = -_exponential*powf(yaw,3) + (1+_exponential)*yaw;
yaw = -_exponential*powf(yaw, 3) + (1+_exponential)*yaw;
}
// Adjust throttle to 0:1 range
......@@ -516,10 +510,10 @@ void Joystick::run(void)
// Set up button pressed information
quint16 newButtonBits = 0; // New set of button which are down
quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal
quint16 newButtonBits = 0; // New set of button which are down
quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal
for (int buttonIndex=0; buttonIndex<_totalButtonCount; buttonIndex++) {
for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
quint16 buttonBit = 1 << buttonIndex;
if (!_rgButtonValues[buttonIndex]) {
......@@ -529,7 +523,6 @@ void Joystick::run(void)
if (_lastButtonBits & buttonBit) {
// Button was up last time through, but is now down which indicates a button press
qCDebug(JoystickLog) << "button triggered" << buttonIndex;
QString buttonAction =_rgButtonActions[buttonIndex];
if (!buttonAction.isEmpty()) {
_buttonAction(buttonAction);
......@@ -542,15 +535,13 @@ void Joystick::run(void)
}
_lastButtonBits = newButtonBits;
qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle" << name() << roll << -pitch << yaw << throttle;
// NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub.
emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode());
emit manualControl(roll, -pitch, yaw, throttle, gimbalPitch, gimbalYaw, buttonPressedBits, _activeVehicle->joystickMode());
}
// Sleep. Update rate of joystick is by default 25 Hz
int mswait = (int)(1000.0f / _frequency);
unsigned long mswait = static_cast<unsigned long>(1000.0f / _frequency);
QGC::SLEEP::msleep(mswait);
}
......@@ -644,17 +635,15 @@ void Joystick::setFunctionAxis(AxisFunction_t function, int axis)
_calibrated = true;
_rgFunctionAxis[function] = axis;
_saveSettings();
emit calibratedChanged(_calibrated);
}
int Joystick::getFunctionAxis(AxisFunction_t function)
{
if (function < 0 || function >= maxFunction) {
if (static_cast<int>(function) < 0 || function >= maxFunction) {
qCWarning(JoystickLog) << "Invalid function" << function;
}
return _rgFunctionAxis[function];
}
......@@ -669,6 +658,10 @@ QStringList Joystick::actions(void)
list << _buttonActionZoomIn << _buttonActionZoomOut;
list << _buttonActionNextStream << _buttonActionPreviousStream;
list << _buttonActionNextCamera << _buttonActionPreviousCamera;
list << _buttonActionTriggerCamera;
list << _buttonActionStartVideoRecord;
list << _buttonActionStopVideoRecord;
list << _buttonActionToggleVideoRecord;
return list;
}
......@@ -846,6 +839,14 @@ void Joystick::_buttonAction(const QString& action)
emit stepStream(action == _buttonActionNextStream ? 1 : -1);
} else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) {
emit stepCamera(action == _buttonActionNextCamera ? 1 : -1);
} else if(action == _buttonActionTriggerCamera) {
emit triggerCamera();
} else if(action == _buttonActionStartVideoRecord) {
emit startVideoRecord();
} else if(action == _buttonActionStopVideoRecord) {
emit stopVideoRecord();
} else if(action == _buttonActionToggleVideoRecord) {
emit toggleVideoRecord();
} else {
qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
}
......
src/Joystick/Joystick.h
View file @ 4f293495
......@@ -49,6 +49,8 @@ public:
pitchFunction,
yawFunction,
throttleFunction,
gimbalPitchFunction,
gimbalYawFunction,
maxFunction
} AxisFunction_t;
......@@ -155,12 +157,14 @@ signals:
void circleCorrectionChanged(bool circleCorrection);
/// Signal containing new joystick information
/// @param roll Range is -1:1, negative meaning roll left, positive meaning roll right
/// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up
/// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right
/// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle
/// @param roll Range is -1:1, negative meaning roll left, positive meaning roll right
/// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up
/// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right
/// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle
/// @param gimbalPitch Range is -1:1
/// @param gimbalYaw Range is -1:1
/// @param mode See Vehicle::JoystickMode_t enum
void manualControl(float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode);
void manualControl (float roll, float pitch, float yaw, float throttle, float gimbalPitch, float gimbalYaw, quint16 buttons, int joystickMmode);
void buttonActionTriggered(int action);
......@@ -168,20 +172,24 @@ signals:
void stepZoom (int direction);
void stepCamera (int direction);
void stepStream (int direction);
void triggerCamera ();
void startVideoRecord ();
void stopVideoRecord ();
void toggleVideoRecord ();
protected:
void _setDefaultCalibration(void);
void _saveSettings(void);
void _loadSettings(void);
float _adjustRange(int value, Calibration_t calibration, bool withDeadbands);
void _buttonAction(const QString& action);
bool _validAxis(int axis);
bool _validButton(int button);
void _setDefaultCalibration ();
void _saveSettings ();
void _loadSettings ();
float _adjustRange (int value, Calibration_t calibration, bool withDeadbands);
void _buttonAction (const QString& action);
bool _validAxis (int axis);
bool _validButton (int button);
private:
virtual bool _open() = 0;
virtual void _close() = 0;
virtual bool _update() = 0;
virtual bool _open() = 0;
virtual void _close() = 0;
virtual bool _update() = 0;
virtual bool _getButton(int i) = 0;
virtual int _getAxis(int i) = 0;
......@@ -196,7 +204,23 @@ private:
protected:
bool _exitThread; ///< true: signal thread to exit
bool _exitThread = false; ///< true: signal thread to exit
bool _calibrationMode = false;
int* _rgAxisValues = nullptr;
Calibration_t* _rgCalibration = nullptr;
bool* _rgButtonValues = nullptr;
quint16 _lastButtonBits = 0;
ThrottleMode_t _throttleMode = ThrottleModeDownZero;
bool _negativeThrust = false;
float _exponential = 0;
bool _accumulator = false;
bool _deadband = false;
bool _circleCorrection = true;
float _frequency = 25.0f;
Vehicle* _activeVehicle = nullptr;
bool _pollingStartedForCalibration = false;
QString _name;
bool _calibrated;
......@@ -207,28 +231,10 @@ protected:
int _totalButtonCount;
static int _transmitterMode;
bool _calibrationMode;
int* _rgAxisValues;
Calibration_t* _rgCalibration;
int _rgFunctionAxis[maxFunction];
int _rgFunctionAxis[maxFunction] = {};
bool* _rgButtonValues;
QStringList _rgButtonActions;
quint16 _lastButtonBits;
ThrottleMode_t _throttleMode;
bool _negativeThrust;
float _exponential;
bool _accumulator;
bool _deadband;
bool _circleCorrection;
float _frequency;
Vehicle* _activeVehicle;
bool _pollingStartedForCalibration;
MultiVehicleManager* _multiVehicleManager;
......@@ -261,6 +267,10 @@ private:
static const char* _buttonActionPreviousStream;
static const char* _buttonActionNextCamera;
static const char* _buttonActionPreviousCamera;
static const char* _buttonActionTriggerCamera;
static const char* _buttonActionStartVideoRecord;
static const char* _buttonActionStopVideoRecord;
static const char* _buttonActionToggleVideoRecord;
private slots:
void _activeVehicleChanged(Vehicle* activeVehicle);
......
src/VehicleSetup/JoystickConfig.qml
View file @ 4f293495
......@@ -27,7 +27,7 @@ SetupPage {
pageName: qsTr("Joystick")
pageDescription: qsTr("Joystick Setup is used to configure a calibrate joysticks.")
readonly property real _maxButtons: 16
readonly property real _maxButtons: 64
Connections {
target: joystickManager
......@@ -67,12 +67,12 @@ SetupPage {
id: axisMonitorDisplayComponent
Item {
property int axisValue: 0
property int deadbandValue: 0
property bool narrowIndicator: false
property color deadbandColor: "#8c161a"
property int axisValue: 0
property int deadbandValue: 0
property bool narrowIndicator: false
property color deadbandColor: "#8c161a"
property color __barColor: qgcPal.windowShade
property color __barColor: qgcPal.windowShade
// Bar
Rectangle {
......@@ -93,9 +93,9 @@ SetupPage {
color: deadbandColor
visible: controller.deadbandToggle
property real _percentDeadband: ((2 * deadbandValue) / (32768.0 * 2))
property real _deadbandWidth: parent.width * _percentDeadband
property real _deadbandPosition: (parent.width - _deadbandWidth) / 2
property real _percentDeadband: ((2 * deadbandValue) / (32768.0 * 2))
property real _deadbandWidth: parent.width * _percentDeadband
property real _deadbandPosition: (parent.width - _deadbandWidth) / 2
}
// Center point
......@@ -183,14 +183,12 @@ SetupPage {
height: ScreenTools.defaultFontPixelHeight
width: 100
sourceComponent: axisMonitorDisplayComponent
property bool mapped: controller.rollAxisMapped
property bool reversed: controller.rollAxisReversed
}
Connections {
target: _activeJoystick
onManualControl: rollLoader.item.axisValue = roll*32768.0
}
}
......@@ -212,14 +210,12 @@ SetupPage {
height: ScreenTools.defaultFontPixelHeight
width: 100
sourceComponent: axisMonitorDisplayComponent
property bool mapped: controller.pitchAxisMapped
property bool reversed: controller.pitchAxisReversed
}
Connections {
target: _activeJoystick
onManualControl: pitchLoader.item.axisValue = pitch*32768.0
}
}
......@@ -241,14 +237,12 @@ SetupPage {
height: ScreenTools.defaultFontPixelHeight
width: 100
sourceComponent: axisMonitorDisplayComponent
property bool mapped: controller.yawAxisMapped
property bool reversed: controller.yawAxisReversed
}
Connections {
target: _activeJoystick
onManualControl: yawLoader.item.axisValue = yaw*32768.0
}
}
......@@ -270,17 +264,70 @@ SetupPage {
height: ScreenTools.defaultFontPixelHeight
width: 100
sourceComponent: axisMonitorDisplayComponent
property bool mapped: controller.throttleAxisMapped
property bool reversed: controller.throttleAxisReversed
}
Connections {
target: _activeJoystick
onManualControl: throttleLoader.item.axisValue = _activeJoystick.negativeThrust ? -throttle*32768.0 : (-2*throttle+1)*32768.0
}
}
Item {
width: parent.width
height: defaultTextHeight * 2
QGCLabel {
id: gimbalPitchLabel
width: ScreenTools.defaultFontPixelWidth * 10
text: qsTr("Gimbal Pitch")
}
Loader {
id: gimbalPitchLoader
anchors.left: gimbalPitchLabel.right
anchors.right: parent.right
height: ScreenTools.defaultFontPixelHeight
width: 100
sourceComponent: axisMonitorDisplayComponent
property bool mapped: controller.gimbalPitchAxisMapped
property bool reversed: controller.gimbalPitchAxisReversed
}
Connections {
target: _activeJoystick
onManualControl: gimbalPitchLoader.item.axisValue = gimbalPitch * 32768.0
}
}
Item {
width: parent.width
height: defaultTextHeight * 2
QGCLabel {
id: gimbalYawLabel
width: ScreenTools.defaultFontPixelWidth * 10
text: qsTr("Gimbal Yaw")
}
Loader {
id: gimbalYawLoader
anchors.left: gimbalYawLabel.right
anchors.right: parent.right
height: ScreenTools.defaultFontPixelHeight
width: 100
sourceComponent: axisMonitorDisplayComponent
property bool mapped: controller.gimbalYawAxisMapped
property bool reversed: controller.gimbalYawAxisReversed
}
Connections {
target: _activeJoystick
onManualControl: gimbalYawLoader.item.axisValue = gimbalYaw * 32768.0
}
}
} // Column - Attitude Control labels
// Command Buttons
......@@ -291,14 +338,12 @@ SetupPage {
QGCButton {
id: skipButton
text: qsTr("Skip")
onClicked: controller.skipButtonClicked()
}
QGCButton {
id: cancelButton
text: qsTr("Cancel")
onClicked: controller.cancelButtonClicked()
}
......@@ -306,7 +351,6 @@ SetupPage {
id: nextButton
primary: true
text: qsTr("Calibrate")
onClicked: controller.nextButtonClicked()
}
} // Row - Buttons
......
src/VehicleSetup/JoystickConfigController.cc
View file @ 4f293495
......@@ -44,23 +44,10 @@ const char* JoystickConfigController::_imagePitchUp = "joystickPitchUp.png
const char* JoystickConfigController::_imagePitchDown = "joystickPitchDown.png";
JoystickConfigController::JoystickConfigController(void)
: _activeJoystick(NULL)
, _transmitterMode(2)
, _currentStep(-1)
, _axisCount(0)
, _rgAxisInfo(NULL)
, _axisValueSave(NULL)
, _axisRawValue(NULL)
, _calState(calStateAxisWait)
, _statusText(NULL)
, _cancelButton(NULL)
, _nextButton(NULL)
, _skipButton(NULL)
, _joystickManager(qgcApp()->toolbox()->joystickManager())
: _joystickManager(qgcApp()->toolbox()->joystickManager())
{
connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &JoystickConfigController::_activeJoystickChanged);
_activeJoystickChanged(_joystickManager->activeJoystick());
_resetInternalCalibrationValues();
}
......@@ -89,61 +76,69 @@ JoystickConfigController::~JoystickConfigController()
/// @brief Returns the state machine entry for the specified state.
const JoystickConfigController::stateMachineEntry* JoystickConfigController::_getStateMachineEntry(int step)
{
static const char* msgBegin = "Allow all sticks to center as shown in diagram.\n\nClick Next to continue";
static const char* msgThrottleUp = "Move the Throttle stick all the way up and hold it there...";
static const char* msgThrottleDown = "Move the Throttle stick all the way down and hold it there...";
static const char* msgYawLeft = "Move the Yaw stick all the way to the left and hold it there...";
static const char* msgYawRight = "Move the Yaw stick all the way to the right and hold it there...";
static const char* msgRollLeft = "Move the Roll stick all the way to the left and hold it there...";
static const char* msgRollRight = "Move the Roll stick all the way to the right and hold it there...";
static const char* msgPitchDown = "Move the Pitch stick all the way down and hold it there...";
static const char* msgPitchUp = "Move the Pitch stick all the way up and hold it there...";
static const char* msgPitchCenter = "Allow the Pitch stick to move back to center...";
static const char* msgComplete = "All settings have been captured. Click Next to enable the joystick.";
static const char* msgBegin = "Allow all sticks to center as shown in diagram.\n\nClick Next to continue";
static const char* msgThrottleUp = "Move the Throttle stick all the way up and hold it there...";
static const char* msgThrottleDown = "Move the Throttle stick all the way down and hold it there...";
static const char* msgYawLeft = "Move the Yaw stick all the way to the left and hold it there...";
static const char* msgYawRight = "Move the Yaw stick all the way to the right and hold it there...";
static const char* msgRollLeft = "Move the Roll stick all the way to the left and hold it there...";
static const char* msgRollRight = "Move the Roll stick all the way to the right and hold it there...";
static const char* msgPitchDown = "Move the Pitch stick all the way down and hold it there...";
static const char* msgPitchUp = "Move the Pitch stick all the way up and hold it there...";
static const char* msgPitchCenter = "Allow the Pitch stick to move back to center...";
static const char* msgGimbalPitchDown = "Move the Gimbal Pitch control all the way down and hold it there...";
static const char* msgGimbalPitchUp = "Move the Gimbal Pitch control all the way up and hold it there...";
static const char* msgGimbalYawLeft = "Move the Gimbal Yaw control all the way to the left and hold it there...";
static const char* msgGimbalYawRight = "Move the Gimbal Yaw control all the way to the right and hold it there...";
static const char* msgComplete = "All settings have been captured. Click Next to enable the joystick.";
static const stateMachineEntry rgStateMachine[] = {
//Function
{ Joystick::maxFunction, msgBegin, _imageCenter, &JoystickConfigController::_inputCenterWaitBegin, &JoystickConfigController::_saveAllTrims, NULL },
{ Joystick::throttleFunction, msgThrottleUp, _imageThrottleUp, &JoystickConfigController::_inputStickDetect, NULL, NULL },
{ Joystick::throttleFunction, msgThrottleDown, _imageThrottleDown, &JoystickConfigController::_inputStickMin, NULL, NULL },
{ Joystick::yawFunction, msgYawRight, _imageYawRight, &JoystickConfigController::_inputStickDetect, NULL, NULL },
{ Joystick::yawFunction, msgYawLeft, _imageYawLeft, &JoystickConfigController::_inputStickMin, NULL, NULL },
{ Joystick::rollFunction, msgRollRight, _imageRollRight, &JoystickConfigController::_inputStickDetect, NULL, NULL },
{ Joystick::rollFunction, msgRollLeft, _imageRollLeft, &JoystickConfigController::_inputStickMin, NULL, NULL },
{ Joystick::pitchFunction, msgPitchUp, _imagePitchUp, &JoystickConfigController::_inputStickDetect, NULL, NULL },
{ Joystick::pitchFunction, msgPitchDown, _imagePitchDown, &JoystickConfigController::_inputStickMin, NULL, NULL },
{ Joystick::pitchFunction, msgPitchCenter, _imageCenter, &JoystickConfigController::_inputCenterWait, NULL, NULL },
{ Joystick::maxFunction, msgComplete, _imageCenter, NULL, &JoystickConfigController::_writeCalibration, NULL },
{ Joystick::maxFunction, msgBegin, _imageCenter, &JoystickConfigController::_inputCenterWaitBegin, &JoystickConfigController::_saveAllTrims, nullptr, 0 },
{ Joystick::throttleFunction, msgThrottleUp, _imageThrottleUp, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 0 },
{ Joystick::throttleFunction, msgThrottleDown, _imageThrottleDown, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 0 },
{ Joystick::yawFunction, msgYawRight, _imageYawRight, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 1 },
{ Joystick::yawFunction, msgYawLeft, _imageYawLeft, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 1 },
{ Joystick::rollFunction, msgRollRight, _imageRollRight, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 2 },
{ Joystick::rollFunction, msgRollLeft, _imageRollLeft, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 2 },
{ Joystick::pitchFunction, msgPitchUp, _imagePitchUp, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 3 },
{ Joystick::pitchFunction, msgPitchDown, _imagePitchDown, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 3 },
{ Joystick::pitchFunction, msgPitchCenter, _imageCenter, &JoystickConfigController::_inputCenterWait, nullptr, nullptr, 3 },
{ Joystick::gimbalPitchFunction, msgGimbalPitchUp, _imageThrottleUp, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 4 },
{ Joystick::gimbalPitchFunction, msgGimbalPitchDown, _imageThrottleDown, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 4 },
{ Joystick::gimbalYawFunction, msgGimbalYawRight, _imageYawRight, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 5 },
{ Joystick::gimbalYawFunction, msgGimbalYawLeft, _imageYawLeft, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 5 },
{ Joystick::maxFunction, msgComplete, _imageCenter, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 },
};
Q_ASSERT(step >=0 && step < (int)(sizeof(rgStateMachine) / sizeof(rgStateMachine[0])));
Q_ASSERT(step >=0 && step < static_cast<int>((sizeof(rgStateMachine) / sizeof(rgStateMachine[0]))));
return &rgStateMachine[step];
}
void JoystickConfigController::_advanceState(void)
void JoystickConfigController::_advanceState()
{
_currentStep++;
const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
while (state->channelID > _axisCount) {
_currentStep++;
state = _getStateMachineEntry(_currentStep);
}
_setupCurrentState();
}
/// @brief Sets up the state machine according to the current step from _currentStep.
void JoystickConfigController::_setupCurrentState(void)
void JoystickConfigController::_setupCurrentState()
{
const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
_statusText->setProperty("text", state->instructions);
_setHelpImage(state->image);
_stickDetectAxis = _axisNoAxis;
_stickDetectSettleStarted = false;
_calSaveCurrentValues();
_nextButton->setEnabled(state->nextFn != NULL);
_skipButton->setEnabled(state->skipFn != NULL);
_nextButton->setEnabled(state->nextFn != nullptr);
_skipButton->setEnabled(state->skipFn != nullptr);
}
void JoystickConfigController::_axisValueChanged(int axis, int value)
......@@ -152,16 +147,12 @@ void JoystickConfigController::_axisValueChanged(int axis, int value)
// We always update raw values
_axisRawValue[axis] = value;
emit axisValueChanged(axis, _axisRawValue[axis]);
//qCDebug(JoystickConfigControllerLog) << "Raw value" << axis << value;
if (_currentStep == -1) {
// Track the axis count by keeping track of how many axes we see
if (axis + 1 > (int)_axisCount) {
if (axis + 1 > static_cast<int>(_axisCount)) {
_axisCount = axis + 1;
}
}
if (_currentStep != -1) {
const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
Q_ASSERT(state);
......@@ -172,7 +163,7 @@ void JoystickConfigController::_axisValueChanged(int axis, int value)
}
}
void JoystickConfigController::nextButtonClicked(void)
void JoystickConfigController::nextButtonClicked()
{
if (_currentStep == -1) {
// Need to have enough channels
......@@ -189,10 +180,9 @@ void JoystickConfigController::nextButtonClicked(void)
}
}
void JoystickConfigController::skipButtonClicked(void)
void JoystickConfigController::skipButtonClicked()
{
Q_ASSERT(_currentStep != -1);
const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
Q_ASSERT(state);
Q_ASSERT(state->skipFn);
......@@ -210,20 +200,17 @@ bool JoystickConfigController::getDeadbandToggle() {
void JoystickConfigController::setDeadbandToggle(bool deadband) {
_activeJoystick->setDeadband(deadband);
_signalAllAttitudeValueChanges();
emit deadbandToggled(deadband);
}
void JoystickConfigController::_saveAllTrims(void)
void JoystickConfigController::_saveAllTrims()
{
// We save all trims as the first step. At this point no axes are mapped but it should still
// allow us to get good trims for the roll/pitch/yaw/throttle even though we don't know which
// axis they are yet. As we continue through the process the other axes will get their
// trims reset to correct values.
for (int i=0; i<_axisCount; i++) {
for (int i = 0; i < _axisCount; i++) {
qCDebug(JoystickConfigControllerLog) << "_saveAllTrims trim" << _axisRawValue[i];
_rgAxisInfo[i].axisTrim = _axisRawValue[i];
}
......@@ -233,7 +220,6 @@ void JoystickConfigController::_saveAllTrims(void)
void JoystickConfigController::_axisDeadbandChanged(int axis, int value)
{
value = abs(value)<_calValidMaxValue?abs(value):_calValidMaxValue;
_rgAxisInfo[axis].deadband = value;
emit axisDeadbandChanged(axis,value);
qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
......@@ -243,14 +229,11 @@ void JoystickConfigController::_axisDeadbandChanged(int axis, int value)
void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t function, int axis, int value)
{
Q_UNUSED(function);
//sensing deadband
if ((abs(value)*1.1f>_rgAxisInfo[axis].deadband)&&(_activeJoystick->deadband())) { //add 10% on top of existing deadband
_axisDeadbandChanged(axis,abs(value)*1.1f);
if ((abs(value) * 1.1f > _rgAxisInfo[axis].deadband) && (_activeJoystick->deadband())) { //add 10% on top of existing deadband
_axisDeadbandChanged(axis, static_cast<int>(abs(value) * 1.1f));
}
_nextButton->setEnabled(true);
// FIXME: Doesn't wait for center
}
......@@ -265,17 +248,13 @@ bool JoystickConfigController::_stickSettleComplete(int axis, int value)
if (abs(_stickDetectValue - value) > _calSettleDelta) {
// Stick is moving too much to consider stopped
qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
_stickDetectValue = value;
_stickDetectSettleStarted = false;
} else {
// Stick is still positioned within the specified small range
if (_stickDetectSettleStarted) {
// We have already started waiting
if (_stickDetectSettleElapsed.elapsed() > _stickDetectSettleMSecs) {
// Stick has stayed positioned in one place long enough, detection is complete.
qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete detection complete, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
......@@ -283,9 +262,7 @@ bool JoystickConfigController::_stickSettleComplete(int axis, int value)
}
} else {
// Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs
qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
_stickDetectSettleStarted = true;
_stickDetectSettleElapsed.start();
}
......@@ -310,12 +287,9 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
if (_stickDetectAxis == _axisNoAxis) {
// We have not detected enough movement on a axis yet
if (abs(_axisValueSave[axis] - value) > _calMoveDelta) {
// Stick has moved far enough to consider it as being selected for the function
qCDebug(JoystickConfigControllerLog) << "_inputStickDetect starting settle wait, function:axis:value" << function << axis << value;
// Setup up to detect stick being pegged to min or max value
_stickDetectAxis = axis;
_stickDetectInitialValue = value;
......@@ -324,25 +298,19 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
} else if (axis == _stickDetectAxis) {
if (_stickSettleComplete(axis, value)) {
AxisInfo* info = &_rgAxisInfo[axis];
// Stick detection is complete. Stick should be at max position.
// Map the axis to the function
_rgFunctionAxisMapping[function] = axis;
info->function = function;
// Axis should be at max value, if it is below initial set point the the axis is reversed.
info->reversed = value < _axisValueSave[axis];
if (info->reversed) {
_rgAxisInfo[axis].axisMin = value;
} else {
_rgAxisInfo[axis].axisMax = value;
}
qCDebug(JoystickConfigControllerLog) << "_inputStickDetect saving values, function:axis:value:reversed:_axisValueSave" << function << axis << value << info->reversed << _axisValueSave[axis];
_signalAllAttitudeValueChanges();
_advanceState();
}
}
......@@ -351,17 +319,14 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, int axis, int value)
{
qCDebug(JoystickConfigControllerLog) << "_inputStickMin function:axis:value" << function << axis << value;
if (!_validAxis(axis)) {
qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
return;
}
// We only care about the axis mapped to the function we are working on
if (_rgFunctionAxisMapping[function] != axis) {
return;
}
if (_stickDetectAxis == _axisNoAxis) {
// Setup up to detect stick being pegged to extreme position
if (_rgAxisInfo[axis].reversed) {
......@@ -381,19 +346,15 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function,
}
} else {
// We are waiting for the selected axis to settle out
if (_stickSettleComplete(axis, value)) {
AxisInfo* info = &_rgAxisInfo[axis];
// Stick detection is complete. Stick should be at min position.
if (info->reversed) {
_rgAxisInfo[axis].axisMax = value;
} else {
_rgAxisInfo[axis].axisMin = value;
}
qCDebug(JoystickConfigControllerLog) << "_inputStickMin saving values, function:axis:value:reversed" << function << axis << value << info->reversed;
_advanceState();
}
}
......@@ -402,7 +363,6 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function,
void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t function, int axis, int value)
{
qCDebug(JoystickConfigControllerLog) << "_inputCenterWait function:axis:value" << function << axis << value;
if (!_validAxis(axis)) {
qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
return;
......@@ -431,79 +391,71 @@ void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t functio
}
/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
void JoystickConfigController::_resetInternalCalibrationValues(void)
void JoystickConfigController::_resetInternalCalibrationValues()
{
// Set all raw axiss to not reversed and center point values
for (int i=0; i<_axisCount; i++) {
// Set all raw axis to not reversed and center point values
for (int i = 0; i < _axisCount; i++) {
struct AxisInfo* info = &_rgAxisInfo[i];
info->function = Joystick::maxFunction;
info->reversed = false;
info->deadband = 0;
emit axisDeadbandChanged(i,info->deadband);
info->axisMin = JoystickConfigController::_calCenterPoint;
info->axisMax = JoystickConfigController::_calCenterPoint;
info->axisMin = JoystickConfigController::_calCenterPoint;
info->axisMax = JoystickConfigController::_calCenterPoint;
info->axisTrim = JoystickConfigController::_calCenterPoint;
}
// Initialize attitude function mapping to function axis not set
for (size_t i=0; i<Joystick::maxFunction; i++) {
for (size_t i = 0; i < Joystick::maxFunction; i++) {
_rgFunctionAxisMapping[i] = _axisNoAxis;
}
_signalAllAttitudeValueChanges();
}
/// @brief Sets internal calibration values from the stored settings
void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
void JoystickConfigController::_setInternalCalibrationValuesFromSettings()
{
Joystick* joystick = _joystickManager->activeJoystick();
// Initialize all function mappings to not set
for (int i=0; i<_axisCount; i++) {
for (int i = 0; i < _axisCount; i++) {
struct AxisInfo* info = &_rgAxisInfo[i];
info->function = Joystick::maxFunction;
}
for (size_t i=0; i<Joystick::maxFunction; i++) {
for (size_t i = 0; i < Joystick::maxFunction; i++) {
_rgFunctionAxisMapping[i] = _axisNoAxis;
}
for (int axis=0; axis<_axisCount; axis++) {
for (int axis = 0; axis < _axisCount; axis++) {
struct AxisInfo* info = &_rgAxisInfo[axis];
Joystick::Calibration_t calibration = joystick->getCalibration(axis);
info->axisTrim = calibration.center;
info->axisMin = calibration.min;
info->axisMax = calibration.max;
info->reversed = calibration.reversed;
info->deadband = calibration.deadband;
info->axisTrim = calibration.center;
info->axisMin = calibration.min;
info->axisMax = calibration.max;
info->reversed = calibration.reversed;
info->deadband = calibration.deadband;
emit axisDeadbandChanged(axis,info->deadband);
qCDebug(JoystickConfigControllerLog) << "Read settings name:axis:min:max:trim:reversed" << joystick->name() << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed;
}
for (int function=0; function<Joystick::maxFunction; function++) {
for (int function = 0; function < Joystick::maxFunction; function++) {
int paramAxis;
paramAxis = joystick->getFunctionAxis((Joystick::AxisFunction_t)function);
if(paramAxis >= 0) {
paramAxis = joystick->getFunctionAxis(static_cast<Joystick::AxisFunction_t>(function));
if(paramAxis >= 0 && paramAxis < _axisCount) {
_rgFunctionAxisMapping[function] = paramAxis;
_rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
_rgAxisInfo[paramAxis].function = static_cast<Joystick::AxisFunction_t>(function);
}
}
_transmitterMode = joystick->getTXMode();
_signalAllAttitudeValueChanges();
}
/// @brief Validates the current settings against the calibration rules resetting values as necessary.
void JoystickConfigController::_validateCalibration(void)
void JoystickConfigController::_validateCalibration()
{
for (int chan = 0; chan<_axisCount; chan++) {
for (int chan = 0; chan < _axisCount; chan++) {
struct AxisInfo* info = &_rgAxisInfo[chan];
if (chan < _axisCount) {
// Validate Min/Max values. Although the axis appears as available we still may
// not have good min/max/trim values for it. Set to defaults if needed.
......@@ -513,7 +465,6 @@ void JoystickConfigController::_validateCalibration(void)
info->axisMax = _calDefaultMaxValue;
info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
}
switch (_rgAxisInfo[chan].function) {
case Joystick::throttleFunction:
case Joystick::yawFunction:
......@@ -543,31 +494,26 @@ void JoystickConfigController::_validateCalibration(void)
}
}
/// @brief Saves the rc calibration values to the board parameters.
void JoystickConfigController::_writeCalibration(void)
void JoystickConfigController::_writeCalibration()
{
Joystick* joystick = _joystickManager->activeJoystick();
_validateCalibration();
for (int axis=0; axis<_axisCount; axis++) {
for (int axis = 0; axis < _axisCount; axis++) {
Joystick::Calibration_t calibration;
struct AxisInfo* info = &_rgAxisInfo[axis];
calibration.center = info->axisTrim;
calibration.min = info->axisMin;
calibration.max = info->axisMax;
calibration.reversed = info->reversed;
calibration.deadband = info->deadband;
struct AxisInfo* info = &_rgAxisInfo[axis];
calibration.center = info->axisTrim;
calibration.min = info->axisMin;
calibration.max = info->axisMax;
calibration.reversed = info->reversed;
calibration.deadband = info->deadband;
joystick->setCalibration(axis, calibration);
}
// Write function mapping parameters
for (int function=0; function<Joystick::maxFunction; function++) {
joystick->setFunctionAxis((Joystick::AxisFunction_t)function, _rgFunctionAxisMapping[function]);
for (int function = 0; function < Joystick::maxFunction; function++) {
joystick->setFunctionAxis(static_cast<Joystick::AxisFunction_t>(function), _rgFunctionAxisMapping[function]);
}
_stopCalibration();
......@@ -580,40 +526,34 @@ void JoystickConfigController::_writeCalibration(void)
}
/// @brief Starts the calibration process
void JoystickConfigController::_startCalibration(void)
void JoystickConfigController::_startCalibration()
{
_activeJoystick->setCalibrationMode(true);
_resetInternalCalibrationValues();
_nextButton->setProperty("text", "Next");
_cancelButton->setEnabled(true);
_currentStep = 0;
_setupCurrentState();
emit calibratingChanged();
}
/// @brief Cancels the calibration process, setting things back to initial state.
void JoystickConfigController::_stopCalibration(void)
void JoystickConfigController::_stopCalibration()
{
_currentStep = -1;
_activeJoystick->setCalibrationMode(false);
_setInternalCalibrationValuesFromSettings();
_statusText->setProperty("text", "");
_nextButton->setProperty("text", tr("Calibrate"));
_nextButton->setEnabled(true);
_cancelButton->setEnabled(false);
_skipButton->setEnabled(false);
_setHelpImage(_imageCenter);
emit calibratingChanged();
}
/// @brief Saves the current axis values, so that we can detect when the use moves an input.
void JoystickConfigController::_calSaveCurrentValues(void)
void JoystickConfigController::_calSaveCurrentValues()
{
qCDebug(JoystickConfigControllerLog) << "_calSaveCurrentValues";
for (int i = 0; i < _axisCount; i++) {
......@@ -622,7 +562,7 @@ void JoystickConfigController::_calSaveCurrentValues(void)
}
/// @brief Set up the Save state of calibration.
void JoystickConfigController::_calSave(void)
void JoystickConfigController::_calSave()
{
_calState = calStateSave;
......@@ -668,34 +608,7 @@ void JoystickConfigController::_setHelpImage(const char* imageFile)
emit imageHelpChanged(file);
}
int JoystickConfigController::axisCount(void)
{
return _axisCount;
}
bool JoystickConfigController::rollAxisMapped(void)
{
return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis;
}
bool JoystickConfigController::pitchAxisMapped(void)
{
return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis;
}
bool JoystickConfigController::yawAxisMapped(void)
{
return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis;
}
bool JoystickConfigController::throttleAxisMapped(void)
{
return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis;
}
bool JoystickConfigController::rollAxisReversed(void)
bool JoystickConfigController::rollAxisReversed()
{
if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].reversed;
......@@ -704,7 +617,7 @@ bool JoystickConfigController::rollAxisReversed(void)
}
}
bool JoystickConfigController::pitchAxisReversed(void)
bool JoystickConfigController::pitchAxisReversed()
{
if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].reversed;
......@@ -713,7 +626,7 @@ bool JoystickConfigController::pitchAxisReversed(void)
}
}
bool JoystickConfigController::yawAxisReversed(void)
bool JoystickConfigController::yawAxisReversed()
{
if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].reversed;
......@@ -722,7 +635,7 @@ bool JoystickConfigController::yawAxisReversed(void)
}
}
bool JoystickConfigController::throttleAxisReversed(void)
bool JoystickConfigController::throttleAxisReversed()
{
if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].reversed;
......@@ -731,6 +644,24 @@ bool JoystickConfigController::throttleAxisReversed(void)
}
}
bool JoystickConfigController::gimbalPitchAxisReversed()
{
if (_rgFunctionAxisMapping[Joystick::gimbalPitchFunction] != _axisNoAxis) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::gimbalPitchFunction]].reversed;
} else {
return false;
}
}
bool JoystickConfigController::gimbalYawAxisReversed()
{
if (_rgFunctionAxisMapping[Joystick::gimbalYawFunction] != _axisNoAxis) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::gimbalYawFunction]].reversed;
} else {
return false;
}
}
void JoystickConfigController::setTransmitterMode(int mode)
{
if (mode > 0 && mode <= 4) {
......@@ -745,17 +676,21 @@ void JoystickConfigController::setTransmitterMode(int mode)
}
}
void JoystickConfigController::_signalAllAttitudeValueChanges(void)
void JoystickConfigController::_signalAllAttitudeValueChanges()
{
emit rollAxisMappedChanged(rollAxisMapped());
emit pitchAxisMappedChanged(pitchAxisMapped());
emit yawAxisMappedChanged(yawAxisMapped());
emit throttleAxisMappedChanged(throttleAxisMapped());
emit gimbalPitchAxisMappedChanged(gimbalPitchAxisMapped());
emit gimbalYawAxisMappedChanged(gimbalYawAxisMapped());
emit rollAxisReversedChanged(rollAxisReversed());
emit pitchAxisReversedChanged(pitchAxisReversed());
emit yawAxisReversedChanged(yawAxisReversed());
emit throttleAxisReversedChanged(throttleAxisReversed());
emit gimbalPitchAxisReversedChanged(pitchAxisReversed());
emit gimbalYawAxisReversedChanged(yawAxisReversed());
emit transmitterModeChanged(_transmitterMode);
}
......@@ -772,7 +707,7 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
delete[] _axisValueSave;
delete[] _axisRawValue;
_axisCount = 0;
_activeJoystick = NULL;
_activeJoystick = nullptr;
}
if (joystick) {
......@@ -781,10 +716,10 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
_stopCalibration();
}
_activeJoystick->setCalibrationMode(false);
_axisCount = _activeJoystick->axisCount();
_rgAxisInfo = new struct AxisInfo[_axisCount];
_axisValueSave = new int[_axisCount];
_axisRawValue = new int[_axisCount];
_axisCount = _activeJoystick->axisCount();
_rgAxisInfo = new struct AxisInfo[_axisCount];
_axisValueSave = new int[_axisCount];
_axisRawValue = new int[_axisCount];
_setInternalCalibrationValuesFromSettings();
connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
}
......
src/VehicleSetup/JoystickConfigController.h
View file @ 4f293495
......@@ -47,70 +47,84 @@ public:
Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
Q_PROPERTY(QQuickItem* skipButton MEMBER _skipButton)
Q_PROPERTY(bool rollAxisMapped READ rollAxisMapped NOTIFY rollAxisMappedChanged)
Q_PROPERTY(bool pitchAxisMapped READ pitchAxisMapped NOTIFY pitchAxisMappedChanged)
Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged)
Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged)
Q_PROPERTY(bool rollAxisMapped READ rollAxisMapped NOTIFY rollAxisMappedChanged)
Q_PROPERTY(bool pitchAxisMapped READ pitchAxisMapped NOTIFY pitchAxisMappedChanged)
Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged)
Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged)
Q_PROPERTY(int rollAxisReversed READ rollAxisReversed NOTIFY rollAxisReversedChanged)
Q_PROPERTY(int pitchAxisReversed READ pitchAxisReversed NOTIFY pitchAxisReversedChanged)
Q_PROPERTY(int yawAxisReversed READ yawAxisReversed NOTIFY yawAxisReversedChanged)
Q_PROPERTY(int throttleAxisReversed READ throttleAxisReversed NOTIFY throttleAxisReversedChanged)
Q_PROPERTY(bool deadbandToggle READ getDeadbandToggle WRITE setDeadbandToggle NOTIFY deadbandToggled)
Q_PROPERTY(bool gimbalPitchAxisMapped READ gimbalPitchAxisMapped NOTIFY gimbalPitchAxisMappedChanged)
Q_PROPERTY(bool gimbalYawAxisMapped READ gimbalYawAxisMapped NOTIFY gimbalYawAxisMappedChanged)
Q_PROPERTY(int rollAxisReversed READ rollAxisReversed NOTIFY rollAxisReversedChanged)
Q_PROPERTY(int pitchAxisReversed READ pitchAxisReversed NOTIFY pitchAxisReversedChanged)
Q_PROPERTY(int yawAxisReversed READ yawAxisReversed NOTIFY yawAxisReversedChanged)
Q_PROPERTY(int throttleAxisReversed READ throttleAxisReversed NOTIFY throttleAxisReversedChanged)
Q_PROPERTY(int gimbalPitchAxisReversed READ gimbalPitchAxisReversed NOTIFY gimbalPitchAxisReversedChanged)
Q_PROPERTY(int gimbalYawAxisReversed READ gimbalYawAxisReversed NOTIFY gimbalYawAxisReversedChanged)
Q_PROPERTY(bool deadbandToggle READ getDeadbandToggle WRITE setDeadbandToggle NOTIFY deadbandToggled)
Q_PROPERTY(int transmitterMode READ transmitterMode WRITE setTransmitterMode NOTIFY transmitterModeChanged)
Q_PROPERTY(QString imageHelp MEMBER _imageHelp NOTIFY imageHelpChanged)
Q_PROPERTY(bool calibrating READ calibrating NOTIFY calibratingChanged)
Q_INVOKABLE void cancelButtonClicked(void);
Q_INVOKABLE void skipButtonClicked(void);
Q_INVOKABLE void nextButtonClicked(void);
Q_INVOKABLE void start(void);
Q_INVOKABLE void setDeadbandValue(int axis, int value);
Q_INVOKABLE void cancelButtonClicked ();
Q_INVOKABLE void skipButtonClicked ();
Q_INVOKABLE void nextButtonClicked ();
Q_INVOKABLE void start ();
Q_INVOKABLE void setDeadbandValue (int axis, int value);
bool rollAxisMapped(void);
bool pitchAxisMapped(void);
bool yawAxisMapped(void);
bool throttleAxisMapped(void);
bool rollAxisReversed(void);
bool pitchAxisReversed(void);
bool yawAxisReversed(void);
bool throttleAxisReversed(void);
bool getDeadbandToggle(void);
void setDeadbandToggle(bool);
bool rollAxisMapped () { return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis; }
bool pitchAxisMapped () { return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis; }
bool yawAxisMapped () { return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis; }
bool throttleAxisMapped () { return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis; }
bool gimbalPitchAxisMapped () { return _rgFunctionAxisMapping[Joystick::gimbalPitchFunction] != _axisNoAxis; }
bool gimbalYawAxisMapped () { return _rgFunctionAxisMapping[Joystick::gimbalYawFunction] != _axisNoAxis; }
int axisCount(void);
bool rollAxisReversed ();
bool pitchAxisReversed ();
bool yawAxisReversed ();
bool throttleAxisReversed ();
bool gimbalPitchAxisReversed ();
bool gimbalYawAxisReversed ();
int transmitterMode(void) { return _transmitterMode; }
void setTransmitterMode(int mode);
bool getDeadbandToggle ();
void setDeadbandToggle (bool);
bool calibrating(void) { return _currentStep != -1; }
int axisCount () { return _axisCount; }
int transmitterMode () { return _transmitterMode; }
void setTransmitterMode (int mode);
bool calibrating () { return _currentStep != -1; }
signals:
void axisValueChanged(int axis, int value);
void axisDeadbandChanged(int axis, int value);
void axisValueChanged (int axis, int value);
void axisDeadbandChanged (int axis, int value);
void rollAxisMappedChanged(bool mapped);
void pitchAxisMappedChanged(bool mapped);
void yawAxisMappedChanged(bool mapped);
void throttleAxisMappedChanged(bool mapped);
void rollAxisMappedChanged (bool mapped);
void pitchAxisMappedChanged (bool mapped);
void yawAxisMappedChanged (bool mapped);
void throttleAxisMappedChanged (bool mapped);
void gimbalPitchAxisMappedChanged (bool mapped);
void gimbalYawAxisMappedChanged (bool mapped);
void rollAxisReversedChanged(bool reversed);
void pitchAxisReversedChanged(bool reversed);
void yawAxisReversedChanged(bool reversed);
void throttleAxisReversedChanged(bool reversed);
void rollAxisReversedChanged (bool reversed);
void pitchAxisReversedChanged (bool reversed);
void yawAxisReversedChanged (bool reversed);
void throttleAxisReversedChanged (bool reversed);
void gimbalPitchAxisReversedChanged (bool reversed);
void gimbalYawAxisReversedChanged (bool reversed);
void deadbandToggled(bool value);
void deadbandToggled (bool value);
void imageHelpChanged(QString source);
void transmitterModeChanged(int mode);
void calibratingChanged(void);
void imageHelpChanged (QString source);
void transmitterModeChanged (int mode);
void calibratingChanged ();
// @brief Signalled when in unit test mode and a message box should be displayed by the next button
void nextButtonMessageBoxDisplayed(void);
void nextButtonMessageBoxDisplayed ();
private slots:
void _activeJoystickChanged(Joystick* joystick);
......@@ -139,6 +153,7 @@ private:
inputFn rcInputFn;
buttonFn nextFn;
buttonFn skipFn;
int channelID;
};
/// @brief A set of information associated with a radio axis.
......@@ -151,45 +166,45 @@ private:
int deadband; ///< Deadband
};
Joystick* _activeJoystick;
Joystick* _activeJoystick = nullptr;
int _transmitterMode;
int _currentStep; ///< Current step of state machine
int _transmitterMode = 2;
int _currentStep = -1; ///< Current step of state machine
const struct stateMachineEntry* _getStateMachineEntry(int step);
void _advanceState(void);
void _setupCurrentState(void);
void _advanceState ();
void _setupCurrentState ();
bool _validAxis(int axis);
bool _validAxis (int axis);
void _inputCenterWaitBegin (Joystick::AxisFunction_t function, int axis, int value);
void _inputStickDetect (Joystick::AxisFunction_t function, int axis, int value);
void _inputStickMin (Joystick::AxisFunction_t function, int axis, int value);
void _inputCenterWait (Joystick::AxisFunction_t function, int axis, int value);
void _switchDetect(Joystick::AxisFunction_t function, int axis, int value, bool moveToNextStep);
void _switchDetect (Joystick::AxisFunction_t function, int axis, int value, bool moveToNextStep);
void _saveFlapsDown(void);
void _skipFlaps(void);
void _saveAllTrims(void);
void _saveFlapsDown ();
void _skipFlaps ();
void _saveAllTrims ();
bool _stickSettleComplete(int axis, int value);
bool _stickSettleComplete (int axis, int value);
void _validateCalibration(void);
void _writeCalibration(void);
void _resetInternalCalibrationValues(void);
void _setInternalCalibrationValuesFromSettings(void);
void _validateCalibration ();
void _writeCalibration ();
void _resetInternalCalibrationValues();
void _setInternalCalibrationValuesFromSettings();
void _startCalibration(void);
void _stopCalibration(void);
void _calSave(void);
void _startCalibration ();
void _stopCalibration ();
void _calSave ();
void _calSaveCurrentValues(void);
void _calSaveCurrentValues ();
void _setHelpImage(const char* imageFile);
void _signalAllAttitudeValueChanges(void);
void _signalAllAttitudeValueChanges();
// Member variables
......@@ -210,20 +225,21 @@ private:
int _rgFunctionAxisMapping[Joystick::maxFunction]; ///< Maps from joystick function to axis index. _axisMax indicates axis not set for this function.
static const int _attitudeControls = 5;
int _axisCount; ///< Number of actual joystick axes available
static const int _axisNoAxis = -1; ///< Signals no axis set
static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4
struct AxisInfo* _rgAxisInfo; ///< Information associated with each axis
int* _axisValueSave; ///< Saved values prior to detecting axis movement
int* _axisRawValue; ///< Current set of raw axis values
static const int _attitudeControls = 5;
int _axisCount = 0; ///< Number of actual joystick axes available
static const int _axisNoAxis = -1; ///< Signals no axis set
static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4
struct AxisInfo* _rgAxisInfo = nullptr; ///< Information associated with each axis
int* _axisValueSave = nullptr; ///< Saved values prior to detecting axis movement
int* _axisRawValue = nullptr; ///< Current set of raw axis values
enum calStates _calState; ///< Current calibration state
int _calStateCurrentAxis; ///< Current axis being worked on in calStateIdentify and calStateDetectInversion
bool _calStateAxisComplete; ///< Work associated with current axis is complete
int _calStateIdentifyOldMapping; ///< Previous mapping for axis being currently identified
int _calStateReverseOldMapping; ///< Previous mapping for axis being currently used to detect inversion
enum calStates _calState = calStateAxisWait; ///< Current calibration state
int _calStateCurrentAxis; ///< Current axis being worked on in calStateIdentify and calStateDetectInversion
bool _calStateAxisComplete; ///< Work associated with current axis is complete
int _calStateIdentifyOldMapping; ///< Previous mapping for axis being currently identified
int _calStateReverseOldMapping; ///< Previous mapping for axis being currently used to detect inversion
static const int _calCenterPoint;
static const int _calValidMinValue;
......@@ -241,11 +257,11 @@ private:
bool _stickDetectSettleStarted;
QTime _stickDetectSettleElapsed;
static const int _stickDetectSettleMSecs;
QQuickItem* _statusText;
QQuickItem* _cancelButton;
QQuickItem* _nextButton;
QQuickItem* _skipButton;
QQuickItem* _statusText = nullptr;
QQuickItem* _cancelButton = nullptr;
QQuickItem* _nextButton = nullptr;
QQuickItem* _skipButton = nullptr;
QString _imageHelp;
......
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