From 7be19f2bc3bb95c12e4a4b22e3b2ae76a0630bf4 Mon Sep 17 00:00:00 2001 From: Bryant Mairs Date: Sun, 15 Jun 2014 13:30:46 -0700 Subject: [PATCH] Reduced size of datatypes involved with JoystickInput class. Most math and backend storage was floats anyways, so most of these changes just half the data copied during function calls without reducing precision. --- src/input/JoystickInput.cc | 8 ++++---- src/input/JoystickInput.h | 6 +++--- src/uas/UAS.cc | 26 +++++++++++++------------- src/uas/UAS.h | 2 +- src/uas/UASInterface.h | 2 +- src/ui/HSIDisplay.cc | 8 ++++---- src/ui/HSIDisplay.h | 2 +- src/ui/JoystickWidget.cc | 4 ++-- src/ui/JoystickWidget.h | 2 +- 9 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index 7f1bc7d7e..223058748 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -272,7 +272,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas) tmp = dynamic_cast(this->uas); if(tmp) { - disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int))); + disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16))); disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); } uasCanReverse = false; @@ -288,7 +288,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas) if (this->uas && (tmp = dynamic_cast(this->uas))) { - connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int))); + connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16))); connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); uasCanReverse = tmp->systemCanReverse(); @@ -439,10 +439,10 @@ void JoystickInput::run() // Build up vectors describing the hat position int hatPosition = SDL_JoystickGetHat(joystick, 0); - int newYHat = 0; + qint8 newYHat = 0; if ((SDL_HAT_UP & hatPosition) > 0) newYHat = 1; if ((SDL_HAT_DOWN & hatPosition) > 0) newYHat = -1; - int newXHat = 0; + qint8 newXHat = 0; if ((SDL_HAT_LEFT & hatPosition) > 0) newXHat = -1; if ((SDL_HAT_RIGHT & hatPosition) > 0) newXHat = 1; if (newYHat != yHat || newXHat != xHat) diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 66070990d..3bca5cbba 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -205,7 +205,7 @@ protected: // Track the last state of the axes, buttons, and hats for only emitting change signals. QList joystickAxes; ///< The values of every axes during the last sample. quint16 joystickButtons; ///< The state of every button. Bitfield supporting 16 buttons with 1s indicating that the button is down. - int xHat, yHat; ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left. + qint8 xHat, yHat; ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left. /** * @brief Called before main run() event loop starts. Waits for joysticks to be connected. @@ -224,7 +224,7 @@ signals: * @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 */ - void joystickChanged(double roll, double pitch, double yaw, double throttle, int xHat, int yHat, int buttons); + void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons); /** * @brief Emit a new value for an axis @@ -267,7 +267,7 @@ signals: * @param x vector in left-right direction * @param y vector in forward-backward direction */ - void hatDirectionChanged(int x, int y); + void hatDirectionChanged(qint8 x, qint8 y); /** @brief Signal that the UAS has been updated for this JoystickInput * Note that any UI updates should NOT query this object for joystick details. That should be done in response to the joystickSettingsChanged signal. diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 7a556bbfb..a9ced328d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2880,18 +2880,18 @@ 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(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons) +void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons) { Q_UNUSED(xHat); Q_UNUSED(yHat); // Store the previous manual commands - static double manualRollAngle = 0.0; - static double manualPitchAngle = 0.0; - static double manualYawAngle = 0.0; - static double manualThrust = 0.0; - static int manualButtons = 0; //FIXME: Change buttons to a uint16_t, as it is defined by MAVLink - static int countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission + static float manualRollAngle = 0.0; + static float manualPitchAngle = 0.0; + static float manualYawAngle = 0.0; + static float manualThrust = 0.0; + 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)) @@ -2928,17 +2928,17 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double manualButtons = buttons; // Store scaling values for all 3 axes - const double axesScaling = 1.0 * 1000.0; + const float axesScaling = 1.0 * 1000.0; // Calculate the new commands for roll, pitch, yaw, and thrust - const double newRollCommand = roll * axesScaling; - const double newPitchCommand = pitch * axesScaling; - const double newYawCommand = yaw * axesScaling; - const double newThrustCommand = thrust * axesScaling; + 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, (float)newPitchCommand, (float)newRollCommand, (float)newThrustCommand, (float)newYawCommand, buttons); + 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 diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 6b57f9c88..794395efb 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -838,7 +838,7 @@ public slots: void toggleAutonomy(); /** @brief Set the values for the manual control of the vehicle */ - void setManualControlCommands(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons); + void setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons); /** @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/uas/UASInterface.h b/src/uas/UASInterface.h index e918104a5..4218d32be 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -524,7 +524,7 @@ signals: void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec); void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec); void attitudeRotationRatesChanged(int uas, double rollrate, double pitchrate, double yawrate, quint64 usec); - void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); + void attitudeThrustSetPointChanged(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec); /** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */ void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); /** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */ diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 4e05a479a..21d47924a 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -921,7 +921,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64))); disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float))); disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); @@ -957,8 +957,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), - this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), + this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64))); connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), @@ -1100,7 +1100,7 @@ void HSIDisplay::sendBodySetPointCoordinates() } } -void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec) +void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec) { Q_UNUSED(uas); Q_UNUSED(usec); diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 6ebfeb738..b4edb3a25 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -55,7 +55,7 @@ public slots: /** @brief Set the width in meters this widget shows from top */ void setMetricWidth(double width); void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used); - void updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); + void updateAttitudeSetpoints(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec); void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time); void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired); void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index 5b5150791..03df9be2c 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -25,7 +25,7 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(joystickButtonPressed(int))); connect(this->joystick, SIGNAL(buttonReleased(int)), this, SLOT(joystickButtonReleased(int))); connect(this->joystick, SIGNAL(axisValueChanged(int,float)), this, SLOT(updateAxisValue(int,float))); - connect(this->joystick, SIGNAL(hatDirectionChanged(int,int)), this, SLOT(setHat(int,int))); + connect(this->joystick, SIGNAL(hatDirectionChanged(qint8,qint8)), this, SLOT(setHat(qint8,qint8))); // Also watch for when new settings were loaded for the current joystick to do a mass UI refresh. connect(this->joystick, SIGNAL(joystickSettingsChanged()), this, SLOT(updateUI())); @@ -238,7 +238,7 @@ void JoystickWidget::updateAxisValue(int axis, float value) } } -void JoystickWidget::setHat(int x, int y) +void JoystickWidget::setHat(qint8 x, qint8 y) { m_ui->statusLabel->setText(tr("Hat position: x: %1, y: %2").arg(x).arg(y)); } diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h index c1eb1d51a..71931b1e3 100644 --- a/src/ui/JoystickWidget.h +++ b/src/ui/JoystickWidget.h @@ -65,7 +65,7 @@ public slots: /** @brief Update the UI with new values for the hat. * @see JoystickInput::hatDirectionChanged */ - void setHat(int x, int y); + void setHat(qint8 x, qint8 y); /** @brief Trigger a UI change based on a button being pressed */ void joystickButtonPressed(int key); /** @brief Trigger a UI change based on a button being released */ -- 2.22.0