I would like to test the Multiwii Serial Protocol (MSP) individually.
The way I am doing this is that I have written a simple program in processing as bellow:
Code: Select all
import processing.serial.*; // serial library
import java.util.*;
import java.lang.*;
String MSP_HEADER = "$M<";
int
MSP_IDENT =100,
MSP_STATUS =101,
MSP_RAW_IMU =102;
Serial myPort;
//flag variable is used to send the request just once
boolean flag;
long delay = 0;
//This function has been copied from the multiwii conf source code
List<Byte> requestMSP (int msp, Character[] payload) {
if(msp < 0) {
return null;
}
List<Byte> bf = new LinkedList<Byte>();
for (byte c : MSP_HEADER.getBytes()) {
bf.add( c );
}
byte checksum=0;
byte pl_size = (byte)((payload != null ? int(payload.length) : 0)&0xFF);
bf.add(pl_size);
checksum ^= (pl_size&0xFF);
bf.add((byte)(msp & 0xFF));
checksum ^= (msp&0xFF);
if (payload != null) {
for (char c :payload){
bf.add((byte)(c&0xFF));
checksum ^= (c&0xFF);
}
}
bf.add(checksum);
return (bf);
}
//This function has been copied from the multiwii conf source code
void sendRequestMSP(List<Byte> msp) {
byte[] arr = new byte[msp.size()];
println("Message is : ");
println(msp);
int i = 0;
for (byte b: msp) {
arr[i++] = b;
}
myPort.write(arr); // send the complete byte sequence in one go
}
void setup(){
printArray(Serial.list());
myPort = new Serial(this, Serial.list()[0], 115200);
flag = true;
}
void draw(){
myPort.buffer(256);
//myPort.clear();
int c;
int i = 0;
//Send request to multiwii
if (flag){
sendRequestMSP(requestMSP (MSP_STATUS,null));
flag = false;
println("Waiting to receive...");
//make delay
for (i=0; i<2; i++){
delay = 0;
while (delay < 10000){
delay += 1;
}
}
//get response from multiwii
while (myPort.available()>0) {
c = (myPort.read());
println(c);
}
}
}
what I am doing is that I am making a MSP_STATUS request and the send it toward the arduino UNO which has been programed with the multiwii program in advance.
I expect to get the response from the arduino back but no response is returned and printed in my computer.
What is wrong with it ?? How can I create a MSP message myself and send it to arduino and get the response back ??
Any alternative way OR Any program has been written previously ?
Thanks.