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

UAS: some cleanup for setManualControlCommands and set idle rate to 5Hz instead of 2.5Hz

QGC4.4
Julian Oes 11 years ago
parent
91cb07caf8
commit
ac3d0c9f3e
1 changed files with 52 additions and 75 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. 127
      src/uas/UAS.cc

127
src/uas/UAS.cc
Unescape View File

@ -2946,83 +2946,60 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
static quint16 manualButtons = 0; static quint16 manualButtons = 0;
static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission 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 // 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
//if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) // 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.
// 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;
// Ensure that another message will be sent the next time this function is called // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
countSinceLastTransmission = 10; 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 // Ensure that another message will be sent the next time this function is called
if (sendCommand) countSinceLastTransmission = 10;
{ }
// Save the new manual control inputs
manualRollAngle = roll; // Now if we should trigger an update, let's do that
manualPitchAngle = pitch; if (sendCommand)
manualYawAngle = yaw; {
manualThrust = thrust; // Save the new manual control inputs
manualButtons = buttons; manualRollAngle = roll;
manualPitchAngle = pitch;
// Store scaling values for all 3 axes manualYawAngle = yaw;
const float axesScaling = 1000.0f; manualThrust = thrust;
manualButtons = buttons;
// Calculate the new commands for roll, pitch, yaw, and thrust
// const float newRollCommand = roll * axesScaling; mavlink_message_t message;
// const float newPitchCommand = pitch * axesScaling;
// const float newYawCommand = yaw * axesScaling; // send an external attitude setpoint command (rate control disabled)
// const float newThrustCommand = thrust * axesScaling; float attitudeQuaternion[4];
//const int16_t rollCommand = (int16_t)(roll * axesScaling); mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
//const int16_t pitchCommand = (int16_t)(pitch * axesScaling); uint8_t typeMask = 0b111; // disable rate control
//const int16_t yawCommand = (int16_t)(yaw * axesScaling); mavlink_msg_attitude_setpoint_external_pack(mavlink->getSystemId(),
//const uint16_t thrustCommand = (uint16_t)(thrust * axesScaling); mavlink->getComponentId(),
&message,
QGC::groundTimeUsecs(),
uint8_t mode = 3; // for velocity setpoint (OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) this->uasId,
0,
// Send the MANUAL_COMMAND message typeMask,
mavlink_message_t message; attitudeQuaternion,
//mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); 0,
// send an external attitude setpoint command (rate control disabled) 0,
float attitudeQuaternion[4]; 0,
mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); thrust
uint8_t typeMask = 0b111; // disable rate control );
mavlink_msg_attitude_setpoint_external_pack(mavlink->getSystemId(), sendMessage(message);
mavlink->getComponentId(),
&message, // Emit an update in control values to other UI elements, like the HSI display
QGC::groundTimeUsecs(), emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
this->uasId,
0,
typeMask,
attitudeQuaternion,
0,
0,
0,
thrust
);
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());
}
} }
} }

Write Preview
Loading…
Cancel
Save