Skip to content

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

Let the sender control the joystick message rate.

parent ee8193f3
Hide whitespace changes
Inline Side-by-side
Showing with 139 additions and 197 deletions
src/uas/UAS.cc
View file @ bfbefa67
...@@ -59,10 +59,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi ...@@ -59,10 +59,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
controlPitchManual(true), controlPitchManual(true),
controlYawManual(true), controlYawManual(true),
controlThrustManual(true), controlThrustManual(true),
manualRollAngle(0),
manualPitchAngle(0),
manualYawAngle(0),
manualThrust(0),
#ifndef __mobile__ #ifndef __mobile__
fileManager(this, vehicle), fileManager(this, vehicle),
...@@ -808,194 +804,145 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, ...@@ -808,194 +804,145 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
*/ */
void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode) void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
{ {
if (!_vehicle) { if (!_vehicle || !_vehicle->priorityLink()) {
return;
}
if (!_vehicle->priorityLink()) {
return; return;
} }
mavlink_message_t message;
// Store the previous manual commands if (joystickMode == Vehicle::JoystickModeAttitude) {
static float manualRollAngle = 0.0; // send an external attitude setpoint command (rate control disabled)
static float manualPitchAngle = 0.0; float attitudeQuaternion[4];
static float manualYawAngle = 0.0; mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
static float manualThrust = 0.0; uint8_t typeMask = 0x7; // disable rate control
static quint16 manualButtons = 0; mavlink_msg_set_attitude_target_pack_chan(
static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission mavlink->getSystemId(),
mavlink->getComponentId(),
// 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 _vehicle->priorityLink()->mavlinkChannel(),
// 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 &message,
// if no command inputs have changed. QGC::groundTimeUsecs(),
this->uasId,
// The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz. 0,
bool sendCommand = false; typeMask,
if (countSinceLastTransmission++ >= 5) { attitudeQuaternion,
sendCommand = true; 0,
countSinceLastTransmission = 0; 0,
} else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) || 0,
(!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) || thrust);
buttons != manualButtons) { } else if (joystickMode == Vehicle::JoystickModePosition) {
sendCommand = true; // Send the the local position setpoint (local pos sp external message)
static float px = 0;
// Ensure that another message will be sent the next time this function is called static float py = 0;
countSinceLastTransmission = 10; static float pz = 0;
} //XXX: find decent scaling
px -= pitch;
// Now if we should trigger an update, let's do that py += roll;
if (sendCommand) { pz -= 2.0f*(thrust-0.5);
// Save the new manual control inputs uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control
manualRollAngle = roll; mavlink_msg_set_position_target_local_ned_pack_chan(
manualPitchAngle = pitch; mavlink->getSystemId(),
manualYawAngle = yaw; mavlink->getComponentId(),
manualThrust = thrust; _vehicle->priorityLink()->mavlinkChannel(),
manualButtons = buttons; &message,
QGC::groundTimeUsecs(),
mavlink_message_t message; this->uasId,
0,
if (joystickMode == Vehicle::JoystickModeAttitude) { MAV_FRAME_LOCAL_NED,
// send an external attitude setpoint command (rate control disabled) typeMask,
float attitudeQuaternion[4]; px,
mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); py,
uint8_t typeMask = 0x7; // disable rate control pz,
mavlink_msg_set_attitude_target_pack_chan(mavlink->getSystemId(), 0,
mavlink->getComponentId(), 0,
_vehicle->priorityLink()->mavlinkChannel(), 0,
&message, 0,
QGC::groundTimeUsecs(), 0,
this->uasId, 0,
0, yaw,
typeMask, 0);
attitudeQuaternion, } else if (joystickMode == Vehicle::JoystickModeForce) {
0, // Send the the force setpoint (local pos sp external message)
0, float dcm[3][3];
0, mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
thrust); const float fx = -dcm[0][2] * thrust;
} else if (joystickMode == Vehicle::JoystickModePosition) { const float fy = -dcm[1][2] * thrust;
// Send the the local position setpoint (local pos sp external message) const float fz = -dcm[2][2] * thrust;
static float px = 0; uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else)
static float py = 0; mavlink_msg_set_position_target_local_ned_pack_chan(
static float pz = 0; mavlink->getSystemId(),
//XXX: find decent scaling mavlink->getComponentId(),
px -= pitch; _vehicle->priorityLink()->mavlinkChannel(),
py += roll; &message,
pz -= 2.0f*(thrust-0.5); QGC::groundTimeUsecs(),
uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control this->uasId,
mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), 0,
mavlink->getComponentId(), MAV_FRAME_LOCAL_NED,
_vehicle->priorityLink()->mavlinkChannel(), typeMask,
&message, 0,
QGC::groundTimeUsecs(), 0,
this->uasId, 0,
0, 0,
MAV_FRAME_LOCAL_NED, 0,
typeMask, 0,
px, fx,
py, fy,
pz, fz,
0, 0,
0, 0);
0, } else if (joystickMode == Vehicle::JoystickModeVelocity) {
0, // Send the the local velocity setpoint (local pos sp external message)
0, static float vx = 0;
0, static float vy = 0;
yaw, static float vz = 0;
0); static float yawrate = 0;
} else if (joystickMode == Vehicle::JoystickModeForce) { //XXX: find decent scaling
// Send the the force setpoint (local pos sp external message) vx -= pitch;
float dcm[3][3]; vy += roll;
mavlink_euler_to_dcm(roll, pitch, yaw, dcm); vz -= 2.0f*(thrust-0.5);
const float fx = -dcm[0][2] * thrust; yawrate += yaw; //XXX: not sure what scale to apply here
const float fy = -dcm[1][2] * thrust; uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control
const float fz = -dcm[2][2] * thrust; mavlink_msg_set_position_target_local_ned_pack_chan(
uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else) mavlink->getSystemId(),
mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), mavlink->getComponentId(),
mavlink->getComponentId(), _vehicle->priorityLink()->mavlinkChannel(),
_vehicle->priorityLink()->mavlinkChannel(), &message,
&message, QGC::groundTimeUsecs(),
QGC::groundTimeUsecs(), this->uasId,
this->uasId, 0,
0, MAV_FRAME_LOCAL_NED,
MAV_FRAME_LOCAL_NED, typeMask,
typeMask, 0,
0, 0,
0, 0,
0, vx,
0, vy,
0, vz,
0, 0,
fx, 0,
fy, 0,
fz, 0,
0, yawrate);
0); } else if (joystickMode == Vehicle::JoystickModeRC) {
} else if (joystickMode == Vehicle::JoystickModeVelocity) { // Store scaling values for all 3 axes
// Send the the local velocity setpoint (local pos sp external message) static const float axesScaling = 1.0 * 1000.0;
static float vx = 0; // Calculate the new commands for roll, pitch, yaw, and thrust
static float vy = 0; const float newRollCommand = roll * axesScaling;
static float vz = 0; // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward
static float yawrate = 0; const float newPitchCommand = -pitch * axesScaling;
//XXX: find decent scaling const float newYawCommand = yaw * axesScaling;
vx -= pitch; const float newThrustCommand = thrust * axesScaling;
vy += roll; // Send the MANUAL_COMMAND message
vz -= 2.0f*(thrust-0.5); mavlink_msg_manual_control_pack_chan(
yawrate += yaw; //XXX: not sure what scale to apply here static_cast<uint8_t>(mavlink->getSystemId()),
uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control static_cast<uint8_t>(mavlink->getComponentId()),
mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), _vehicle->priorityLink()->mavlinkChannel(),
mavlink->getComponentId(), &message,
_vehicle->priorityLink()->mavlinkChannel(), static_cast<uint8_t>(this->uasId),
&message, static_cast<int16_t>(newPitchCommand),
QGC::groundTimeUsecs(), static_cast<int16_t>(newRollCommand),
this->uasId, static_cast<int16_t>(newThrustCommand),
0, static_cast<int16_t>(newYawCommand),
MAV_FRAME_LOCAL_NED, buttons);
typeMask,
0,
0,
0,
vx,
vy,
vz,
0,
0,
0,
0,
yawrate);
} else if (joystickMode == Vehicle::JoystickModeRC) {
// 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_msg_manual_control_pack_chan(
static_cast<uint8_t>(mavlink->getSystemId()),
static_cast<uint8_t>(mavlink->getComponentId()),
_vehicle->priorityLink()->mavlinkChannel(),
&message,
static_cast<uint8_t>(this->uasId),
static_cast<int16_t>(newPitchCommand),
static_cast<int16_t>(newRollCommand),
static_cast<int16_t>(newThrustCommand),
static_cast<int16_t>(newYawCommand),
buttons);
}
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
} }
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
} }
#ifndef __mobile__ #ifndef __mobile__
......
src/uas/UAS.h
View file @ bfbefa67
...@@ -56,9 +56,9 @@ public: ...@@ -56,9 +56,9 @@ public:
/** @brief The time interval the robot is switched on */ /** @brief The time interval the robot is switched on */
quint64 getUptime() const; quint64 getUptime() const;
/// Vehicle is about to go away /// Vehicle is about to go away
void shutdownVehicle(void); void shutdownVehicle(void);
// Setters for HIL noise variance // Setters for HIL noise variance
void setXaccVar(float var){ void setXaccVar(float var){
xacc_var = var; xacc_var = var;
...@@ -138,11 +138,6 @@ protected: ...@@ -138,11 +138,6 @@ protected:
bool controlYawManual; ///< status flag, true if yaw is controlled manually bool controlYawManual; ///< status flag, true if yaw is controlled manually
bool controlThrustManual; ///< status flag, true if thrust is controlled manually bool controlThrustManual; ///< status flag, true if thrust is controlled manually
double manualRollAngle; ///< Roll angle set by human pilot (radians)
double manualPitchAngle; ///< Pitch angle set by human pilot (radians)
double manualYawAngle; ///< Yaw angle set by human pilot (radians)
double manualThrust; ///< Thrust set by human pilot (radians)
/// POSITION /// POSITION
bool isGlobalPositionKnown; ///< If the global position has been received for this MAV bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
......
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