/// <summary>
/// Send an MSP request with a payload
/// </summary>
/// <param name="cmd">The MSP command to be sent</param>
/// <param name="payload">The payload of the command</param>
void IProtocol.SendRequestMSP(MSPCode cmd, byte[] payload)
{
if (Protocol != null)
{
Protocol.SendRequestMSP(cmd, payload);
}
}
/// <summary>
/// Send an MSP request without a payload
/// </summary>
/// <param name="cmd">The MSP command to be sent</param>
void IProtocol.SendRequestMSP(MSPCode cmd)
{
if (Protocol != null)
{
Protocol.SendRequestMSP(cmd);
}
}
//
// Protected methods
//
/// <summary>
/// Given an explict MSP command, the passed buffer is read and required information is extracted
/// </summary>
/// <param name="cmd">The MSP command wo decode the byte data as</param>
/// <param name="inBuffer">A buffer containing the MSP command response payload</param>
/// <param name="offset">An offset of for the payload buffer</param>
protected void EvaluateCommand(MSPCode cmd, byte[] inBuffer, int offset)
{
switch (cmd)
{
// API Version:
// - MSP Protocol Version
// - API Version
case MSPCode.MSP_API_VERSION:
FlightController.Configuration.MSPVersion = Read8(inBuffer, ref offset); // MSP Protocol Version
FlightController.Configuration.APIVersion = Read8(inBuffer, ref offset) + "." + Read8(inBuffer, ref offset) + ".0"; // API verison
break;
// Raw IMU:
// - Accelerometers [x, y, z]
// - Gyroscope [x, y, z]
case MSPCode.MSP_RAW_IMU:
// Scaling assumed at 512
FlightController.SensorData.Accelerometer.X = Read16(inBuffer, ref offset) / 512f;
FlightController.SensorData.Accelerometer.Y = Read16(inBuffer, ref offset) / 512f;
FlightController.SensorData.Accelerometer.Z = Read16(inBuffer, ref offset) / 512f;
// Correctly scaled
FlightController.SensorData.Gyroscope.X = Read16(inBuffer, ref offset) * (4 / 16.4f);
FlightController.SensorData.Gyroscope.Y = Read16(inBuffer, ref offset) * (4 / 16.4f);
FlightController.SensorData.Gyroscope.Z = Read16(inBuffer, ref offset) * (4 / 16.4f);
break;
// Raw GPS Data
// - Fix
// - Number of satallites
// - Latitude
// - Longitude
// - Altitude
// - Speed
// - Ground course
case MSPCode.MSP_RAW_GPS:
FlightController.GPSData.Fix = Read8(inBuffer, ref offset);
FlightController.GPSData.NumberOfSatellites = Read8(inBuffer, ref offset);
FlightController.GPSData.Latitude = Read32(inBuffer, ref offset) / 10000000d;
FlightController.GPSData.Longitude = Read32(inBuffer, ref offset) / 10000000d;
FlightController.GPSData.Altitude = ReadU16(inBuffer, ref offset);
FlightController.GPSData.Speed = ReadU16(inBuffer, ref offset);
FlightController.GPSData.GroundCourse = ReadU16(inBuffer, ref offset);
break;
case MSPCode.MSP_ATTITUDE:
FlightController.SensorData.Kinematics.X = Read16(inBuffer, ref offset) / 10.0f;
FlightController.SensorData.Kinematics.Y = Read16(inBuffer, ref offset) / 10.0f;
FlightController.SensorData.Kinematics.Z = Read16(inBuffer, ref offset);
FlightController.OnKinematicDataChanged(EventArgs.Empty);
break;
default: // Command evaluation has not been implemented
break;
}
}
/// <summary>
/// Send an MSP request with a payload
/// </summary>
/// <param name="cmd">The MSP command to be sent</param>
/// <param name="payload">The payload of the command</param>
public void SendRequestMSP(MSPCode cmd, byte[] payload)
{
// Is msp cmd valid
if (cmd == MSPCode.Undefined)
{
return;
}
// Get bytes from MSP Header string
List <byte> bf = new List <byte>();
foreach (char c in MSP_HEADER.ToCharArray())
{
bf.Add((byte)c);
}
// Add Payload size
byte payloadSize = (byte)((payload != null ? payload.Length : 0) & 0xFF);
bf.Add(payloadSize);
// Add MSP Command
bf.Add((byte)((byte)cmd & 0xFF));
// Calculate checksum
byte checksum = 0;
checksum ^= (byte)(payloadSize & 0xFF); // Payload Size
checksum ^= (byte)((byte)cmd & 0xFF); // MSP Command
// Add payload to MSP Request and checksum
if (payload != null)
{
foreach (byte b in payload)
{
bf.Add((byte)(b & 0xFF));
checksum ^= (byte)(b & 0xFF);
}
}
// Add Checksum to request
bf.Add(checksum);
// Return request
SendRequestMSP(bf.ToArray());
}
//
// Constructors
//
/// <summary>
/// Class constructor
/// </summary>
/// <param name="protocol">The protocol to connect to the cleanflight board/device</param>
public FlightController(IProtocol _protocol)
{
protocol = (Protocol.Protocol)_protocol;
protocol.FlightController = this;
RefreshData = new MSPCode[]
{
MSPCode.MSP_RAW_IMU,
MSPCode.MSP_RAW_GPS,
MSPCode.MSP_ATTITUDE
};
Configuration = new FlightControllerConfiguration();
FeatureConfiguration = new FeatureConfiguration();
MixerConfiguration = new MixerConfiguration();
BoardAlignmentConfiguration = new BoardAlignmentConfiguration();
RC = new RC();
RCTuning = new RCTuning();
GPSData = new GPSData();
SensorData = new SensorData();
refreshTimer = new Timer(REFRESH_INTERVAL);
refreshTimer.Elapsed += RefreshTimer_Elapsed;
}
/// <summary>
/// Send an MSP request without a payload
/// </summary>
/// <param name="cmd">The MSP command to be sent</param>
public void SendRequestMSP(MSPCode cmd)
{
SendRequestMSP(cmd, null);
}
private void MspResponseQueue_CollectionChanged(object sender, System.Collections.Specialized.NotifyCollectionChangedEventArgs e)
{
// Proccess the received bytes
while (!mspResponseQueue.IsEmpty)
{
lock (mspResponseQueue)
{
byte[] mspResponse;
if (mspResponseQueue.TryDequeue(out mspResponse))
{
// Define variables
byte[] inBuf = new byte[IN_BUFFER_SIZE];
MSPState mspState = MSPState.IDLE;
bool errorReceived = false;
byte checksum = 0;
MSPCode cmd = MSPCode.Undefined;
int offset = 0, dataSize = 0;
// Process each byte in response
foreach (byte c in mspResponse)
{
if (IsOpen)
{
// Header
if (mspState == MSPState.IDLE)
{
mspState = (c == '$') ? MSPState.HEADER_START : MSPState.IDLE;
}
// 'M'
else if (mspState == MSPState.HEADER_START)
{
mspState = (c == 'M') ? MSPState.HEADER_M : MSPState.IDLE;
}
// Arrow or '!'
else if (mspState == MSPState.HEADER_M)
{
if (c == '>')
{
mspState = MSPState.HEADER_ARROW;
}
else if (c == '!')
{
mspState = MSPState.HEADER_ERR;
}
else
{
mspState = MSPState.IDLE;
}
}
// Payload size
else if (mspState == MSPState.HEADER_ARROW || mspState == MSPState.HEADER_ERR)
{
// Is this an error message?
errorReceived = (mspState == MSPState.HEADER_ERR);
// Now expecting the payload size
dataSize = (c & 0xFF);
// Reset index variables
offset = 0;
checksum = 0;
checksum ^= (byte)(c & 0xFF);
// The command is to follow
mspState = MSPState.HEADER_SIZE;
}
// MSP Command
else if (mspState == MSPState.HEADER_SIZE)
{
cmd = (MSPCode)(byte)(c & 0xFF);
checksum ^= (byte)(c & 0xFF);
mspState = MSPState.HEADER_CMD;
}
else if (mspState == MSPState.HEADER_CMD && offset < dataSize)
{
checksum ^= (byte)(c & 0xFF);
inBuf[offset++] = (byte)(c & 0xFF);
}
else if (mspState == MSPState.HEADER_CMD && offset >= dataSize)
{
// Compare calculated and transferred checksum
if ((checksum & 0xFF) == (c & 0xFF))
{
if (errorReceived)
{
Console.Out.WriteLine("Board did not understand request type " + c);
}
else
{
// We got a valid response packet, evaluate it
EvaluateCommand(cmd, inBuf, 0);
}
}
else
{
Console.Out.WriteLine("invalid checksum for command " + ((byte)cmd & 0xFF) + ": " + (checksum & 0xFF) + " expected, got " + (c & 0xFF));
}
// Ready for anymore responses
mspState = MSPState.IDLE;
}
}
}
}
}
}
}
