public ThrustCommand limit (ThrustCommand input)
{
ThrustCommand output = new ThrustCommand ();
output.header = input.header;
output.thrust = (float) thrust.limit ( input.thrust );
return output;
}
public YawRateCommand limit (YawRateCommand input)
{
YawRateCommand output = new YawRateCommand ();
output.header = input.header;
output.turnrate = (float) turnrate.limit ( input.turnrate );
return output;
}
public Vector3 limit (Vector3 input)
{
Vector3 output = new Vector3 ();
output.x = x.limit ( input.x );
output.y = y.limit ( input.y );
output.z = z.limit ( input.z );
double absolute_value_xy = System.Math.Sqrt ( output.x * output.x + output.y * output.y );
if ( absolute_value_xy > absolute_maximum_xy )
{
output.x *= absolute_maximum_xy / absolute_value_xy;
output.y *= absolute_maximum_xy / absolute_value_xy;
output.z *= absolute_maximum_xy / absolute_value_xy;
}
double absolute_value = System.Math.Sqrt ( output.x * output.x + output.y * output.y + output.z * output.z );
if ( absolute_value > absolute_maximum )
{
output.x *= absolute_maximum / absolute_value;
output.y *= absolute_maximum / absolute_value;
output.z *= absolute_maximum / absolute_value;
}
return output;
}
public Point limit (Point input)
{
Point output = new Point ();
output.x = x.limit ( input.x );
output.y = y.limit ( input.y );
output.z = z.limit ( input.z );
return output;
}
public AttitudeCommand limit (AttitudeCommand input)
{
AttitudeCommand output = new AttitudeCommand ();
output.header = input.header;
output.roll = (float) roll.limit ( input.roll );
output.pitch = (float) pitch.limit ( input.pitch );
double absolute_value = System.Math.Sqrt ( output.roll * output.roll + output.pitch * output.pitch );
if ( absolute_value > absolute_max )
{
output.roll *= (float) ( absolute_max / absolute_value );
output.pitch *= (float) ( absolute_max / absolute_value );
}
return output;
}