Commit bbeb00e3 authored by Bryant Mairs's avatar Bryant Mairs

Alter UAS.cc so that it transmits MANUAL_CONTROL messages at 5Hz when no inputs have changed.

Otherwise MANUAL_CONTROL messages are still sent at the regular 50Hz. This reduces transmission bandwidth from 0.9kbps when the joystick is used continuously to 0.1kbps when it's idle.
To prevent possible issues from the last high-bandwidth message being lost and the system then not responding for .1s, the last messsage is double-transmit which should prevent those issues.
parent abe60a9a
...@@ -2885,29 +2885,65 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double ...@@ -2885,29 +2885,65 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
Q_UNUSED(xHat); Q_UNUSED(xHat);
Q_UNUSED(yHat); Q_UNUSED(yHat);
// Scale values // Store the previous manual commands
double rollPitchScaling = 1.0f * 1000.0f; static double manualRollAngle = 0.0;
double yawScaling = 1.0f * 1000.0f; static double manualPitchAngle = 0.0;
double thrustScaling = 1.0f * 1000.0f; static double manualYawAngle = 0.0;
static double manualThrust = 0.0;
manualRollAngle = roll * rollPitchScaling; static int manualButtons = 0; //FIXME: Change buttons to a uint16_t, as it is defined by MAVLink
manualPitchAngle = pitch * rollPitchScaling; static int countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission
manualYawAngle = yaw * yawScaling;
manualThrust = thrust * thrustScaling; // We only transmit manual command messages if the system has manual inputs enabled and is armed
// If 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)) if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
{ {
mavlink_message_t message;
mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualPitchAngle, (float)manualRollAngle, (float)manualThrust, (float)manualYawAngle, buttons);
sendMessage(message);
//qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); // 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
else // if no command inputs have changed.
{ // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz.
//qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first"; 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
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;
// Store scaling values for all 3 axes
const double axesScaling = 1.0 * 1000.0;
// Calculate the new commands for roll, pitch, yaw, and thrust
const double newRollCommand = roll * axesScaling;
const double newPitchCommand = pitch * axesScaling;
const double newYawCommand = yaw * axesScaling;
const double newThrustCommand = thrust * axesScaling;
// Send the MANUAL_COMMAND message
mavlink_message_t message;
mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)newPitchCommand, (float)newRollCommand, (float)newThrustCommand, (float)newYawCommand, buttons);
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