Skip to content

Q
qgroundcontrol
Commit ac3d0c9f authored by Julian Oes's avatar Julian Oes
Browse files
Options

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

parent 91cb07ca
Hide whitespace changes
Inline Side-by-side
Showing with 52 additions and 75 deletions
src/uas/UAS.cc
View file @ ac3d0c9f
......@@ -2946,83 +2946,60 @@ 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);
uint8_t mode = 3; // for velocity setpoint (OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY)
// Send the MANUAL_COMMAND message
mavlink_message_t 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
);
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
);
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());
}
}
......
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