To try this code, you need a file in the sketch directory called SerialPort.txt. The code writes to it the serial port and the baud rate. Mine looks like this: /dev/ttyUSB0;57600
You should also have an Arduino type board running MultiWii2.3 If nothing is connected to the USB port then clicking connect causes an error.
//import controlP5.*;
import processing.serial.Serial; // serial library
import controlP5.*; // controlP5 library
import processing.opengl.*;
//import java.util.List;
import java.lang.StringBuffer; // for efficient String concatemation
import javax.swing.SwingUtilities; // required for swing and EDT
import javax.swing.JFileChooser; // Saving dialogue
import javax.swing.filechooser.FileFilter; // for our configuration file filter "*.mwi"
import javax.swing.JOptionPane; // for message dialogue
//Added For Processing 2.0.x compabillity
import java.util.*;
import java.util.List;
import java.io.*;
float gx, gy, gz, ax, ay, az, magx, magy, magz, alt, head, angx, angy,
debug1, debug2, debug3, debug4,
angyLevelControl, angCalc;
int i, j; // enummerators
int byteRC_RATE, byteRC_EXPO, byteRollPitchRate, byteYawRate,
byteDynThrPID, byteThrottle_EXPO, byteThrottle_MID, byteSelectSetting,
cycleTime, i2cError,
version, versionMisMatch, horizonInstrSize,
GPS_distanceToHome, GPS_directionToHome,
GPS_numSat, GPS_fix, GPS_update, GPS_altitude, GPS_speed,
GPS_latitude, GPS_longitude,
init_com, graph_on, pMeterSum, intPowerTrigger, bytevbat;
float size = 38.0;
PrintWriter output;
BufferedReader reader;
int multiCapability = 1;
int multiType = 1;
float mot[] = new float[8],
servo[] = new float[8],
RCChan[] = new float[16];
boolean axGraph =true, ayGraph=true, azGraph=true, gxGraph=true, gyGraph=true, gzGraph=true, altGraph=true, headGraph=true, magxGraph =true, magyGraph=true, magzGraph=true,
debug1Graph = false, debug2Graph = false, debug3Graph = false, debug4Graph = false, hideDraw=false, GraphicsInited=false, gimbalConfig=false, flapperons=false,
flaps=false, InitServos=true;
boolean toggleServo=false, toggleWriteServo=false, toggleWing=false, toggleWriteWing=false, toggleLive=false, toggleWriteServoLive=false, toggleWriteWingLive=false,
toggleSaveHeli=false, toggleWaitHeli=false, toggleGimbal=false,
graphEnabled = false, Mag_=false, gimbal=false, servoStretch=false, camTrigger=false, ExportServo=false,
toggleTrigger=false, ServosActive=false;
cDataArray accPITCH = new cDataArray(200), accROLL = new cDataArray(200), accYAW = new cDataArray(200),
gyroPITCH = new cDataArray(200), gyroROLL = new cDataArray(200), gyroYAW = new cDataArray(200),
magxData = new cDataArray(200), magyData = new cDataArray(200), magzData = new cDataArray(200),
altData = new cDataArray(200), headData = new cDataArray(200),
debug1Data = new cDataArray(200), debug2Data = new cDataArray(200), debug3Data = new cDataArray(200),
debug4Data = new cDataArray(200);
MyPanel controlPanel;
String portnameFile="SerialPort.txt"; // Name of file for Autoconnect.
int GUI_BaudRate = 57600; // Default.
int SerialPort;
Serial g_serial;
int commListMax;
int tabHeight = 20;
cGraph g_graph;
int windowsX = 1000;
int windowsY = 540+tabHeight;
int xGraph = 10;
int yGraph = 325+tabHeight;
int xObj = 520;
int yObj = 293+tabHeight;
int xCompass = 920;
int yCompass = 341+tabHeight;
int xLevelObj = 920;
int yLevelObj = 80+tabHeight;
int xParam = 120;
int yParam = 5+tabHeight;
int xRC = 690;
int yRC = 10+tabHeight;
int xMot = 690;
int yMot = 155+tabHeight;
int xButton = 845;
int yButton = 231+tabHeight;
int xBox = 415;
int yBox = 10+tabHeight;
int xGPS = 853;
int yGPS = 438+tabHeight;
int xServ = 350;
int yServ = 20+tabHeight;
//int c_state = IDLE;
// coded by Eberhard Rensch
// Truncates a long port name for better (readable) display in the GUI
String shortifyPortName(String portName, int maxlen) {
String shortName = portName;
if (shortName.startsWith("/dev/cu.")) shortName = "";// only collect the corresponding tty. devices
return shortName;
/*
if(shortName.startsWith("/dev/")) shortName = shortName.substring(5);
if(shortName.startsWith("tty.")) shortName = shortName.substring(4); // get rid of leading tty. part of device name
if(portName.length()>maxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2));
if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices
return shortName;
*/
}
String Selected;
void setup() {
size(1000, 540, OPENGL);
frameRate(20);
JFrame frame =new JFrame("Controls");
frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
controlPanel = new MyPanel();
controlPanel.setOpaque(true); //content panes must be opaque
frame.setContentPane(controlPanel);
//Display the window.
frame.pack();
frame.setVisible(true);
g_graph = new cGraph(xGraph+110, yGraph, 480, 200);
// End of setup()
}
/******************************* Multiwii Serial Protocol **********************/
private static final String MSP_HEADER = "$M<";
private static final int
MSP_IDENT =100,
MSP_STATUS =101,
MSP_RAW_IMU =102,
MSP_SERVO =103,
MSP_MOTOR =104,
MSP_RC =105,
MSP_RAW_GPS =106,
MSP_COMP_GPS =107,
MSP_ATTITUDE =108,
MSP_ALTITUDE =109,
MSP_ANALOG =110,
MSP_RC_TUNING =111,
MSP_PID =112,
MSP_BOX =113,
MSP_MISC =114,
MSP_MOTOR_PINS =115,
MSP_BOXNAMES =116,
MSP_PIDNAMES =117,
MSP_SERVO_CONF =120,
MSP_SET_RAW_RC =200,
MSP_SET_RAW_GPS =201,
MSP_SET_PID =202,
MSP_SET_BOX =203,
MSP_SET_RC_TUNING =204,
MSP_ACC_CALIBRATION =205,
MSP_MAG_CALIBRATION =206,
MSP_SET_MISC =207,
MSP_RESET_CONF =208,
MSP_SELECT_SETTING =210,
MSP_SET_HEAD =211, // Not used
MSP_SET_SERVO_CONF =212,
MSP_SET_MOTOR =214,
MSP_BIND =240,
MSP_EEPROM_WRITE =250,
MSP_DEBUGMSG =253,
MSP_DEBUG =254
;
public static final int
IDLE = 0,
HEADER_START = 1,
HEADER_M = 2,
HEADER_ARROW = 3,
HEADER_SIZE = 4,
HEADER_CMD = 5,
HEADER_ERR = 6
;
int c_state = IDLE;
boolean err_rcvd = false;
byte checksum=0;
byte cmd;
int offset=0, dataSize=0;
byte[] inBuf = new byte[256];
int p;
int read32() {
return (inBuf[p++]&0xff) + ((inBuf[p++]&0xff)<<8) + ((inBuf[p++]&0xff)<<16) + ((inBuf[p++]&0xff)<<24);
}
int read16() {
return (inBuf[p++]&0xff) + ((inBuf[p++])<<8);
}
int read8() {
return inBuf[p++]&0xff;
}
int mode;
boolean toggleRead = false, toggleReset = false, toggleCalibAcc = false, toggleCalibMag = false, toggleWrite = false,
toggleRXbind = false, toggleSetSetting = false, toggleVbat=true, toggleMotor=false, motorcheck=true;
//send msp without payload
private List<Byte> requestMSP(int msp) {
return requestMSP( msp, null);
}
//send multiple msp without payload
private List<Byte> requestMSP (int[] msps) {
List<Byte> s = new LinkedList<Byte>();
for (int m : msps) {
s.addAll(requestMSP(m, null));
}
return s;
}
//send msp with payload
private 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);
}
void sendRequestMSP(List<Byte> msp) {
byte[] arr = new byte[msp.size()];
int i = 0;
for (byte b : msp) {
arr[i++] = b;
}
g_serial.write(arr); // send the complete byte sequence in one go
}
public void evaluateCommand(byte cmd, int dataSize) {
int i;
int icmd = (int)(cmd&0xFF);
switch(icmd) {
case MSP_IDENT:
version = read8();
multiType = read8();
read8(); // MSP version
multiCapability = read32();// capability
/* if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");}
if ((multiCapability&4)>0) controlP5.addTab("Motors").show();
if ((multiCapability&8)>0) flaps=true;
if (!GraphicsInited) create_ServoGraphics(); */
break;
case MSP_STATUS:
cycleTime = read16();
i2cError = read16();
present = read16();
mode = read32();
break;
case MSP_RAW_IMU:
ax = read16();
ay = read16();
az = read16();
ActiveTab = "Motors";
if (ActiveTab=="Motors") { // Show unfilterd values in graph.
gx = read16();
gy = read16();
gz = read16();
magx = read16();
magy = read16();
magz = read16();
} else {
gx = read16()/8;
gy = read16()/8;
gz = read16()/8;
magx = read16()/3;
magy = read16()/3;
magz = read16()/3;
}
break;
case MSP_SERVO:
for (i=0; i<8; i++) servo[i] = read16();
break;
case MSP_MOTOR:
for (i=0; i<8; i++) {
mot[i] = read16();
}
// if (multiType == SINGLECOPTER)servo[7]=mot[0];
// if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];}
break;
case MSP_RC:
for (i=0; i<8; i++) {
RCChan[i]=read16();
////////////////////////////// TX_StickSlider[i].setValue(RCChan[i]);
}
break;
case MSP_RAW_GPS:
GPS_fix = read8();
GPS_numSat = read8();
GPS_latitude = read32();
GPS_longitude = read32();
GPS_altitude = read16();
GPS_speed = read16();
break;
case MSP_COMP_GPS:
GPS_distanceToHome = read16();
GPS_directionToHome = read16();
GPS_update = read8();
break;
case MSP_ATTITUDE:
angx = read16()/10;
angy = read16()/10;
head = read16();
break;
case MSP_ALTITUDE:
alt = read32();
break;
case MSP_ANALOG:
bytevbat = read8();
pMeterSum = read16();
break;
case MSP_RC_TUNING:
byteRC_RATE = read8();
byteRC_EXPO = read8();
byteRollPitchRate = read8();
byteYawRate = read8();
byteDynThrPID = read8();
byteThrottle_MID = read8();
byteThrottle_EXPO = read8();
break;
case MSP_ACC_CALIBRATION:
break;
case MSP_MAG_CALIBRATION:
break;
case MSP_PID:
break;
case MSP_BOX:
break;
case MSP_BOXNAMES:
// create_checkboxes(new String(inBuf, 0, dataSize).split(";"));
break;
case MSP_PIDNAMES:
/* TODO create GUI elements from this message */
//System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize));
break;
case MSP_SERVO_CONF:
break;
case MSP_MISC:
break;
case MSP_MOTOR_PINS:
// for( i=0;i<8;i++) {byteMP[i] = read8();}
break;
case MSP_DEBUGMSG:
while (dataSize-- > 0) {
char c = (char)read8();
if (c != 0) {
System.out.print( c );
}
}
break;
case MSP_DEBUG:
debug1 = read16();
debug2 = read16();
debug3 = read16();
debug4 = read16();
break;
default:
//println("Don't know how to handle reply "+icmd);
}
}
private int present = 0;
int time, time2, time3, time4;
void draw() {
background(0);
List<Character> payload;
int i, aa;
float val, inter, a, b, h;
int c;
if (init_com==1 && graph_on==1) {
time=millis();
if ((time-time4)>40 ) {
time4=time;
}
if ((time-time2)>40 && ! toggleRead && ! toggleWrite && ! toggleSetSetting) {
time2=time;
int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC, MSP_RAW_GPS, MSP_COMP_GPS, MSP_ALTITUDE, MSP_DEBUGMSG, MSP_DEBUG};
sendRequestMSP(requestMSP(requests));
println(angx, angy);
}
if ((time-time3)>20 && ! toggleRead && ! toggleWrite && ! toggleSetSetting) {
sendRequestMSP(requestMSP(MSP_ATTITUDE));
time3=time;
}
if (toggleReset) {
toggleReset=false;
toggleRead=true;
sendRequestMSP(requestMSP(MSP_RESET_CONF));
}
if (toggleRead) {
toggleRead=false;
int[] requests = {MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_BOX, MSP_MISC, MSP_IDENT, MSP_MOTOR_PINS }; // MSP_PIDNAMES
sendRequestMSP(requestMSP(requests));
// buttonWRITE.setColorBackground(green_);
// buttonSETTING.setColorBackground(green_);
// confSelectSetting.setColorBackground(green_);
}
if (toggleSetSetting) {
toggleSetSetting=false;
toggleRead=true;
payload = new ArrayList<Character>();
// payload.add(char( round(confSelectSetting.value())) );
sendRequestMSP(requestMSP(MSP_SELECT_SETTING, payload.toArray( new Character[payload.size()]) ));
}
if (toggleCalibAcc) {
toggleCalibAcc=false;
sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION));
}
if (toggleCalibMag) {
toggleCalibMag=false;
sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION));
}
if (toggleWrite) {
toggleWrite=false;
// MSP_SET_RC_TUNING
payload = new ArrayList<Character>();
sendRequestMSP(requestMSP(MSP_SET_RC_TUNING, payload.toArray( new Character[payload.size()]) ));
// MSP_SET_PID
payload = new ArrayList<Character>();
sendRequestMSP(requestMSP(MSP_SET_PID, payload.toArray(new Character[payload.size()])));
// MSP_SET_BOX
payload = new ArrayList<Character>();
sendRequestMSP(requestMSP(MSP_SET_BOX, payload.toArray(new Character[payload.size()])));
// MSP_SET_MISC
payload = new ArrayList<Character>();
sendRequestMSP(requestMSP(MSP_SET_MISC, payload.toArray(new Character[payload.size()])));
}
if (toggleRXbind) {
toggleRXbind=false;
sendRequestMSP(requestMSP(MSP_BIND));
bSTOP();
InitSerial(9999);
}
while (g_serial.available()>0) {
c = (g_serial.read());
println(angx, angy);
if (c_state == IDLE) {
c_state = (c=='$') ? HEADER_START : IDLE;
} else if (c_state == HEADER_START) {
c_state = (c=='M') ? HEADER_M : IDLE;
} else if (c_state == HEADER_M) {
if (c == '>') {
c_state = HEADER_ARROW;
} else if (c == '!') {
c_state = HEADER_ERR;
} else {
c_state = IDLE;
}
} else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) {
/* is this an error message? */
err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */
dataSize = (c&0xFF);
/* reset index variables */
p = 0;
offset = 0;
checksum = 0;
checksum ^= (c&0xFF);
/* the command is to follow */
c_state = HEADER_SIZE;
} else if (c_state == HEADER_SIZE) {
cmd = (byte)(c&0xFF);
checksum ^= (c&0xFF);
c_state = HEADER_CMD;
} else if (c_state == HEADER_CMD && offset < dataSize) {
checksum ^= (c&0xFF);
inBuf[offset++] = (byte)(c&0xFF);
} else if (c_state == HEADER_CMD && offset >= dataSize) {
/* compare calculated and transferred checksum */
if ((checksum&0xFF) == (c&0xFF)) {
if (err_rcvd) {
//System.err.println("Copter did not understand request type "+c);
} else {
/* we got a valid response packet, evaluate it */
evaluateCommand(cmd, (int)dataSize);
}
} else {
System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF));
System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {");
for (i=0; i<dataSize; i++) {
if (i!=0) {
System.err.print(' ');
}
System.out.print((inBuf[i] & 0xFF));
}
System.out.println("} ["+c+"]");
System.out.println(new String(inBuf, 0, dataSize));
}
c_state = IDLE;
}
}
}
}
String ActiveTab="default";
//Start of functions and classes//
public void bSTART() {
// if(graphEnabled == false) {return;}
graph_on=1;
toggleRead=true;
g_serial.clear();
}
public void bSTOP() {
graph_on=0;
}
public void READ() {
toggleLive = false ;
toggleRead = true;
toggleVbat=true;
}
void InitSerial(float portValue) {
///// if (portValue >=0){
println(portValue);
// if (portValue < commListMax) {
// String portPos = Selected; //Serial.list()[int(portValue)];
String portPos = Serial.list()[int(portValue)];
println(portPos, Selected);
//////////////// txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8));
g_serial = new Serial(this, portPos, GUI_BaudRate);
SaveSerialPort(portPos);
init_com=1;
////////////////// commListbox.setColorBackground(green_);
// buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_);
// buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_);
// buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_);
// graphEnabled = true;
g_serial.buffer(256);
///////////////////// btnQConnect.hide();
// } else {
////////////////// txtlblWhichcom.setValue("Comm Closed");
/////////////init_com=0;
////////////// buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_);
////////////////////graphEnabled = false;
//////////////////init_com=0;
///////////////////g_serial.stop();
///////////////// btnQConnect.show();
// }
}
void SaveSerialPort(String port ) {
output = createWriter(portnameFile);
output.print( port + ';' + GUI_BaudRate); // Write the comport to the file
output.flush(); // Writes the remaining data to the file
output.close(); // Finishes the file
}
public void bQCONN() {
ReadSerialPort();
InitSerial(SerialPort);
bSTART();
toggleRead=true;
}
void ReadSerialPort() {
reader = createReader(portnameFile);
String line;
try {
line = reader.readLine();
}
catch (IOException e) {
e.printStackTrace();
line = null;
}
if (line == null) {
// Stop reading because of an error or file is empty
// Roll on with no input
/////////////// btnQConnect.hide();
return;
} else {
String[] pieces = split(line, ';');
int Port = int(pieces[0]);
GUI_BaudRate= int(pieces[1]);
if (commListMax==1) {
}
String pPort=pieces[0];
for ( i=0; i<commListMax; i++) {
String[] pn = match(shortifyPortName(Serial.list()[i], 13), pieces[0]);
if ( pn !=null) {
SerialPort=i;
}
}
}
}
class cDataArray {
float[] m_data;
int m_maxSize, m_startIndex = 0, m_endIndex = 0, m_curSize;
cDataArray(int maxSize) {
m_maxSize = maxSize;
m_data = new float[maxSize];
}
void addVal(float val) {
m_data[m_endIndex] = val;
m_endIndex = (m_endIndex+1)%m_maxSize;
if (m_curSize == m_maxSize) {
m_startIndex = (m_startIndex+1)%m_maxSize;
} else {
m_curSize++;
}
}
float getVal(int index) {
return m_data[(m_startIndex+index)%m_maxSize];
}
int getCurSize() {
return m_curSize;
}
int getMaxSize() {
return m_maxSize;
}
float getMaxVal() {
float res = 0.0;
for ( i=0; i<m_curSize-1; i++) if ((m_data[i] > res) || (i==0)) res = m_data[i];
return res;
}
float getMinVal() {
float res = 0.0;
for ( i=0; i<m_curSize-1; i++) if ((m_data[i] < res) || (i==0)) res = m_data[i];
return res;
}
float getRange() {
return getMaxVal() - getMinVal();
}
}
// This class takes the data and helps graph it
class cGraph {
float m_gWidth, m_gHeight, m_gLeft, m_gBottom, m_gRight, m_gTop;
cGraph(float x, float y, float w, float h) {
m_gWidth = w;
m_gHeight = h;
m_gLeft = x;
m_gBottom = y;
m_gRight = x + w;
m_gTop = y + h;
}
void drawGraphBox() {
stroke(0, 0, 0);
rectMode(CORNERS);
rect(m_gLeft, m_gBottom, m_gRight, m_gTop);
}
void drawLine(cDataArray data, float minRange, float maxRange) {
float graphMultX = m_gWidth/data.getMaxSize();
float graphMultY = m_gHeight/(maxRange-minRange);
for ( i=0; i<data.getCurSize()-1; ++i) {
float x0 = i*graphMultX+m_gLeft;
float y0 = m_gTop-(((data.getVal(i)-(maxRange+minRange)/2)+(maxRange-minRange)/2)*graphMultY);
float x1 = (i+1)*graphMultX+m_gLeft;
float y1 = m_gTop-(((data.getVal(i+1)-(maxRange+minRange)/2 )+(maxRange-minRange)/2)*graphMultY);
line(x0, y0, x1, y1);
}
}
}