public class Flight extends BaseViewPart { public static final String ID = "it.xbladecopter.flight"; private LogManager logger = LogManager.getInstance(); private static Group sensorData; private static Text rollText; private static Text pitchText; private static Text headingText; private static Text accXText; private static Text accYText; private static Text accZText; private static Text awXText; private static Text awYText; private static Text awZText; private static Text magXText; private static Text magZText; private static Text magYText; private static Text m0Text; private static Text m1Text; private static Text m2Text; private static Text m3Text; private static CLabel gM0; private static CLabel gM1; private static CLabel gM2; private static CLabel gM3; @Override public void createPartControl(Composite parent) { logger.logInfo("Starting flight data view...", this.getClass()); sensorData = new Group(parent, SWT.NONE); sensorData.setLayout(new GridLayout(1, false)); GridData layoutData = new GridData(); layoutData.horizontalSpan = 1; layoutData.grabExcessVerticalSpace = true; sensorData.setLayoutData(layoutData); sensorData.setText("Flight Data"); GridData layoutGradient = new GridData(GridData.HORIZONTAL_ALIGN_FILL); layoutGradient.widthHint = 40; layoutGradient.heightHint = 15; layoutGradient.horizontalSpan = 1; GridData layoutSensor = new GridData(); layoutSensor.minimumWidth = 20; layoutSensor.widthHint = 50; GridData layoutMotor = new GridData(); layoutMotor.minimumWidth = 20; layoutMotor.widthHint = 30; GridData layoutLabelAttitude = new GridData(); layoutLabelAttitude.minimumWidth = 20; layoutLabelAttitude.widthHint = 50; GridData layoutLabelS = new GridData(); layoutLabelS.minimumWidth = 20; layoutLabelS.widthHint = 30; GridData layoutLabel = new GridData(); layoutLabel.minimumWidth = 20; layoutLabel.widthHint = 50; Group angleData = new Group(sensorData, SWT.NONE); angleData.setLayout(new GridLayout(4, true)); angleData.setText("Attitude Angles (°)"); angleData.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); Label label = new Label(angleData, SWT.NONE); label.setText("Roll: "); label.setLayoutData(layoutLabelAttitude); rollText = new Text(angleData, SWT.NONE); rollText.setEnabled(false); rollText.setLayoutData(layoutSensor); rollText.setText("0.0"); label = new Label(angleData, SWT.NONE); label.setText("Pitch: "); label.setLayoutData(layoutLabelAttitude); pitchText = new Text(angleData, SWT.NONE); pitchText.setEnabled(false); pitchText.setLayoutData(layoutSensor); pitchText.setText("0.0"); label = new Label(angleData, SWT.NONE); label.setText("Heading: "); label.setLayoutData(layoutLabelAttitude); headingText = new Text(angleData, SWT.NONE); headingText.setEnabled(false); headingText.setLayoutData(layoutSensor); headingText.setText("0.0"); Group dof1Data = new Group(sensorData, SWT.NONE); dof1Data.setLayout(new GridLayout(6, false)); dof1Data.setText("Acceleration (g)"); dof1Data.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); label = new Label(dof1Data, SWT.NONE); label.setText("x: "); label.setLayoutData(layoutLabelS); accXText = new Text(dof1Data, SWT.NONE); accXText.setEnabled(false); accXText.setLayoutData(layoutSensor); accXText.setText("0.0"); label = new Label(dof1Data, SWT.NONE); label.setText("y: "); label.setLayoutData(layoutLabelS); accYText = new Text(dof1Data, SWT.NONE); accYText.setEnabled(false); accYText.setLayoutData(layoutSensor); accYText.setText("0.0"); label = new Label(dof1Data, SWT.NONE); label.setText("z: "); label.setLayoutData(layoutLabelS); accZText = new Text(dof1Data, SWT.NONE); accZText.setEnabled(false); accZText.setLayoutData(layoutSensor); accZText.setText("0.0"); Group dof2Data = new Group(sensorData, SWT.NONE); dof2Data.setLayout(new GridLayout(6, false)); dof2Data.setText("Angular Velocity (°/s)"); dof2Data.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); label = new Label(dof2Data, SWT.NONE); label.setText("x: "); label.setLayoutData(layoutLabelS); awXText = new Text(dof2Data, SWT.NONE); awXText.setEnabled(false); awXText.setLayoutData(layoutSensor); awXText.setText("0.0"); label = new Label(dof2Data, SWT.NONE); label.setText("y: "); label.setLayoutData(layoutLabelS); awYText = new Text(dof2Data, SWT.NONE); awYText.setEnabled(false); awYText.setLayoutData(layoutSensor); awYText.setText("0.0"); label = new Label(dof2Data, SWT.NONE); label.setText("z: "); label.setLayoutData(layoutLabelS); awZText = new Text(dof2Data, SWT.NONE); awZText.setEnabled(false); awZText.setLayoutData(layoutSensor); awZText.setText("0.0"); Group dof3Data = new Group(sensorData, SWT.NONE); dof3Data.setLayout(new GridLayout(6, false)); dof3Data.setText("Magnetic Field (uGauss)"); dof3Data.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); label = new Label(dof3Data, SWT.NONE); label.setText("x: "); label.setLayoutData(layoutLabelS); magXText = new Text(dof3Data, SWT.NONE); magXText.setEnabled(false); magXText.setLayoutData(layoutSensor); magXText.setText("0.0"); label = new Label(dof3Data, SWT.NONE); label.setText("y: "); label.setLayoutData(layoutLabelS); magYText = new Text(dof3Data, SWT.NONE); magYText.setEnabled(false); magYText.setLayoutData(layoutSensor); magYText.setText("0.0"); label = new Label(dof3Data, SWT.NONE); label.setText("z: "); label.setLayoutData(layoutLabelS); magZText = new Text(dof3Data, SWT.NONE); magZText.setEnabled(false); magZText.setLayoutData(layoutSensor); magZText.setText("0.0"); Group control = new Group(sensorData, SWT.NONE); control.setLayout(new GridLayout(6, false)); layoutData = new GridData(GridData.HORIZONTAL_ALIGN_FILL); layoutData.horizontalSpan = 6; control.setLayoutData(layoutData); control.setText("Motors Speed"); label = new Label(control, SWT.NONE); label.setText("Front (R): "); label.setLayoutData(layoutLabel); gM0 = new CLabel(control, SWT.SHADOW_IN); gM0.setLayoutData(layoutGradient); gM0.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {100, 100}); m0Text = new Text(control, SWT.NONE); m0Text.setEnabled(false); m0Text.setLayoutData(layoutMotor); m0Text.setText("0.0"); label = new Label(control, SWT.NONE); label.setText("Front (L): "); label.setLayoutData(layoutLabel); gM1 = new CLabel(control, SWT.SHADOW_IN); gM1.setLayoutData(layoutGradient); gM1.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {100, 100}); m1Text = new Text(control, SWT.NONE); m1Text.setEnabled(false); m1Text.setLayoutData(layoutMotor); m1Text.setText("0.0"); label = new Label(control, SWT.NONE); label.setText("Back (R): "); label.setLayoutData(layoutLabel); gM2 = new CLabel(control, SWT.SHADOW_IN); gM2.setLayoutData(layoutGradient); gM2.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {100, 100}); m2Text = new Text(control, SWT.NONE); m2Text.setEnabled(false); m2Text.setLayoutData(layoutMotor); m2Text.setText("0.0"); label = new Label(control, SWT.NONE); label.setText("Back (L): "); label.setLayoutData(layoutLabel); gM3 = new CLabel(control, SWT.SHADOW_IN); gM3.setLayoutData(layoutGradient); gM3.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {100, 100}); m3Text = new Text(control, SWT.NONE); m3Text.setEnabled(false); m3Text.setLayoutData(layoutMotor); m3Text.setText("0.0"); } public static void updateFlight() { try { if (!sensorData.isDisposed() && sensorData.isVisible()) { rollText.setText(String.format("%3.3f", rpy.getRoll())); pitchText.setText(String.format("%3.3f", rpy.getPitch())); headingText.setText(String.format("%3.3f", rpy.getHeading())); accXText.setText(String.format("%3.3f", rpy.getAccX())); accYText.setText(String.format("%3.3f", rpy.getAccY())); accZText.setText(String.format("%3.3f", rpy.getAccZ())); awXText.setText(String.format("%3.3f", rpy.getGyroX())); awYText.setText(String.format("%3.3f", rpy.getGyroY())); awZText.setText(String.format("%3.3f", rpy.getGyroZ())); magXText.setText(String.format("%3.3f", rpy.getMagX())); magYText.setText(String.format("%3.3f", rpy.getMagY())); magZText.setText(String.format("%3.3f", rpy.getMagZ())); if (rpy.getM3() <= 100 && rpy.getM3() >= 0) { gM1.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {(int) (100 - rpy.getM3()), 100}); m1Text.setText(String.format("%3.1f", rpy.getM3())); gM1.layout(); } if (rpy.getM2() <= 100 && rpy.getM2() >= 0) { gM0.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {(int) (100 - rpy.getM2()), 100}); m0Text.setText(String.format("%3.1f", rpy.getM2())); gM0.layout(); } if (rpy.getM0() <= 100 && rpy.getM0() >= 0) { gM3.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {(int) (100 - rpy.getM0()), 100}); m3Text.setText(String.format("%3.1f", rpy.getM0())); gM3.layout(); } if (rpy.getM1() <= 100 && rpy.getM1() >= 0) { gM2.setBackground( new Color[] { Display.getDefault().getSystemColor(SWT.COLOR_GREEN), Display.getDefault().getSystemColor(SWT.COLOR_YELLOW), Display.getDefault().getSystemColor(SWT.COLOR_RED) }, new int[] {(int) (100 - rpy.getM1()), 100}); m2Text.setText(String.format("%3.1f", rpy.getM1())); gM2.layout(); } } } catch (Exception e) { } } @Override public void setFocus() {} }
public class Command extends ViewPart { private LogManager logger = LogManager.getInstance(); static final Color background1 = new Color(Display.getCurrent(), new RGB(76, 127, 220)); static final Color background2 = new Color(Display.getCurrent(), new RGB(255, 130, 0)); static final Color foregroundWhite = new Color(Display.getCurrent(), new RGB(255, 255, 255)); private float roll = 0; private float pitch = 0; private boolean enable = false; private boolean enableDoubleClick = false; private Text rollText; private Text pitchText; private static Text rollMax; private Button lockRoll; private Button lockPitch; private static Text pitchMax; private Canvas canvas; boolean running = true; private Text throttleText; private CommandThread commandThread; private int prevX = 0; private int prevY = 0; /** The ID of the view as specified by the extension. */ public static final String ID = "it.xbladecopter.command"; /** The constructor. */ public Command() { commandThread = new CommandThread(); commandThread.setRunning(true); ExecutorService exec = Executors.newSingleThreadExecutor(); exec.execute(commandThread); exec.shutdown(); } @Override public void dispose() { commandThread.setRunning(false); super.dispose(); } public static void updateFromPreferences() { IPreferenceStore store = Activator.getDefault().getPreferenceStore(); if (rollMax != null && !rollMax.isDisposed()) rollMax.setText(store.getString(PreferenceConstants.P_CMD_ROLL)); if (pitchMax != null && !pitchMax.isDisposed()) pitchMax.setText(store.getString(PreferenceConstants.P_CMD_PITCH)); } public void createPartControl(final Composite parent) { parent.setLayout(new GridLayout(3, false)); final Composite comp = new Composite(parent, SWT.BORDER); comp.setLayout(new FillLayout()); comp.setLayoutData(new GridData(GridData.FILL_BOTH)); final Scale scale = new Scale(parent, SWT.BORDER | SWT.VERTICAL); scale.setLayoutData(new GridData(GridData.FILL_VERTICAL)); scale.setMaximum(100); scale.setMinimum(0); scale.setPageIncrement(1); scale.setSelection(100); scale.setToolTipText("Throttle Command."); scale.addListener( SWT.Selection, new Listener() { @Override public void handleEvent(Event event) { int value = 100 - scale.getSelection(); // Inverted! from 100 to 0. if (throttleText != null) throttleText.setText(String.valueOf(value) + " %"); commandThread.getData().setM2(value); commandThread.getData().setM3(value); commandThread.getData().setM1(value); commandThread.getData().setM0(value); } }); canvas = new Canvas(comp, SWT.DOUBLE_BUFFERED); final Composite data = new Composite(parent, SWT.NONE); data.setLayout(new GridLayout(1, true)); data.setLayoutData(new GridData(GridData.FILL_VERTICAL)); Group groupThrottle = new Group(data, SWT.NONE); groupThrottle.setText("Throttle Command"); groupThrottle.setLayout(new GridLayout(2, true)); groupThrottle.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); Label label = new Label(groupThrottle, SWT.NONE); label.setText("Throttle:"); throttleText = new Text(groupThrottle, SWT.BORDER); throttleText.setEditable(false); throttleText.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); throttleText.setText("0 %"); Group groupRoll = new Group(data, SWT.NONE); groupRoll.setText("Roll Command"); groupRoll.setLayout(new GridLayout(2, true)); groupRoll.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); Group groupPitch = new Group(data, SWT.NONE); groupPitch.setText("Pitch Command"); groupPitch.setLayout(new GridLayout(2, true)); groupPitch.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); label = new Label(groupRoll, SWT.NONE); label.setText("Lock Roll:"); lockRoll = new Button(groupRoll, SWT.CHECK); label = new Label(groupRoll, SWT.NONE); label.setText("Roll (°):"); rollText = new Text(groupRoll, SWT.BORDER); rollText.setEditable(false); rollText.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); label = new Label(groupPitch, SWT.NONE); label.setText("Lock Pitch:"); lockPitch = new Button(groupPitch, SWT.CHECK); label = new Label(groupPitch, SWT.NONE); label.setText("Pitch (°):"); pitchText = new Text(groupPitch, SWT.BORDER); pitchText.setEditable(false); pitchText.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); label = new Label(groupRoll, SWT.NONE); label.setText("Max Roll (°):"); rollMax = new Text(groupRoll, SWT.BORDER); rollMax.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); rollMax.addFocusListener( new FocusListener() { @Override public void focusLost(FocusEvent e) { String value = ((Text) e.widget).getText(); IPreferenceStore store = Activator.getDefault().getPreferenceStore(); int intVal = 0; try { intVal = Integer.valueOf(value); } catch (NumberFormatException e1) { ((Text) e.widget).setText(store.getString(PreferenceConstants.P_CMD_ROLL)); } if (intVal > 90 || intVal < 0) ((Text) e.widget).setText(store.getString(PreferenceConstants.P_CMD_ROLL)); } @Override public void focusGained(FocusEvent arg0) {} }); label = new Label(groupPitch, SWT.NONE); label.setText("Max Pitch (°):"); pitchMax = new Text(groupPitch, SWT.BORDER); pitchMax.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); pitchMax.addFocusListener( new FocusListener() { @Override public void focusLost(FocusEvent e) { String value = ((Text) e.widget).getText(); IPreferenceStore store = Activator.getDefault().getPreferenceStore(); int intVal = 0; try { intVal = Integer.valueOf(value); } catch (NumberFormatException e1) { ((Text) e.widget).setText(store.getString(PreferenceConstants.P_CMD_PITCH)); } if (intVal > 90 || intVal < 0) ((Text) e.widget).setText(store.getString(PreferenceConstants.P_CMD_PITCH)); } @Override public void focusGained(FocusEvent arg0) {} }); final Composite buttons = new Composite(data, SWT.NONE); buttons.setLayout(new GridLayout(2, true)); buttons.setLayoutData(new GridData(GridData.FILL_HORIZONTAL)); final Button recordBtn = new Button(buttons, SWT.TOGGLE); recordBtn.addListener( SWT.Selection, new Listener() { @Override public void handleEvent(Event event) { if (recordBtn.getSelection()) TelemetryDataSingleton.getInstance().setLogEnabled(true); else TelemetryDataSingleton.getInstance().setLogEnabled(false); recordBtn.setSelection(TelemetryDataSingleton.getInstance().getLogEnabled()); logger.logInfo( "Data recording " + (recordBtn.getSelection() ? "enabled." : "disabled."), this.getClass()); } }); recordBtn.setImage( PlatformUI.getWorkbench().getSharedImages().getImage(ISharedImages.IMG_ELCL_STOP)); recordBtn.setLayoutData(new GridData(GridData.FILL_BOTH)); recordBtn.setToolTipText("Record telemetry to file."); final Button enableBtn = new Button(buttons, SWT.TOGGLE); enableBtn.addListener( SWT.Selection, new Listener() { @Override public void handleEvent(Event event) { if (enableBtn.getSelection()) commandThread.setPause(true); else commandThread.setPause(false); recordBtn.setSelection(TelemetryDataSingleton.getInstance().getLogEnabled()); if (enableBtn.getSelection()) logger.logInfo("Command data sent enabled.", this.getClass()); else logger.logWarning("Command data sent disabled.", this.getClass()); } }); enableBtn.setImage( PlatformUI.getWorkbench().getSharedImages().getImage(ISharedImages.IMG_ELCL_COLLAPSEALL)); enableBtn.setLayoutData(new GridData(GridData.FILL_BOTH)); enableBtn.setToolTipText("Enable command data sent."); canvas.setToolTipText("Pitch [left] & Roll [right] Commands."); canvas.addPaintListener( new PaintListener() { @Override public void paintControl(PaintEvent e) { // Get the canvas for drawing and its width and height Canvas canvas = (Canvas) e.widget; int width = canvas.getSize().x; int height = canvas.getSize().y - comp.getBorderWidth(); drawAttitudeControls(e.gc, width, height); } }); canvas.addMouseListener( new MouseListener() { @Override public void mouseUp(MouseEvent e) { enable = false; roll = 0; pitch = 0; update(); } @Override public void mouseDown(MouseEvent e) { enable = true; prevX = e.x; prevY = e.y; } @Override public void mouseDoubleClick(MouseEvent e) { if (enableDoubleClick) { enableDoubleClick = false; roll = 0; pitch = 0; update(); } else enableDoubleClick = true; } }); canvas.addMouseMoveListener( new MouseMoveListener() { private float pitchAngleStep; private float rollAngleStep; @Override public void mouseMove(MouseEvent e) { if (enable || enableDoubleClick) { int maxRoll = Integer.valueOf(rollMax.getText()); int maxPitch = Integer.valueOf(pitchMax.getText()); // Calculate deltas and scale value rollAngleStep = Math.abs(e.x - prevX) * 0.5f; pitchAngleStep = Math.abs(e.y - prevY) * 1.5f; if (roll >= maxRoll) { roll = maxRoll; } else if (roll <= -maxRoll) { roll = -maxRoll; } if (pitch >= maxPitch) { pitch = maxPitch; } else if (pitch <= -maxPitch) { pitch = -maxPitch; } if (e.x < prevX && !lockRoll.getSelection()) roll -= rollAngleStep; else if (e.x > prevX && !lockRoll.getSelection()) roll += rollAngleStep; else if (e.y < prevY && !lockPitch.getSelection()) pitch -= pitchAngleStep; else if (e.y > prevY && !lockPitch.getSelection()) pitch += pitchAngleStep; prevX = e.x; prevY = e.y; update(); } } }); updateFromPreferences(); } private void update() { commandThread.getData().setPitch(-pitch); commandThread.getData().setRoll(-roll); if (pitchText != null) { pitchText.setText(String.valueOf(-pitch)); pitchText.update(); } if (rollText != null) { rollText.setText(String.valueOf(-roll)); rollText.update(); } if (canvas != null) canvas.redraw(); } /** Passing the focus request to the viewer's control. */ public void setFocus() {} private void drawAttitudeControls(GC gc, int width, int height) { int size = Math.min(width / 2, height); int verticalOffset = (height - size) / 2; gc.setLineWidth(2); gc.setBackground(background1); gc.fillArc(width / 2 - size, verticalOffset, size, size, 0, 360); gc.fillArc(width / 2, verticalOffset, size, size, 0, 360); gc.setBackground(background2); gc.fillArc(width / 2 - size, verticalOffset, size, size, (int) pitch, -180); gc.fillArc(width / 2, verticalOffset, size, size, (int) roll, -180); gc.setForeground(foregroundWhite); gc.drawArc(width / 2 - size, verticalOffset, size, size, 0, 360); gc.drawArc(width / 2, verticalOffset, size, size, 0, 360); String value = ""; gc.setForeground(foregroundWhite); for (int i = 0; i < 2; i++) { int centerX = width / 2 - i * size; int tickSize = 20; int step = 20; for (int theta = 10; theta <= 360; theta += step) { int x = (int) (size / 2 + size / 2 * Math.cos(theta * Math.PI / 180.0)) + centerX; int y = (int) (size / 2 + size / 2 * Math.sin(theta * Math.PI / 180.0)); int xx = (int) ((size) / 2 + (size - tickSize) / 2 * Math.cos(theta * Math.PI / 180.0)) + centerX; int yy = (int) ((size) / 2 + (size - tickSize) / 2 * Math.sin(theta * Math.PI / 180.0)); gc.drawLine(x, verticalOffset + y, xx, verticalOffset + yy); if (theta < 180) { if (theta > 90 && theta < 180) { yy = yy - 15; xx = xx + 2; value = String.valueOf(theta); } else if (theta > 45 && theta <= 90) { yy = yy - 15; xx = xx - 10; value = String.valueOf(theta); } else if (theta < 45) { yy = yy - 15; xx = xx - 15; value = String.valueOf(theta); } gc.drawText(value, xx, verticalOffset + yy, true); } else { if (theta > 270) { xx = xx - 9; value = String.valueOf(theta - 360); } else { xx = xx - 3; value = String.valueOf(theta - 360); } if (theta == 180) { yy = yy - 7; xx = xx + 10; } else if (theta == 360) { yy = yy - 7; xx = xx - 3; } gc.drawText(value, xx, verticalOffset + yy, true); } } } } }