Skip to content

Q
qgroundcontrol
Commit fccdc662 authored by Thomas Gubler's avatar Thomas Gubler
Browse files
Options

Merge remote-tracking branch 'upstream/mavlinksubmodule_joystickinput' into... 

Merge remote-tracking branch 'upstream/mavlinksubmodule_joystickinput' into mavlinksubmodule_joystickinput

Conflicts:
	src/uas/UAS.cc
parents 465c19c7 ac3d0c9f
Hide whitespace changes
Inline Side-by-side
Showing with 84 additions and 111 deletions
src/uas/UAS.cc
View file @ fccdc662
......@@ -2946,111 +2946,91 @@ 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))
// Lets always send velocity commands for now
if (true)
{
// 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;
// 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.
// Ensure that another message will be sent the next time this function is called
countSinceLastTransmission = 10;
}
// 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;
// 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 = 1000.0f;
// 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;
//const int16_t rollCommand = (int16_t)(roll * axesScaling);
//const int16_t pitchCommand = (int16_t)(pitch * axesScaling);
//const int16_t yawCommand = (int16_t)(yaw * axesScaling);
//const uint16_t thrustCommand = (uint16_t)(thrust * axesScaling);
mavlink_message_t message;
//// Send the attitude setpoint external message
////mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
//// send an external attitude setpoint command (rate control disabled)
//float attitudeQuaternion[4];
//mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
//uint8_t typeMask = 0b111; // disable rate control
//mavlink_msg_attitude_setpoint_external_pack(mavlink->getSystemId(),
//mavlink->getComponentId(),
//&message,
//QGC::groundTimeUsecs(),
//this->uasId,
//0,
//typeMask,
//attitudeQuaternion,
//0,
//0,
//0,
//thrust
//);
// Send the the force setpoint (local pos sp external message)
//mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
// send an external attitude setpoint command (rate control disabled)
float dcm[3][3];
mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
const float fx = -dcm[0][2];
const float fy = -dcm[1][2];
const float fz = -dcm[2][2];
uint16_t typeMask = 0b0000001000111111; // disable rate control
mavlink_msg_local_ned_position_setpoint_external_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
);
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());
}
// 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;
mavlink_message_t message;
// send an external attitude setpoint command (rate control disabled)
float attitudeQuaternion[4];
mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
uint8_t typeMask = 0b111; // disable rate control
mavlink_msg_attitude_setpoint_external_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&message,
QGC::groundTimeUsecs(),
this->uasId,
0,
typeMask,
attitudeQuaternion,
0,
0,
0,
thrust
);
// TODO: This is a prototype to be used depending on the joystick mode
// // Send the the force setpoint (local pos sp external message)
// //mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
// // send an external attitude setpoint command (rate control disabled)
// float dcm[3][3];
// mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
// const float fx = -dcm[0][2];
// const float fy = -dcm[1][2];
// const float fz = -dcm[2][2];
// uint16_t typeMask = 0b0000001000111111; // disable rate control
// mavlink_msg_local_ned_position_setpoint_external_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
// );
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());
}
}
......
src/ui/JoystickWidget.ui
View file @ fccdc662
......@@ -81,7 +81,7 @@
<property name="frameShadow">
<enum>QFrame::Sunken</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,0,0,0">
<layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,0,0">
<property name="sizeConstraint">
<enum>QLayout::SetMinimumSize</enum>
</property>
......@@ -181,13 +181,6 @@
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="calibrationStatusLabel">
<property name="text">
<string>Range status</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
......
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