public static int[] getAttitude()
{
byte[] angxArray = new byte[2];
byte[] angyArray = new byte[2];
byte[] headingArray = new byte[2];
int[] result = new int[3];
byte[] mspAttitude = MultiWiiFunctions.mspAttitude();
angxArray[0] = mspAttitude[2];
angxArray[1] = mspAttitude[3];
int angx = BitConverter.ToInt16(angxArray, 0);
Console.WriteLine("Ang x: " + angx);
result[0] = angx;
angyArray[0] = mspAttitude[4];
angyArray[1] = mspAttitude[5];
int angy = BitConverter.ToInt16(angyArray, 0);
Console.WriteLine("Ang y: " + angy);
result[1] = angy;
headingArray[0] = mspAttitude[6];
headingArray[1] = mspAttitude[7];
int heading = BitConverter.ToInt16(headingArray, 0);
Console.WriteLine("Heading: " + heading);
result[2] = heading;
return(result);
}
public static int[] getAltitude()
{
byte[] cmArray = new byte[4];
byte[] cmsArray = new byte[2];
int[] result = new int[2];
byte[] mspAltitude = MultiWiiFunctions.mspAltitude();
cmArray[0] = mspAltitude[2];
cmArray[1] = mspAltitude[3];
cmArray[2] = mspAltitude[4];
cmArray[3] = mspAltitude[5];
int cm = BitConverter.ToInt32(cmArray, 0);
Console.WriteLine("Altitude in cm: " + cm);
result[0] = cm;
cmsArray[0] = mspAltitude[6];
cmsArray[1] = mspAltitude[7];
int cms = BitConverter.ToInt16(cmsArray, 0);
Console.WriteLine("Velocity in cm/s: " + cms);
result[1] = cms;
return(result);
}
public static Boolean yaw(int yaw)
{
Boolean result = false;
if (yaw < 1000 || yaw > 2000)
{
return(result);
}
int[] data = { channelsActualValues[0], channelsActualValues[1], yaw, channelsActualValues[3] };
channelsActualValues[2] = yaw;
Console.WriteLine(string.Join(",", data));
if (!MultiWiiFunctions.mspSetRawRC(data).Equals(new byte[0]))
{
result = true;
}
return(result);
}
public static Boolean pitch(int pitch)
{
Boolean result = false;
if (pitch < 1000 || pitch > 2000)
{
return(result);
}
int[] data = { channelsActualValues[0], pitch, channelsActualValues[2], channelsActualValues[3] };
channelsActualValues[1] = pitch;
Console.WriteLine(string.Join(",", data));
if (!MultiWiiFunctions.mspSetRawRC(data).Equals(new byte[0]))
{
result = true;
}
return(result);
}
public static Boolean disarm()
{
Boolean result = false;
int[] data = { 1500, 1500, 1500, 1000, 1000 };
channelsActualValues[0] = 1500;
channelsActualValues[1] = 1500;
channelsActualValues[2] = 1500;
channelsActualValues[3] = 1000;
channelsActualValues[4] = 1000;
Console.WriteLine(string.Join(",", data));
if (!MultiWiiFunctions.mspSetRawRC(data).Equals(new byte[0]))
{
result = true;
}
return(result);
}
public static Boolean roll(int roll)
{
Boolean result = false;
if (roll < 1000 || roll > 2000)
{
return(result);
}
int[] data = { roll, channelsActualValues[1], channelsActualValues[2], channelsActualValues[3] };
channelsActualValues[0] = roll;
Console.WriteLine(string.Join(",", data));
if (!MultiWiiFunctions.mspSetRawRC(data).Equals(new byte[0]))
{
result = true;
}
return(result);
}
public static String getMultiWiiVersion()
{
byte[] mspIdent = MultiWiiFunctions.mspIdent();
while (numExceedError > 0 && mspIdent.Length != 10)
{
Console.WriteLine("Num of Error: " + numExceedError);
numExceedError--;
mspIdent = MultiWiiFunctions.mspIdent();
}
if (numExceedError <= 0)
{
Console.WriteLine("ERROR 102: Couldn't read the correct values");
numExceedError = 4;
return("Incorrect Value");
}
int version = Convert.ToUInt16(mspIdent[2]);
String versionString = version.ToString();
versionString = versionString[0] + "." + versionString.Substring(1);
Console.WriteLine("MultiWii Version is: " + versionString);
return(versionString);
}
public static Boolean throttle(int throttle)
{
Boolean result = false;
if (throttle < 1000 || throttle > 2000)
{
return(result);
}
if (throttle == 1000)
{
channelsActualValues[0] = 1500;
channelsActualValues[1] = 1500;
channelsActualValues[2] = 1500;
}
int[] data = { channelsActualValues[0], channelsActualValues[1], channelsActualValues[2], throttle };
channelsActualValues[3] = throttle;
Console.WriteLine(string.Join(",", data));
if (!MultiWiiFunctions.mspSetRawRC(data).Equals(new byte[0]))
{
result = true;
}
return(result);
}
public static int[] getMotorsValues()
{
byte[] motor1 = new byte[2];
byte[] motor2 = new byte[2];
byte[] motor3 = new byte[2];
byte[] motor4 = new byte[2];
byte[] motor5 = new byte[2];
byte[] motor6 = new byte[2];
byte[] motor7 = new byte[2];
byte[] motor8 = new byte[2];
int[] result = new int[8];
byte[] mspMotor = MultiWiiFunctions.mspMotor();
while (numExceedError > 0 && mspMotor.Length != 19)
{
Console.WriteLine("Num of Error: " + numExceedError);
numExceedError--;
mspMotor = MultiWiiFunctions.mspMotor();
}
if (numExceedError <= 0)
{
Console.WriteLine("ERROR 102: Couldn't read the correct values");
numExceedError = 4;
return(new int[0]);
}
motor1[0] = mspMotor[2];
motor1[1] = mspMotor[3];
int motor1Channel = BitConverter.ToInt16(motor1, 0);
Console.WriteLine("Motor1 Channel: " + motor1Channel);
result[0] = motor1Channel;
motorsActualValues[0] = motor1Channel;
motor2[0] = mspMotor[4];
motor2[1] = mspMotor[5];
int motor2Channel = BitConverter.ToInt16(motor2, 0);
Console.WriteLine("Motor2 Channel: " + motor2Channel);
result[1] = motor2Channel;
motorsActualValues[1] = motor2Channel;
motor3[0] = mspMotor[6];
motor3[1] = mspMotor[7];
int motor3Channel = BitConverter.ToInt16(motor3, 0);
Console.WriteLine("Motor3 Channel: " + motor3Channel);
result[2] = motor3Channel;
motorsActualValues[2] = motor3Channel;
motor4[0] = mspMotor[8];
motor4[1] = mspMotor[9];
int motor4Channel = BitConverter.ToInt16(motor4, 0);
Console.WriteLine("Motor4 Channel: " + motor4Channel);
result[3] = motor4Channel;
motorsActualValues[3] = motor4Channel;
motor5[0] = mspMotor[10];
motor5[1] = mspMotor[11];
int motor5Channel = BitConverter.ToInt16(motor5, 0);
Console.WriteLine("Motor5 Channel: " + motor5Channel);
result[4] = motor5Channel;
motorsActualValues[4] = motor5Channel;
motor6[0] = mspMotor[12];
motor6[1] = mspMotor[13];
int motor6Channel = BitConverter.ToInt16(motor6, 0);
Console.WriteLine("Motor6 Channel: " + motor6Channel);
result[5] = motor6Channel;
motorsActualValues[5] = motor6Channel;
motor7[0] = mspMotor[14];
motor7[1] = mspMotor[15];
int motor7Channel = BitConverter.ToInt16(motor7, 0);
Console.WriteLine("Motor7 Channel: " + motor7Channel);
result[6] = motor7Channel;
motorsActualValues[6] = motor7Channel;
motor8[0] = mspMotor[16];
motor8[1] = mspMotor[17];
int motor8Channel = BitConverter.ToInt16(motor8, 0);
Console.WriteLine("Motor8 Channel: " + motor8Channel);
result[7] = motor8Channel;
motorsActualValues[7] = motor8Channel;
numExceedError = 4;
return(result);
}
public static int[] getInertialMeasureUnitValues(Boolean accelerometer, Boolean gyroscope, Boolean magnetometer)
{
byte[] accX = new byte[2];
byte[] accY = new byte[2];
byte[] accZ = new byte[2];
byte[] gyroX = new byte[2];
byte[] gyroY = new byte[2];
byte[] gyroZ = new byte[2];
byte[] magX = new byte[2];
byte[] magY = new byte[2];
byte[] magZ = new byte[2];
int numberOfElements = 0;
if (accelerometer)
{
numberOfElements += 3;
}
if (gyroscope)
{
numberOfElements += 3;
}
if (magnetometer)
{
numberOfElements += 3;
}
int[] result = new int[numberOfElements];
byte[] mspRawIMU = MultiWiiFunctions.mspRawIMU();
while (numExceedError > 0 && mspRawIMU.Length != 21)
{
Console.WriteLine("Num of Error: " + numExceedError);
numExceedError--;
mspRawIMU = MultiWiiFunctions.mspRawIMU();
}
if (numExceedError <= 0)
{
Console.WriteLine("ERROR 102: Couldn't read the correct values");
numExceedError = 4;
return(new int[0]);
}
if (accelerometer)
{
accX[0] = mspRawIMU[2];
accX[1] = mspRawIMU[3];
int accXValue = BitConverter.ToInt16(accX, 0);
Console.WriteLine("Acceloremeter, Coordinate X Value: " + accXValue);
result[0] = accXValue;
accY[0] = mspRawIMU[4];
accY[1] = mspRawIMU[5];
int accYValue = BitConverter.ToInt16(accY, 0);
Console.WriteLine("Accelerometer, Coordinate Y Value: " + accYValue);
result[1] = accYValue;
accZ[0] = mspRawIMU[6];
accZ[1] = mspRawIMU[7];
int accZValue = BitConverter.ToInt16(accZ, 0);
Console.WriteLine("Accelerometer, Coordinate Z Value: " + accZValue);
result[2] = accZValue;
}
if (gyroscope)
{
gyroX[0] = mspRawIMU[8];
gyroX[1] = mspRawIMU[9];
int gyroXValue = BitConverter.ToInt16(gyroX, 0);
Console.WriteLine("Gyroscope, Coordinate X Value: " + gyroXValue);
gyroY[0] = mspRawIMU[10];
gyroY[1] = mspRawIMU[11];
int gyroYValue = BitConverter.ToInt16(gyroY, 0);
Console.WriteLine("Gyroscope, Coordinate Y Value: " + gyroYValue);
gyroZ[0] = mspRawIMU[12];
gyroZ[1] = mspRawIMU[13];
int gyroZValue = BitConverter.ToInt16(gyroZ, 0);
Console.WriteLine("Gyroscope, Coordinate Z Value: " + gyroZValue);
if (accelerometer)
{
result[3] = gyroXValue;
result[4] = gyroYValue;
result[5] = gyroZValue;
}
else
{
result[0] = gyroXValue;
result[1] = gyroYValue;
result[2] = gyroZValue;
}
}
if (magnetometer)
{
magX[0] = mspRawIMU[14];
magX[1] = mspRawIMU[15];
int magXValue = BitConverter.ToInt16(magX, 0);
Console.WriteLine("Magnetometer, Coordinate X Value: " + magXValue);
magY[0] = mspRawIMU[16];
magY[1] = mspRawIMU[17];
int magYValue = BitConverter.ToInt16(magY, 0);
Console.WriteLine("Magnetometer, Coordinate Y Value: " + magYValue);
magZ[0] = mspRawIMU[18];
magZ[1] = mspRawIMU[19];
int magZValue = BitConverter.ToInt16(magZ, 0);
Console.WriteLine("Magnetometer, Coordinate Z Value: " + magZValue);
if (accelerometer && gyroscope)
{
result[6] = magXValue;
result[7] = magYValue;
result[8] = magZValue;
}
else
{
if (accelerometer || gyroscope)
{
result[3] = magXValue;
result[4] = magYValue;
result[5] = magZValue;
}
else
{
result[0] = magXValue;
result[1] = magYValue;
result[2] = magZValue;
}
}
}
numExceedError = 4;
return(result);
}
public static int[] getRCChannels()
{
byte[] roll = new byte[2];
byte[] pitch = new byte[2];
byte[] yaw = new byte[2];
byte[] throttle = new byte[2];
byte[] aux1 = new byte[2];
byte[] aux2 = new byte[2];
byte[] aux3 = new byte[2];
byte[] aux4 = new byte[2];
int[] result = new int[8];
byte[] mspRC = MultiWiiFunctions.mspRC();
while (numExceedError > 0 && mspRC.Length != 19)
{
Console.WriteLine("Num of Error: " + numExceedError);
numExceedError--;
mspRC = MultiWiiFunctions.mspRC();
}
if (numExceedError <= 0)
{
Console.WriteLine("ERROR 102: Couldn't read the correct values");
numExceedError = 4;
return(new int[0]);
}
roll[0] = mspRC[2];
roll[1] = mspRC[3];
int rollChannel = BitConverter.ToInt16(roll, 0);
Console.WriteLine("Roll Channel: " + rollChannel);
result[0] = rollChannel;
Boolean rollbool = rollChannel >= 1000 && rollChannel <= 2000;
pitch[0] = mspRC[4];
pitch[1] = mspRC[5];
int pitchChannel = BitConverter.ToInt16(pitch, 0);
Console.WriteLine("Pitch Channel: " + pitchChannel);
result[1] = pitchChannel;
Boolean pitchbool = pitchChannel >= 1000 && pitchChannel <= 2000;
yaw[0] = mspRC[6];
yaw[1] = mspRC[7];
int yawChannel = BitConverter.ToInt16(yaw, 0);
Console.WriteLine("Yaw Channel: " + yawChannel);
result[2] = yawChannel;
Boolean yawbool = yawChannel >= 1000 && yawChannel <= 2000;
throttle[0] = mspRC[8];
throttle[1] = mspRC[9];
int throttleChannel = BitConverter.ToInt16(throttle, 0);
Console.WriteLine("Throttle Channel: " + throttleChannel);
result[3] = throttleChannel;
Boolean throttlebool = throttleChannel >= 1000 && throttleChannel <= 2000;
aux1[0] = mspRC[10];
aux1[1] = mspRC[11];
int aux1Channel = BitConverter.ToInt16(aux1, 0);
Console.WriteLine("Aux1 Channel: " + aux1Channel);
result[4] = aux1Channel;
Boolean aux1bool = aux1Channel >= 1000 && aux1Channel <= 2000;
aux2[0] = mspRC[12];
aux2[1] = mspRC[13];
int aux2Channel = BitConverter.ToInt16(aux2, 0);
Console.WriteLine("Aux2 Channel: " + aux2Channel);
result[5] = aux2Channel;
aux3[0] = mspRC[14];
aux3[1] = mspRC[15];
int aux3Channel = BitConverter.ToInt16(aux3, 0);
Console.WriteLine("Aux3 Channel: " + aux3Channel);
result[6] = aux3Channel;
aux4[0] = mspRC[16];
aux4[1] = mspRC[17];
int aux4Channel = BitConverter.ToInt16(aux4, 0);
Console.WriteLine("Aux4 Channel: " + aux4Channel);
result[7] = aux4Channel;
if (numExceedError > 0 && (!rollbool || !pitchbool || !yawbool || !throttlebool || !aux1bool))
{
Console.WriteLine("Num of Error: " + numExceedError);
numExceedError--;
result = getRCChannels();
}
if (numExceedError <= 0)
{
Console.WriteLine("ERROR 102: Couldn't read the correct values");
numExceedError = 4;
return(new int[0]);
}
numExceedError = 4;
return(result);
}
public static Boolean setMotor(int i, int intensity)
{
Boolean result = false;
byte[] mspSetMotor;
if (intensity < 1000 || intensity > 2000)
{
return(result);
}
if (motorsActualValues[0] == 0)
{
getMotorsValues();
}
switch (i)
{
case 1:
{
int[] data = { intensity, motorsActualValues[1], motorsActualValues[2], motorsActualValues[3], motorsActualValues[4], motorsActualValues[5], motorsActualValues[6], motorsActualValues[7] };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
case 2:
{
int[] data = { motorsActualValues[0], intensity, motorsActualValues[2], motorsActualValues[3], motorsActualValues[4], motorsActualValues[5], motorsActualValues[6], motorsActualValues[7] };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
case 3:
{
int[] data = { motorsActualValues[0], motorsActualValues[1], intensity, motorsActualValues[3], motorsActualValues[4], motorsActualValues[5], motorsActualValues[6], motorsActualValues[7] };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
case 4:
{
int[] data = { motorsActualValues[0], motorsActualValues[1], motorsActualValues[2], intensity, motorsActualValues[4], motorsActualValues[5], motorsActualValues[6], motorsActualValues[7] };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
case 5:
{
int[] data = { motorsActualValues[0], motorsActualValues[1], motorsActualValues[2], motorsActualValues[3], intensity, motorsActualValues[5], motorsActualValues[6], motorsActualValues[7] };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
case 6:
{
int[] data = { motorsActualValues[0], motorsActualValues[1], motorsActualValues[2], motorsActualValues[3], motorsActualValues[4], intensity, motorsActualValues[6], motorsActualValues[7] };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
case 7:
{
int[] data = { motorsActualValues[0], motorsActualValues[1], motorsActualValues[2], motorsActualValues[3], motorsActualValues[4], motorsActualValues[5], intensity, motorsActualValues[7] };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
case 8:
{
int[] data = { motorsActualValues[0], motorsActualValues[1], motorsActualValues[2], motorsActualValues[3], motorsActualValues[4], motorsActualValues[5], motorsActualValues[6], intensity };
mspSetMotor = MultiWiiFunctions.mspSetMotor(data);
if (mspSetMotor != new byte[0])
{
result = true;
}
break;
}
default: break;
}
return(result);
}