zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #8234 from mavlink/pr-do-not-throttle-joystick-msg 

Let the sender control the joystick message rate.
QGC4.4
Gus Grubba 5 years ago committed by GitHub
parent
fb1d0d2828 bfbefa67eb
commit
72b172b452
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 139 additions and 197 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 325
      src/uas/UAS.cc
  2. 9
      src/uas/UAS.h

325
src/uas/UAS.cc
Unescape View File

@ -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,
*/ */
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__

9
src/uas/UAS.h
Unescape View File

@ -56,8 +56,8 @@ 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){
@ -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

Write Preview
Loading…
Cancel
Save