From 7c2c126b18df71bc333d083fb81fbfced91115e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 10:25:10 +0200 Subject: [PATCH] Introduce manual control and position setpoints from Joystick inputs --- src/input/JoystickInput.cc | 173 ++++++++++++++++++++++++++++++--- src/input/JoystickInput.h | 64 +++++++++++- src/uas/UAS.cc | 236 ++++++++++++++++++++++++++++++++++++--------- src/uas/UAS.h | 3 +- src/ui/JoystickAxis.h | 2 + src/ui/JoystickWidget.cc | 24 +++++ src/ui/JoystickWidget.h | 2 + src/ui/JoystickWidget.ui | 12 ++- 8 files changed, 448 insertions(+), 68 deletions(-) diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index ff6039c..cd60e0c 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -7,6 +7,7 @@ * * @author Lorenz Meier * @author Andreas Romer + * @author Julian Oes getActiveUAS()); @@ -74,6 +71,7 @@ void JoystickInput::loadGeneralSettings() settings.beginGroup("JOYSTICK_INPUT"); isEnabled = settings.value("ENABLED", false).toBool(); joystickName = settings.value("JOYSTICK_NAME", "").toString(); + mode = (JOYSTICK_MODE)settings.value("JOYSTICK_MODE", JOYSTICK_MODE_ATTITUDE).toInt(); settings.endGroup(); } @@ -109,6 +107,8 @@ void JoystickInput::loadJoystickSettings() // Now that both the autopilot and system type are available, update some references. QMap* joystickAxesInverted = &joystickSettings[autopilotType][systemType].axesInverted; QMap* joystickAxesLimited = &joystickSettings[autopilotType][systemType].axesLimited; + QMap* joystickAxesMinRange = &joystickSettings[autopilotType][systemType].axesMaxRange; + QMap* joystickAxesMaxRange = &joystickSettings[autopilotType][systemType].axesMinRange; QMap* joystickButtonActions = &joystickSettings[autopilotType][systemType].buttonActions; // Read back the joystickAxesInverted QList one element at a time. @@ -133,6 +133,28 @@ void JoystickInput::loadJoystickSettings() } settings.endArray(); + // Read back the joystickAxesMinRange QList one element at a time. + axesStored = settings.beginReadArray("AXES_MIN_RANGE"); + for (int k = 0; k < axesStored; k++) + { + settings.setArrayIndex(k); + int index = settings.value("INDEX", 0).toInt(); + float min = settings.value("MIN_RANGE", false).toFloat(); + joystickAxesMinRange->insert(index, min); + } + settings.endArray(); + + // Read back the joystickAxesMaxRange QList one element at a time. + axesStored = settings.beginReadArray("AXES_MAX_RANGE"); + for (int k = 0; k < axesStored; k++) + { + settings.setArrayIndex(k); + int index = settings.value("INDEX", 0).toInt(); + float max = settings.value("MAX_RANGE", false).toFloat(); + joystickAxesMaxRange->insert(index, max); + } + settings.endArray(); + // Read back the button->action mapping. int buttonsStored = settings.beginReadArray("BUTTONS_ACTIONS"); for (int k = 0; k < buttonsStored; k++) @@ -159,6 +181,7 @@ void JoystickInput::storeGeneralSettings() const settings.beginGroup("JOYSTICK_INPUT"); settings.setValue("ENABLED", isEnabled); settings.setValue("JOYSTICK_NAME", joystickName); + settings.setValue("JOYSTICK_MODE", mode); settings.endGroup(); } @@ -196,6 +219,8 @@ void JoystickInput::storeJoystickSettings() const // Now that both the autopilot and system type are available, update some references. QMapIterator joystickAxesInverted(joystickSettings[autopilotType][systemType].axesInverted); QMapIterator joystickAxesLimited(joystickSettings[autopilotType][systemType].axesLimited); + QMapIterator joystickAxesMinRange(joystickSettings[autopilotType][systemType].axesMaxRange); + QMapIterator joystickAxesMaxRange(joystickSettings[autopilotType][systemType].axesMinRange); QMapIterator joystickButtonActions(joystickSettings[autopilotType][systemType].buttonActions); settings.beginWriteArray("AXES_INVERTED"); @@ -215,6 +240,32 @@ void JoystickInput::storeJoystickSettings() const } settings.endArray(); + settings.beginWriteArray("AXES_MIN_RANGE"); + k = 0; + while (joystickAxesMinRange.hasNext()) + { + joystickAxesMinRange.next(); + float min = joystickAxesMinRange.value(); + settings.setArrayIndex(k++); + int index = joystickAxesMinRange.key(); + settings.setValue("INDEX", index); + settings.setValue("MIN_RANGE", min); + } + settings.endArray(); + + settings.beginWriteArray("AXES_MAX_RANGE"); + k = 0; + while (joystickAxesMaxRange.hasNext()) + { + joystickAxesMaxRange.next(); + float max = joystickAxesMaxRange.value(); + settings.setArrayIndex(k++); + int index = joystickAxesMaxRange.key(); + settings.setValue("INDEX", index); + settings.setValue("MAX_RANGE", max); + } + settings.endArray(); + settings.beginWriteArray("AXES_LIMITED"); k = 0; while (joystickAxesLimited.hasNext()) @@ -268,7 +319,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas) tmp = dynamic_cast(this->uas); if(tmp) { - disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16))); + disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8))); disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); } uasCanReverse = false; @@ -284,7 +335,8 @@ void JoystickInput::setActiveUAS(UASInterface* uas) if (this->uas && (tmp = dynamic_cast(this->uas))) { - connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16))); + connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8))); + qDebug() << "connected joystick"; connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); uasCanReverse = tmp->systemCanReverse(); @@ -397,26 +449,47 @@ void JoystickInput::run() // This is generally not used for controlling a vehicle, but a UI representation, so it being slightly off is fine. // Here we map the joystick axis value into the initial range of [0:1]. float axisValue = SDL_JoystickGetAxis(joystick, i); + + // during calibration save min and max values + if (isCalibrating) + { + if (joystickSettings[autopilotType][systemType].axesMinRange.value(i) > axisValue) + { + joystickSettings[autopilotType][systemType].axesMinRange[i] = axisValue; + } + + if (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) < axisValue) + { + joystickSettings[autopilotType][systemType].axesMaxRange[i] = axisValue; + } + } + if (joystickSettings[autopilotType][systemType].axesInverted[i]) { - axisValue = (axisValue - calibrationNegative[i]) / (calibrationPositive[i] - calibrationNegative[i]); + axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMinRange.value(i)) / + (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) - + joystickSettings[autopilotType][systemType].axesMinRange.value(i)); } else { - axisValue = (axisValue - calibrationPositive[i]) / (calibrationNegative[i] - calibrationPositive[i]); + axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMaxRange.value(i)) / + (joystickSettings[autopilotType][systemType].axesMinRange.value(i) - + joystickSettings[autopilotType][systemType].axesMaxRange.value(i)); } axisValue = 1.0f - axisValue; // For non-throttle axes or if the UAS can reverse, go ahead and convert this into the range [-1:1]. - if (uasCanReverse || throttleAxis != i) + + //if (uasCanReverse || throttleAxis != i) + // don't take into account if UAS can reverse. This means to reverse position but not throttle + // therefore deactivated for now + if (throttleAxis != i) { axisValue = axisValue * 2.0f - 1.0f; } - // Otherwise if this vehicle can only go forward, but the axis is limited to only the positive range, - // scale this so the negative values are ignored for this axis and it's clamped to [0:1]. + // Otherwise if this vehicle can only go forward, scale it to [0:1]. else if (throttleAxis == i && joystickSettings[autopilotType][systemType].axesLimited.value(i)) { - axisValue = axisValue * 2.0f - 1.0f; if (axisValue < 0.0f) { axisValue = 0.0f; @@ -476,11 +549,11 @@ void JoystickInput::run() float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:numeric_limits::quiet_NaN(); float yaw = yawAxis > -1?joystickAxes[yawAxis]:numeric_limits::quiet_NaN(); float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:numeric_limits::quiet_NaN(); - emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons); + emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons, mode); } - // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms) - QGC::SLEEP::msleep(20); + // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms) + QGC::SLEEP::msleep(40); } } @@ -531,6 +604,38 @@ void JoystickInput::setActiveJoystick(int id) emit joystickSettingsChanged(); } +void JoystickInput::setCalibrating(bool active) +{ + if (active) + { + setEnabled(false); + isCalibrating = true; + + // set range small so that limits can be re-found + for (int i = 0; i < joystickNumAxes; i++) + { + joystickSettings[autopilotType][systemType].axesMinRange[i] = -10.0f; + joystickSettings[autopilotType][systemType].axesMaxRange[i] = 10.0f; + } + + } else { + + // store calibration values + storeJoystickSettings(); + + qDebug() << "Calibration result:"; + for (int i = 0; i < joystickNumAxes; i++) + { + qDebug() << i << ": " << + joystickSettings[autopilotType][systemType].axesMinRange[i] << + " - " << + joystickSettings[autopilotType][systemType].axesMaxRange[i]; + } + setEnabled(true); + isCalibrating = false; + } +} + void JoystickInput::setAxisMapping(int axis, JOYSTICK_INPUT_MAPPING newMapping) { switch (newMapping) @@ -589,6 +694,24 @@ void JoystickInput::setAxisRangeLimit(int axis, bool limitRange) } } +void JoystickInput::setAxisRangeLimitMin(int axis, float min) +{ + if (axis < joystickNumAxes) + { + joystickSettings[autopilotType][systemType].axesMinRange[axis] = min; + storeJoystickSettings(); + } +} + +void JoystickInput::setAxisRangeLimitMax(int axis, float max) +{ + if (axis < joystickNumAxes) + { + joystickSettings[autopilotType][systemType].axesMaxRange[axis] = max; + storeJoystickSettings(); + } +} + void JoystickInput::setButtonAction(int button, int action) { if (button < joystickNumButtons) @@ -625,6 +748,24 @@ bool JoystickInput::getRangeLimitForAxis(int axis) const return false; } +float JoystickInput::getAxisRangeLimitMinForAxis(int axis) const +{ + if (axis < joystickNumAxes) + { + return joystickSettings[autopilotType][systemType].axesMinRange.value(axis); + } + return sdlJoystickMin; +} + +float JoystickInput::getAxisRangeLimitMaxForAxis(int axis) const +{ + if (axis < joystickNumAxes) + { + return joystickSettings[autopilotType][systemType].axesMaxRange.value(axis); + } + return sdlJoystickMax; +} + int JoystickInput::getActionForButton(int button) const { if (button < joystickNumButtons && joystickSettings[autopilotType][systemType].buttonActions.contains(button)) diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 3bca5cb..4b89fe7 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -37,6 +37,7 @@ This file is part of the PIXHAWK project * * @author Lorenz Meier * @author Andreas Romer + * @author Julian Oes */ #ifndef _JOYSTICKINPUT_H_ @@ -56,6 +57,8 @@ This file is part of the PIXHAWK project struct JoystickSettings { QMap axesInverted; ///< Whether each axis should be used inverted from what was reported. QMap axesLimited; ///< Whether each axis should be limited to only the positive range. Currently this only applies to the throttle axis, but is kept generic here to possibly support other axes. + QMap axesMaxRange; ///< The maximum values per axis + QMap axesMinRange; ///< The minimum values per axis QMap buttonActions; ///< The index of the action associated with every button. }; Q_DECLARE_METATYPE(JoystickSettings) @@ -87,6 +90,19 @@ public: }; /** + * @brief The JOYSTICK_MODE enum stores the values for each item in the mode combobox. + * This should match the order of items in the mode combobox in JoystickWidget.ui. + */ + enum JOYSTICK_MODE + { + JOYSTICK_MODE_ATTITUDE = 0, + JOYSTICK_MODE_POSITION = 1, + JOYSTICK_MODE_FORCE = 2, + JOYSTICK_MODE_VELOCITY = 3, + JOYSTICK_MODE_MANUAL = 4 + }; + + /** * @brief Load joystick-specific settings. */ void loadJoystickSettings(); @@ -109,6 +125,11 @@ public: return isEnabled; } + bool calibrating() const + { + return isCalibrating; + } + int getMappingThrottleAxis() const { return throttleAxis; @@ -154,6 +175,11 @@ public: return numJoysticks; } + JOYSTICK_MODE getMode() const + { + return mode; + } + QString getJoystickNameById(int id) const { return QString(SDL_JoystickName(id)); @@ -162,16 +188,17 @@ public: float getCurrentValueForAxis(int axis) const; bool getInvertedForAxis(int axis) const; bool getRangeLimitForAxis(int axis) const; + float getAxisRangeLimitMinForAxis(int axis) const; + float getAxisRangeLimitMaxForAxis(int axis) const; int getActionForButton(int button) const; const double sdlJoystickMin; const double sdlJoystickMax; protected: - double calibrationPositive[10]; - double calibrationNegative[10]; bool isEnabled; ///< Track whether the system should emit the higher-level signals: joystickChanged & actionTriggered. + bool isCalibrating; ///< Track if calibration in progress bool done; SDL_Joystick* joystick; @@ -194,6 +221,9 @@ protected: int joystickNumAxes; int joystickNumButtons; + // mode of joystick (attitude, position, force, ... (see JOYSTICK_MODE enum)) + JOYSTICK_MODE mode; + // Track axis/button settings based on a Joystick/AutopilotType/SystemType triplet. // This is only a double-map, because settings are stored/loaded based on joystick // name first, so only the settings for the current joystick need to be stored at any given time. @@ -223,8 +253,9 @@ signals: * @param throttle Throttle, -100%:-1.0, 0%: 0.0, 100%: 1.0. If the roll axis isn't defined, NaN is transmit instead. * @param xHat hat vector in forward-backward direction, +1 forward, 0 center, -1 backward * @param yHat hat vector in left-right direction, -1 left, 0 center, +1 right + * @param mode (setpoint type) see JOYSTICK_MODE enum */ - void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons); + void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons, quint8 mode); /** * @brief Emit a new value for an axis @@ -287,6 +318,8 @@ public slots: void setActiveUAS(UASInterface* uas); /** @brief Switch to a new joystick by ID number. Both buttons and axes are updated with the proper signals emitted. */ void setActiveJoystick(int id); + /** @brief Switch calibration mode active */ + void setCalibrating(bool active); /** * @brief Change the control mapping for a given joystick axis. * @param axisID The axis to modify (0-indexed) @@ -300,12 +333,28 @@ public slots: * @param inverted True indicates inverted from normal. Varies by controller. */ void setAxisInversion(int axis, bool inverted); + /** * @brief Specify that an axis should only transmit the positive values. Useful for controlling throttle from auto-centering axes. * @param axis Which axis has its range limited. * @param limitRange If true only the positive half of this axis will be read. */ void setAxisRangeLimit(int axis, bool limitRange); + + /** + * @brief Specify minimum value for axis. + * @param axis Which axis should be set. + * @param min Value to be set. + */ + void setAxisRangeLimitMin(int axis, float min); + + /** + * @brief Specify maximum value for axis. + * @param axis Which axis should be set. + * @param max Value to be set. + */ + void setAxisRangeLimitMax(int axis, float max); + /** * @brief Specify a button->action mapping for the given uas. * This mapping is applied based on UAS autopilot type and UAS system type. @@ -314,6 +363,15 @@ public slots: * @param action The numeric ID of the action for this UAS to map to. */ void setButtonAction(int button, int action); + + /** + * @brief Specify which setpoints should be sent to the UAS when moving the joystick + * @param newMode the mode (setpoint type) see the JOYSTICK_MODE enum + */ + void setMode(int newMode) + { + mode = (JOYSTICK_MODE)newMode; + } }; #endif // _JOYSTICKINPUT_H_ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index fabee6e..52b82ff 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2401,7 +2401,7 @@ void UAS::toggleAutonomy() * Set the manual control commands. * This can only be done if the system has manual inputs enabled and is armed. */ -void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons) +void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8 joystickMode) { Q_UNUSED(xHat); Q_UNUSED(yHat); @@ -2414,56 +2414,198 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr static quint16 manualButtons = 0; static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission - // We only transmit manual command messages if the system has manual inputs enabled and is armed - if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) - { + // Transmit the external setpoints only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with + // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate + // if no command inputs have changed. + + // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz. + bool sendCommand = false; + if (countSinceLastTransmission++ >= 5) { + sendCommand = true; + countSinceLastTransmission = 0; + } else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) || + (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) || + buttons != manualButtons) { + sendCommand = true; + + // Ensure that another message will be sent the next time this function is called + countSinceLastTransmission = 10; + } + + // Now if we should trigger an update, let's do that + if (sendCommand) { + // Save the new manual control inputs + manualRollAngle = roll; + manualPitchAngle = pitch; + manualYawAngle = yaw; + manualThrust = thrust; + manualButtons = buttons; - // Transmit the manual commands only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with - // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate - // if no command inputs have changed. - // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz. - bool sendCommand = false; - if (countSinceLastTransmission++ >= 10) - { - sendCommand = true; - countSinceLastTransmission = 0; - } - else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) || - (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) || - buttons != manualButtons) - { - sendCommand = true; + mavlink_message_t message; - // Ensure that another message will be sent the next time this function is called - countSinceLastTransmission = 10; + if (joystickMode == JoystickInput::JOYSTICK_MODE_ATTITUDE) { + // send an external attitude setpoint command (rate control disabled) + float attitudeQuaternion[4]; + mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); + uint8_t typeMask = 0x7; // disable rate control + mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + typeMask, + attitudeQuaternion, + 0, + 0, + 0, + thrust + ); + } else if (joystickMode == JoystickInput::JOYSTICK_MODE_POSITION) { + // Send the the local position setpoint (local pos sp external message) + static float px = 0; + static float py = 0; + static float pz = 0; + //XXX: find decent scaling + px -= pitch; + py += roll; + pz -= 2.0f*(thrust-0.5); + uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + px, + py, + pz, + 0, + 0, + 0, + 0, + 0, + 0, + yaw, + 0 + ); + } else if (joystickMode == JoystickInput::JOYSTICK_MODE_FORCE) { + // Send the the force setpoint (local pos sp external message) + float dcm[3][3]; + mavlink_euler_to_dcm(roll, pitch, yaw, dcm); + const float fx = -dcm[0][2] * thrust; + const float fy = -dcm[1][2] * thrust; + const float fz = -dcm[2][2] * thrust; + uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else) + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + 0, + 0, + 0, + 0, + 0, + 0, + fx, + fy, + fz, + 0, + 0 + ); + } else if (joystickMode == JoystickInput::JOYSTICK_MODE_VELOCITY) { + // Send the the local velocity setpoint (local pos sp external message) + static float vx = 0; + static float vy = 0; + static float vz = 0; + static float yawrate = 0; + //XXX: find decent scaling + vx -= pitch; + vy += roll; + vz -= 2.0f*(thrust-0.5); + yawrate += yaw; //XXX: not sure what scale to apply here + uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + 0, + 0, + 0, + vx, + vy, + vz, + 0, + 0, + 0, + 0, + yawrate + ); + } else if (joystickMode == JoystickInput::JOYSTICK_MODE_MANUAL) { + if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) { + + // Transmit the manual commands only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with + // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate + // if no command inputs have changed. + // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz. + bool sendCommand = false; + if (countSinceLastTransmission++ >= 10) { + sendCommand = true; + countSinceLastTransmission = 0; + } else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) || + (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) || + buttons != manualButtons) { + sendCommand = true; + + // Ensure that another message will be sent the next time this function is called + countSinceLastTransmission = 10; + } + + // Now if we should trigger an update, let's do that + if (sendCommand) { + // Save the new manual control inputs + manualRollAngle = roll; + manualPitchAngle = pitch; + manualYawAngle = yaw; + manualThrust = thrust; + manualButtons = buttons; + + // Store scaling values for all 3 axes + const float axesScaling = 1.0 * 1000.0; + + // Calculate the new commands for roll, pitch, yaw, and thrust + const float newRollCommand = roll * axesScaling; + // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward + const float newPitchCommand = -pitch * axesScaling; + const float newYawCommand = yaw * axesScaling; + const float newThrustCommand = thrust * axesScaling; + + // Send the MANUAL_COMMAND message + mavlink_message_t message; + mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); + sendMessage(message); + + // Emit an update in control values to other UI elements, like the HSI display + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); + } + } else { + return; } - // Now if we should trigger an update, let's do that - if (sendCommand) - { - // Save the new manual control inputs - manualRollAngle = roll; - manualPitchAngle = pitch; - manualYawAngle = yaw; - manualThrust = thrust; - manualButtons = buttons; - - // Store scaling values for all 3 axes - const float axesScaling = 1.0 * 1000.0; - - // Calculate the new commands for roll, pitch, yaw, and thrust - const float newRollCommand = roll * axesScaling; - const float newPitchCommand = pitch * axesScaling; - const float newYawCommand = yaw * axesScaling; - const float newThrustCommand = thrust * axesScaling; - - // Send the MANUAL_COMMAND message - mavlink_message_t message; - mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); - sendMessage(message); - - // Emit an update in control values to other UI elements, like the HSI display - emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); + sendMessage(message); + + // Emit an update in control values to other UI elements, like the HSI display + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } } } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index e421cb9..358e427 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCJSBSimLink.h" #include "QGCXPlaneLink.h" #include "FileManager.h" +#include "JoystickInput.h" Q_DECLARE_LOGGING_CATEGORY(UASLog) @@ -780,7 +781,7 @@ public slots: void toggleAutonomy(); /** @brief Set the values for the manual control of the vehicle */ - void setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons); + void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8); /** @brief Set the values for the 6dof manual control of the vehicle */ void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw); diff --git a/src/ui/JoystickAxis.h b/src/ui/JoystickAxis.h index 6bb94b3..7d76623 100644 --- a/src/ui/JoystickAxis.h +++ b/src/ui/JoystickAxis.h @@ -45,6 +45,8 @@ public: void setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING newMapping); void setInverted(bool newValue); void setRangeLimit(bool newValue); + void setAxisRangeMin(float min); + void setAxisRangeMax(float max); signals: /** @brief Signal a change in this axis' yaw/pitch/roll mapping */ diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index 3409772..dec912b 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -45,6 +45,9 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : styleChanged(qgcApp()->styleIsDark()); connect(qgcApp(), &QGCApplication::styleChanged, this, &JoystickWidget::styleChanged); + // change mode when mode combobox is changed + connect(m_ui->joystickModeComboBox, SIGNAL(currentIndexChanged(int)), this->joystick, SLOT(setMode(int))); + // Display the widget above all other windows. this->raise(); this->show(); @@ -68,6 +71,14 @@ void JoystickWidget::initUI() m_ui->joystickFrame->setEnabled(true); } + // mode combo box + m_ui->joystickModeComboBox->addItem("Attitude"); + m_ui->joystickModeComboBox->addItem("Position"); + m_ui->joystickModeComboBox->addItem("Force"); + m_ui->joystickModeComboBox->addItem("Velocity"); + m_ui->joystickModeComboBox->addItem("Manual"); + m_ui->joystickModeComboBox->setCurrentIndex(joystick->getMode()); + // Create the initial UI. createUIForJoystick(); } @@ -228,6 +239,8 @@ void JoystickWidget::createUIForJoystick() { m_ui->axesBox->hide(); } + + connect(m_ui->calibrationButton, SIGNAL(clicked()), this, SLOT(cycleCalibrationButton())); } void JoystickWidget::updateAxisValue(int axis, float value) @@ -253,3 +266,14 @@ void JoystickWidget::joystickButtonReleased(int key) { buttons.at(key)->setStyleSheet(""); } + +void JoystickWidget::cycleCalibrationButton() +{ + if (this->joystick->calibrating()) { + this->joystick->setCalibrating(false); + m_ui->calibrationButton->setText("Calibrate range"); + } else { + this->joystick->setCalibrating(true); + m_ui->calibrationButton->setText("End calibration"); + } +} diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h index 7fbc13a..6f7cbbb 100644 --- a/src/ui/JoystickWidget.h +++ b/src/ui/JoystickWidget.h @@ -70,6 +70,8 @@ public slots: void joystickButtonPressed(int key); /** @brief Trigger a UI change based on a button being released */ void joystickButtonReleased(int key); + /** @brief Toggle the calibration button */ + void cycleCalibrationButton(); /** @brief Update the UI color scheme when the MainWindow theme changes. */ void styleChanged(bool styleIsDark); /** Update the UI assuming the joystick has stayed the same. */ diff --git a/src/ui/JoystickWidget.ui b/src/ui/JoystickWidget.ui index a56cc40..dbd2842 100644 --- a/src/ui/JoystickWidget.ui +++ b/src/ui/JoystickWidget.ui @@ -68,6 +68,9 @@ + + + false @@ -78,7 +81,7 @@ QFrame::Sunken - + QLayout::SetMinimumSize @@ -171,6 +174,13 @@ + + + + Calibrate range + + +