public void GyroSetOverrideFromGridReferenceFrame(Quaternion targetRotation, IMyGyro specificGyro = null, bool allGyros = false)
{
List <IMyGyro> gyros;
if (allGyros)
{
gyros = GetBlocks <IMyGyro>();
}
else if (specificGyro == null)
{
//TODO Error message location
return;
}
else
{
gyros = new List <IMyGyro>();
gyros.Add(specificGyro);
}
//TODO: Optimise quaternion operations, possibly by sorting gyros using their orientaions
foreach (IMyGyro gyro in gyros)
{
Quaternion rotationOperation = GyroMovementFromLocalRotation(targetRotation, gyro);
RollPitchYaw rpy = QuaternionToRollPitchYaw(rotationOperation);
GyroSetOverrideToRollPitchYaw(gyro, rpy);
}
}
//TODO: Change all floats to doubles?
/// <summary>
/// Converts quaternion to RollPitchYaw object.
/// </summary>
/// <param name="quaternion"></param>
/// <returns></returns>
public RollPitchYaw QuaternionToRollPitchYaw(Quaternion quaternion)
{
RollPitchYaw rpy = new RollPitchYaw();
QuaternionToRollPitchYaw(quaternion, out rpy.roll, out rpy.pitch, out rpy.yaw);
return(rpy);
}
public RollPitchYaw toRPY()
{
RollPitchYaw rpy = new RollPitchYaw();
rpy.rpy[0] = Math.Atan2(2 * (this.w * this.x + this.y * this.z), 1 - 2 * (this.x * this.x + this.y * this.y));
rpy.rpy[1] = Math.Asin(2 * (this.w * this.y - this.z * this.x));
rpy.rpy[2] = Math.Atan2(2 * (this.w * this.z + this.x * this.y), 1 - 2 * (this.y * this.y + this.z * this.z));
return(rpy);
}
private void update_autoframe(int i)
{
if (fcheck[i].Checked == true)
{
if (fbox2[i].SelectedIndex != 2 && getQ.Enabled == true)
{
try
{
if (renew_quaternion)
{
arduino.getQ();
DateTime time_start = DateTime.Now;
while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100)
{
;
}
arduino.dataRecieved = false;
autoq.w = arduino.quaternion[0];
autoq.x = arduino.quaternion[1];
autoq.y = arduino.quaternion[2];
autoq.z = arduino.quaternion[3];
renew_quaternion = false;
}
RollPitchYaw rpy = (autoq.Normalized().Round(4) * q.Normalized().Round(4).Inverse()).toRPY();
int gain = (int)Math.Round(rpy.rpy[fbox2[i].SelectedIndex] * (180 / Math.PI) * p_gain[i]);
if (Math.Abs(autogain[i] - gain) > 4 * p_gain[i])
{
autogain[i] = gain;
}
}
catch
{
autogain[i] = 0;
}
}
else
{
autogain[i] = 0;
}
int pos = (int)homeframe[i] + offset[i] + autogain[i];
if ((uint)pos >= min[i] && (uint)pos <= Max[i])
{
if (autoframe[i] != pos)
{
autoframe[i] = pos;
send_msg = true;
}
return;
}
}
autoframe[i] = 0;
}
//TODO: Clean this up and debug the value issues.
/// <summary>
/// Sets override to given roll pitch yaw object.
/// </summary>
/// <param name="gyro"></param>
/// <param name="rpy"></param>
public void GyroSetOverrideToRollPitchYaw(IMyGyro gyro, RollPitchYaw rpy, bool enableGyro = true)
{
RollPitchYaw tmpRpy = rpy;
tmpRpy.ConvertToUnit(AngleUnit.FREQUENCY);
if (enableGyro)
{
gyro.GyroOverride = true;
}
gyro.Roll = tmpRpy.roll;
gyro.Pitch = tmpRpy.pitch;
gyro.Yaw = tmpRpy.yaw;
}
public void SetNext(LatLonAlt nextPosition, RollPitchYaw nextOrientation)
{
this.position = nextPosition;
this.orientation = nextOrientation;
}
public JumpCameraController(Host host, CameraController <PredictiveCamera> nextCamera, LatLonAlt nextPosition, RollPitchYaw nextOrientation)
: base()
{
this.host = host;
this.Next = nextCamera;
this.SetNext(nextPosition, nextOrientation);
}
private void update_autoframe(int i)
{
if (fcheck[i].Checked == true)
{
if ((fbox2[i].SelectedIndex != 0 || fbox3[i].SelectedIndex != 0) && getQ.Enabled == true)
{
try
{
if (renew_quaternion)
{
arduino.getQ();
DateTime time_start = DateTime.Now;
while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100)
{
;
}
arduino.dataRecieved = false;
autoq.w = arduino.quaternion[0];
autoq.x = arduino.quaternion[1];
autoq.y = arduino.quaternion[2];
autoq.z = arduino.quaternion[3];
arduino.pin_capture(11);
while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100)
{
;
}
arduino.dataRecieved = false;
omega[0] = 0.1 * omega[0] + 0.9 * arduino.captured_float;
arduino.pin_capture(12);
while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100)
{
;
}
arduino.dataRecieved = false;
omega[1] = 0.1 * omega[1] + 0.9 * arduino.captured_float;
renew_quaternion = false;
}
RollPitchYaw rpy = (autoq.Normalized().Round(4) * q.Normalized().Round(4).Inverse()).toRPY();
int gain = 0;
for (int src = 0; src < 2; src++)
{
if (src == 0 && p_gain[i] != 0)
{
if (fbox2[i].SelectedIndex == 1 || fbox2[i].SelectedIndex == 2)
{
gain += (int)Math.Round(rpy.rpy[fbox2[i].SelectedIndex - 1] * (180 / Math.PI) * p_gain[i]);
}
else if (fbox2[i].SelectedIndex == 3 || fbox2[i].SelectedIndex == 4)
{
gain += (int)Math.Round(omega[fbox2[i].SelectedIndex - 3] * p_gain[i]);
}
}
else if (src == 1 && s_gain[i] != 0)
{
if (fbox3[i].SelectedIndex == 1 || fbox3[i].SelectedIndex == 2)
{
gain += (int)Math.Round(rpy.rpy[fbox3[i].SelectedIndex - 1] * (180 / Math.PI) * s_gain[i]);
}
else if (fbox3[i].SelectedIndex == 3 || fbox3[i].SelectedIndex == 4)
{
gain += (int)Math.Round(omega[fbox3[i].SelectedIndex - 3] * s_gain[i]);
}
}
}
autogain[i] = gain;
}
catch
{
autogain[i] = 0;
}
}
else
{
autogain[i] = 0;
}
int pos = (int)homeframe[i] + offset[i] + autogain[i];
if ((uint)pos >= min[i] && (uint)pos <= Max[i])
{
if (autoframe[i] != pos)
{
autoframe[i] = pos;
send_msg = true;
}
return;
}
}
autoframe[i] = 0;
}
internal VERollPitchYaw(RollPitchYaw rpy)
{
this.roll = rpy.Roll * 180.0 / Math.PI;
this.pitch = rpy.Pitch * 180.0 / Math.PI;
this.yaw = rpy.Yaw * 180.0 / Math.PI;
}
