Hello guys i’m trying to read altitude attitude using C#, even if i sent a request the only think i receive is the heartbeat.
can someone help please.
it have been 1 week i’m working on that.
this is my code check it please ?
public int i;
MAVLink.MAVLinkMessage t = new MAVLink.MAVLinkMessage();
SerialPort mySerialPort = new SerialPort("COM14");
public static MAVLink.Mavlink_heartbeat_t hb = new MAVLink.Mavlink_heartbeat_t();
List<byte> heartbeatData = new List<byte>();
public Form1()
{
InitializeComponent();
OpenSerialPort();
}
private void OpenSerialPort()
{
mySerialPort.BaudRate = 57000;
mySerialPort.Parity = Parity.None;
mySerialPort.StopBits = StopBits.One;
mySerialPort.DataBits = 8;
mySerialPort.Handshake = Handshake.None;
try
{
mySerialPort.DataReceived += SerialPortDataReceived;
mySerialPort.Open();
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine(ex.Message + ex.StackTrace);
}
}
private void button1_Click(object sender, EventArgs e)
{
}
private void SerialPortDataReceived(object sender, SerialDataReceivedEventArgs e)
{
Mavlink_request_data(30, 0x02);//send request to read attitude id==30
var serialPort = (SerialPort)sender;
var data = serialPort.ReadByte();
Console.WriteLine("Data : " + data);
heartbeatData.Add((byte)data);
}
void Mavlink_request_data(byte id, byte rate)
{
MAVLink.MAVLinkMessage mAVLinkMessage = new MAVLink.MAVLinkMessage();
uint[] buffer = new uint[MAVLink.MAVLINK_MAX_PACKET_LEN];
MAVLink.MAVLinkParamList mAVLinkParams = new MAVLink.MAVLinkParamList();
MAVLink.Mavlink_request_data_stream_t req = new MAVLink.Mavlink_request_data_stream_t
{
target_system = 1,
target_component = 1,
req_message_rate = rate,
start_stop = 1, // start
req_stream_id = (byte)id // id
};
// send each one twice.
GeneratePacket((byte)MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
GeneratePacket((byte)MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
}
void GeneratePacket(byte messageType, object indata)
{
if (!mySerialPort.IsOpen)
{
return;
}
byte[] data;
data = MavlinkUtil.StructureToByteArray(indata);
//Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
byte[] packet = new byte[data.Length + 6 + 2];
packet[0] = 254;
packet[1] = (byte)data.Length;
packet[2] = (byte)0; //numero de la sequence ici
packet[3] = 255; // this is always 255 - MYGCS
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
packet[5] = messageType;
int i = 6;
foreach (byte b in data)
{
packet[i] = b;
i++;
}
ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[messageType], checksum);
byte ck_a = (byte)(checksum & 0xFF); ///< High byte
byte ck_b = (byte)(checksum >> 8); ///< Low byte
packet[i] = ck_a;
i += 1;
packet[i] = ck_b;
i += 1;
if (mySerialPort.IsOpen)
{
mySerialPort.Write(packet, 0, i);
}
}
