Bläddra i källkod

Merge pull request #47 from dangpzanco/main

trackball fix
GNU 1 år sedan
förälder
incheckning
7bc186a184

Filskillnaden har hållts tillbaka eftersom den är för stor
+ 7 - 6
Code/devterm_keyboard/README.md


+ 0 - 53
Code/devterm_keyboard/debouncer.h

@@ -1,53 +0,0 @@
-#ifndef DEBOUNCER_H
-#define DEBOUNCER_H
-
-#include <cstdint>
-
-typedef uint8_t millis_t;
-
-const millis_t DEBOUNCE_MS = 5;
-
-/**
-   @brief Asymmetric debouncer
-*/
-class Debouncer {
-  public:
-    Debouncer();
-    void updateTime(millis_t delta);
-    bool sample(bool value);
-  private:
-    millis_t timeout;
-};
-
-template<typename T, T millis>
-class Timeout {
-  public:
-    Timeout() {
-      timeout = 0;
-    }
-
-    void updateTime(millis_t delta) {
-      if (timeout > delta) {
-        timeout -= delta;
-      } else {
-        timeout = 0;
-      }
-    }
-
-    void expire() {
-      timeout = 0;
-    }
-
-    bool get() const {
-      return timeout == 0;
-    }
-
-    void reset() {
-      timeout = millis;
-    }
-  private:
-    uint16_t timeout;
-};
-
-
-#endif

+ 0 - 23
Code/devterm_keyboard/debouncer.ino

@@ -1,23 +0,0 @@
-#include "debouncer.h"
-
-Debouncer::Debouncer()
-  : timeout(0)
-{
-}
-
-void Debouncer::updateTime(millis_t delta) {
-  if (timeout > delta) {
-    timeout -= delta;
-  } else {
-    timeout = 0;
-  }
-}
-
-bool Debouncer::sample(bool value) {
-  if (value || timeout == 0) {
-    timeout = DEBOUNCE_MS;
-    return true;
-  }
-
-  return false;
-}

+ 0 - 29
Code/devterm_keyboard/glider.h

@@ -1,29 +0,0 @@
-#ifndef GLIDER_H
-#define GLIDER_H
-
-#include <cstdint>
-
-
-class Glider {
-public:
-  Glider();
-  void setDirection(int8_t);
-  void update(float velocity, uint16_t sustain);
-  void updateSpeed(float velocity);
-  void stop();
-
-  struct GlideResult {
-    int8_t value;
-    bool stopped;
-  };
-  GlideResult glide(uint8_t delta);
-
-public:
-  int8_t direction;
-  float speed;
-  uint16_t sustain;
-  uint16_t release;
-  float error;
-};
-
-#endif

+ 0 - 60
Code/devterm_keyboard/glider.ino

@@ -1,60 +0,0 @@
-#include <cmath>
-
-#include "glider.h"
-#include "math.h"
-
-Glider::Glider()
-: speed(0),
-  sustain(0),
-  release(0),
-  error(0)
-{}
-
-void Glider::setDirection(int8_t direction) {
-  if (this->direction != direction) {
-    stop();
-  }
-  this->direction = direction;
-}
-
-void Glider::update(float speed, uint16_t sustain) {
-  this->speed = speed;
-  this->sustain = sustain;
-  this->release = sustain;
-}
-
-void Glider::updateSpeed(float speed) {
-  this->speed = speed;
-}
-
-void Glider::stop() {
-  this->speed = 0;
-  this->sustain = 0;
-  this->release = 0;
-  this->error = 0;
-}
-
-Glider::GlideResult Glider::glide(millis_t delta) {
-  const auto alreadyStopped = speed == 0;
-
-  error += speed * delta;
-  int8_t distance = 0;
-  if (error > 0) {
-    distance = clamp<int8_t>(std::ceil(error));
-  }
-  error -= distance;
-
-  if (sustain > 0) {
-    const auto sustained = min(sustain, (uint16_t)delta);
-    sustain -= sustained;
-  } else if (release > 0) {
-    const auto released = min(release, (uint16_t)delta);
-    speed = speed * (release - released) / release;
-    release -= released;
-  } else {
-    speed = 0;
-  }
-
-  const int8_t result = direction * distance;
-  return GlideResult { result, !alreadyStopped && speed == 0 };
-}

+ 0 - 38
Code/devterm_keyboard/ratemeter.h

@@ -1,38 +0,0 @@
-#ifndef RATEMETER_H
-#define RATEMETER_H
-
-#include <cstdint>
-
-#include "debouncer.h"
-
-class RateMeter {
-public:
-  RateMeter();
-  void onInterrupt();
-  void tick(millis_t delta);
-  void expire();
-
-  uint16_t delta() const;
-  // Hall sensor edges per seconds.
-  // stopped: 0
-  // really slow => ~3
-  // medium => ~30
-  // fast => < 300
-  // max => 1000
-  float rate() const;
-
-private:
-  uint32_t lastTime;
-
-  // really Range, emperically:
-  // fast => < 5_000 us,
-  // medium => 20_000 - 40_000 us
-  // really slow => 250_000 us
-  uint32_t averageDelta;
-
-  static const uint16_t CUTOFF_MS = 1000;
-  // Cut off after some seconds to prevent multiple timestamp overflow (~70 mins)
-  Timeout<uint16_t, CUTOFF_MS> cutoff;
-};
-
-#endif

+ 0 - 47
Code/devterm_keyboard/ratemeter.ino

@@ -1,47 +0,0 @@
-#include <Arduino.h>
-#include <cstdint>
-
-#include "ratemeter.h"
-#include "math.h"
-
-RateMeter::RateMeter()
-: lastTime(0)
-{}
-
-void RateMeter::onInterrupt() {
-  const auto now = millis();
-  if (cutoff.get()) {
-    averageDelta = CUTOFF_MS;
-  } else {
-    const auto delta = getDelta(lastTime, now, CUTOFF_MS);
-    averageDelta = (averageDelta + delta) / 2;
-  }
-  lastTime = now;
-  cutoff.reset();
-}
-
-void RateMeter::tick(millis_t delta) {
-  cutoff.updateTime(delta);
-  if (!cutoff.get()) {
-    averageDelta += delta;
-  }
-}
-
-void RateMeter::expire() {
-  cutoff.expire();
-}
-
-uint16_t RateMeter::delta() const {
-  return averageDelta;
-}
-
-float RateMeter::rate() const {
-  if (cutoff.get()) {
-    return 0.0f;
-  } else if (averageDelta == 0) {
-    // to ensure range 0 ~ 1000.0
-    return 1000.0f;
-  } else {
-    return 1000.0f / (float)averageDelta;
-  }
-}

+ 40 - 2
Code/devterm_keyboard/state.h

@@ -5,13 +5,51 @@
 #include <array>
 #include <USBComposite.h>
 
-#include "debouncer.h"
-
 enum class TrackballMode : uint8_t {
   Wheel,
   Mouse,
 };
 
+template <typename T, T millis>
+class Timeout
+{
+public:
+  Timeout()
+  {
+    timeout = 0;
+  }
+
+  void updateTime(uint8_t delta)
+  {
+    if (timeout > delta)
+    {
+      timeout -= delta;
+    }
+    else
+    {
+      timeout = 0;
+    }
+  }
+
+  void expire()
+  {
+    timeout = 0;
+  }
+
+  bool get() const
+  {
+    return timeout == 0;
+  }
+
+  void reset()
+  {
+    timeout = millis;
+  }
+
+private:
+  T timeout;
+};
+
 class State
 {
   public:

+ 1 - 1
Code/devterm_keyboard/state.ino

@@ -11,7 +11,7 @@ State::State()
 {
 }
 
-void State::tick(millis_t delta)
+void State::tick(uint8_t delta)
 {
   middleClickTimeout.updateTime(delta);
 }

+ 198 - 107
Code/devterm_keyboard/trackball.ino

@@ -1,5 +1,5 @@
 /*
- * clockworkpi devterm trackball 
+ * ClockworkPi DevTerm Trackball
  */
 #include "keys_io_map.h"
 
@@ -9,122 +9,213 @@
 
 #include <USBComposite.h>
 
-
 #include "trackball.h"
-
-#include "ratemeter.h"
-#include "glider.h"
 #include "math.h"
 
-
- 
-enum Axis: uint8_t {
-  AXIS_X,
-  AXIS_Y,
-  AXIS_NUM,
+// Choose the type of filter (add a `_` to the #define you're not using):
+// - FIR uses more memory and takes longer to run, but you can tweak the FILTER_SIZE
+// - IIR is faster (has less coefficients), but the coefficients are pre-calculated (via scipy)
+#define _USE_FIR
+#define USE_IIR
+
+// Enable debug messages via serial
+#define _DEBUG_TRACKBALL
+
+// Source: https://github.com/dangpzanco/dsp
+#include <dsp_filters.h>
+
+// Simple trackball pin direction counter
+enum TrackballPin : uint8_t
+{
+    PIN_LEFT,
+    PIN_RIGHT,
+    PIN_UP,
+    PIN_DOWN,
+    PIN_NUM,
 };
-static TrackballMode lastMode;
-static int8_t distances[AXIS_NUM];
-static RateMeter rateMeter[AXIS_NUM];
-static Glider glider[AXIS_NUM];
-
-static const int8_t WHEEL_DENOM = 2;
-static int8_t wheelBuffer;
-
-static float rateToVelocityCurve(float input) {
-  //return std::pow(std::abs(input) / 50, 1.4);
-  return std::abs(input) / 30;
-}
-
-template<Axis AXIS, int8_t Direction >
-static void interrupt( ) {
-  distances[AXIS] += Direction;
-  rateMeter[AXIS].onInterrupt();
-  glider[AXIS].setDirection(Direction);
-
-  const auto rx = rateMeter[AXIS_X].rate();
-  const auto ry = rateMeter[AXIS_Y].rate();
-
-  const auto rate = std::sqrt(rx * rx + ry * ry);
-  const auto ratio = rateToVelocityCurve(rate) / rate;
-
-  const auto vx = rx * ratio;
-  const auto vy = ry * ratio;
-
-  if (AXIS == AXIS_X) {
-    glider[AXIS_X].update(vx, std::sqrt(rateMeter[AXIS_X].delta()));
-    glider[AXIS_Y].updateSpeed(vy);
-    
-  } else {
-    glider[AXIS_X].updateSpeed(vx);
-    glider[AXIS_Y].update(vy, std::sqrt(rateMeter[AXIS_Y].delta()));
-
-  }
-
- 
+static uint8_t direction_counter[PIN_NUM] = {0};
+
+// Mouse and Wheel sensitivity values
+static const float MOUSE_SENSITIVITY = 10.0f;
+static const float WHEEL_SENSITIVITY = 0.25f;
+
+#ifdef USE_IIR
+// Infinite Impulse Response (IIR) Filter
+// Filter design (https://docs.scipy.org/doc/scipy/reference/signal.html):
+// Low-pass Butterworth filter [b, a = scipy.signal.butter(N=2, Wn=0.1)]
+static const int8_t IIR_SIZE = 3;
+static float iir_coeffs_b[IIR_SIZE] = {0.020083365564211232, 0.040166731128422464, 0.020083365564211232};
+static float iir_coeffs_a[IIR_SIZE] = {1.0, -1.5610180758007182, 0.6413515380575631};
+IIR iir_x, iir_y;
+#endif
+
+#ifdef USE_FIR
+// FIR Filter
+static const int8_t FILTER_SIZE = 10;
+static float fir_coeffs[FILTER_SIZE];
+FIR fir_x, fir_y;
+
+static void init_fir()
+{
+    // Moving Average Finite Impulse Response (FIR) Filter:
+    // - Smooths out corners (the trackball normally only moves in 90 degree angles)
+    // - Filters out noisy data (avoids glitchy movements)
+    // - Adds delay to the movement. To tweak this:
+    //    1. Change the FILTER_SIZE (delay is proportional to the size, use millis() to measure time)
+    //    2. Redesign the filter with the desired delay
+    for (int8_t i = 0; i < FILTER_SIZE; i++)
+        fir_coeffs[i] = 1.0f / FILTER_SIZE;
 }
- 
-void trackball_task(DEVTERM*dv) {
-  int8_t x = 0, y = 0, w = 0;
-  noInterrupts();
-  const auto mode = dv->state->moveTrackball();
-  if (lastMode != mode) {
-    rateMeter[AXIS_X].expire();
-    rateMeter[AXIS_Y].expire();
-    wheelBuffer = 0;
-  }
-  else {
-    rateMeter[AXIS_X].tick(dv->delta);
-    rateMeter[AXIS_Y].tick(dv->delta);
-  }
-  lastMode = mode; 
-  
-  switch(mode){
-    case TrackballMode::Mouse: {
-      const auto rX = glider[AXIS_X].glide(dv->delta);
-      const auto rY = glider[AXIS_Y].glide(dv->delta);
-      x = rX.value;
-      y = rY.value;
-      if (rX.stopped) {
-        glider[AXIS_Y].stop();
-      }
-      if (rY.stopped) {
-        glider[AXIS_Y].stop();
-      }
-
-      break;
+#endif
+
+#ifdef DEBUG_TRACKBALL
+static uint32_t time = millis();
+
+// Useful debug function
+template <typename T>
+void print_vec(DEVTERM *dv, T *vec, uint32_t size, bool newline = true)
+{
+    for (int8_t i = 0; i < size - 1; i++)
+    {
+        dv->_Serial->print(vec[i]);
+        dv->_Serial->print(",");
     }
-    case TrackballMode::Wheel: {
-      wheelBuffer += distances[AXIS_Y];
-      w = wheelBuffer / WHEEL_DENOM;
-      wheelBuffer -= w * WHEEL_DENOM;
-      if(w != 0){
-        dv->state->setScrolled();
-      }
-      break;
+    if (newline)
+    {
+        dv->_Serial->println(vec[size - 1]);
     }
-  }
-  distances[AXIS_X] = 0;
-  distances[AXIS_Y] = 0;
-  interrupts();
-
-  if(x !=0 || y != 0 || -w!=0) {
-    dv->Mouse->move(x, y, -w);
-  }
- 
+    else
+    {
+        dv->_Serial->print(vec[size - 1]);
+        dv->_Serial->print(",");
+    }
+}
+#endif
+
+template <TrackballPin PIN>
+static void interrupt()
+{
+    // Count the number of times the trackball rolls towards a certain direction
+    // (when the corresponding PIN changes its value). This part of the code should be minimal,
+    // so that the next interrupts are not blocked from happening.
+    direction_counter[PIN] += 1;
 }
 
+static float position_scale(float x)
+{
+    // Exponential scaling of the mouse movement:
+    // - Small values remain small (precise movement)
+    // - Slightly larger values get much larger (fast movement)
+    // This function may be tweaked further, but it's good enough for now.
+    return MOUSE_SENSITIVITY * sign(x) * std::exp(std::abs(x) / std::sqrt(MOUSE_SENSITIVITY));
+}
 
-void trackball_init(DEVTERM*dv){
+void trackball_task(DEVTERM *dv)
+{
+
+#ifdef DEBUG_TRACKBALL
+    // Measure elapsed time
+    uint32_t elapsed = millis() - time;
+    time += elapsed;
+
+    // Send raw data via serial (CSV format)
+    dv->_Serial->print(elapsed);
+    dv->_Serial->print(",");
+    print_vec(dv, direction_counter, PIN_NUM);
+#endif
+
+
+    // Stop interrupts from happening. Don't forget to re-enable them!
+    noInterrupts();
+
+    // Calculate x and y positions
+    float x = direction_counter[PIN_RIGHT] - direction_counter[PIN_LEFT];
+    float y = direction_counter[PIN_DOWN] - direction_counter[PIN_UP];
+
+    // Clear counters
+    // memset(direction_counter, 0, sizeof(direction_counter));
+    std::fill(std::begin(direction_counter), std::end(direction_counter), 0);
+
+    // Re-enable interrupts (Mouse.move needs interrupts)
+    interrupts();
+
+    // Non-linear scaling
+    x = position_scale(x);
+    y = position_scale(y);
+
+    // Wheel rolls with the (reverse) vertical axis (no filter needed)
+    int8_t w = clamp<int8_t>(-y * WHEEL_SENSITIVITY);
+
+    // Filter x and y
+#ifdef USE_FIR
+    x = fir_filt(&fir_x, x);
+    y = fir_filt(&fir_y, y);
+#endif
+#ifdef USE_IIR
+    x = iir_filt(&iir_x, x);
+    y = iir_filt(&iir_y, y);
+#endif
+
+    // Move Trackball (either Mouse or Wheel)
+    switch (dv->state->moveTrackball())
+    {
+    case TrackballMode::Mouse:
+    {
+        // Move mouse
+        while ((int)x != 0 || (int)y != 0)
+        {
+            // Only 8bit values are allowed,
+            // so clamp and execute move() multiple times
+            int8_t x_byte = clamp<int8_t>(x);
+            int8_t y_byte = clamp<int8_t>(y);
+
+            // Move mouse with values in the range [-128, 127]
+            dv->Mouse->move(x_byte, y_byte, 0);
+
+            // Decrement the original value, stop if done
+            x -= x_byte;
+            y -= y_byte;
+        }
+
+        break;
+    }
+    case TrackballMode::Wheel:
+    {
+        if (w != 0)
+        {
+            // Only scroll the wheel [move cursor by (0,0)]
+            dv->Mouse->move(0, 0, w);
+            dv->state->setScrolled();
+        }
+
+        break;
+    }
+    }
+}
 
-  pinMode(LEFT_PIN, INPUT);
-  pinMode(UP_PIN, INPUT);
-  pinMode(RIGHT_PIN, INPUT);
-  pinMode(DOWN_PIN, INPUT);
+void trackball_init(DEVTERM *dv)
+{
+    // Enable trackball pins
+    pinMode(LEFT_PIN, INPUT);
+    pinMode(UP_PIN, INPUT);
+    pinMode(RIGHT_PIN, INPUT);
+    pinMode(DOWN_PIN, INPUT);
+
+    // Initialize filters
+#ifdef USE_FIR
+    init_fir();
+    fir_init(&fir_x, FILTER_SIZE, fir_coeffs);
+    fir_init(&fir_y, FILTER_SIZE, fir_coeffs);
+#endif
+#ifdef USE_IIR
+    iir_init(&iir_x, IIR_SIZE, iir_coeffs_b, IIR_SIZE, iir_coeffs_a);
+    iir_init(&iir_y, IIR_SIZE, iir_coeffs_b, IIR_SIZE, iir_coeffs_a);
+#endif
+
+    // Run interrupt function when corresponding PIN changes its value
+    attachInterrupt(LEFT_PIN, &interrupt<PIN_LEFT>, ExtIntTriggerMode::CHANGE);
+    attachInterrupt(RIGHT_PIN, &interrupt<PIN_RIGHT>, ExtIntTriggerMode::CHANGE);
+    attachInterrupt(UP_PIN, &interrupt<PIN_UP>, ExtIntTriggerMode::CHANGE);
+    attachInterrupt(DOWN_PIN, &interrupt<PIN_DOWN>, ExtIntTriggerMode::CHANGE);
 
-  attachInterrupt(LEFT_PIN, &interrupt<AXIS_X,-1> , ExtIntTriggerMode::CHANGE);
-  attachInterrupt(RIGHT_PIN, &interrupt<AXIS_X, 1>, ExtIntTriggerMode::CHANGE);
-  attachInterrupt(UP_PIN, &interrupt<AXIS_Y, -1>, ExtIntTriggerMode::CHANGE);
-  attachInterrupt(DOWN_PIN, &interrupt<AXIS_Y, 1>, ExtIntTriggerMode::CHANGE);
-  
 }

+ 59 - 0
Code/devterm_keyboard/upload/dfumode.py

@@ -0,0 +1,59 @@
+#!/usr/bin/python
+
+import time
+import serial
+import serial.tools.list_ports as lports
+import argparse
+
+
+def main(args):
+
+    # Find which serial port to use (assumes only the keyboard is connected)
+    if args.port == '':
+        port_list = lports.comports()
+        if len(port_list) != 1:
+            print('No serial ports detected, quitting...')
+            quit()
+        else:
+            port = port_list[0].device
+    else:
+        port = args.port
+
+    # Based on https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/master/tools/linux/src/upload-reset/upload-reset.c
+    # Send magic sequence of DTR and RTS followed by the magic word "1EAF"
+    with serial.Serial(port=port, baudrate=args.baudrate) as ser:
+
+        ser.rts = False
+        ser.dtr = False
+        ser.dtr = True
+
+        time.sleep(0.05)
+
+        ser.dtr = False
+        ser.rts = True
+        ser.dtr = True
+
+        time.sleep(0.05)
+
+        ser.dtr = False
+
+        time.sleep(0.05)
+
+        ser.write(b'1EAF')
+
+    # Wait for the DFU mode reboot
+    time.sleep(args.sleep * 1e-3)
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser = argparse.ArgumentParser(description="Enter DFU mode of DevTerm's keyboard.")
+    parser.add_argument('-p', '--port', dest='port', type=str, default='',
+                        help='Serial port (default: detect serial ports).')
+    parser.add_argument('-b', '--baudrate', dest='baudrate', type=int, default=9600,
+                        help='Baudrate (default: 9600bps).')
+    parser.add_argument('-s', '--sleep', dest='sleep', type=int, default=1500,
+                        help='Wait SLEEP milliseconds (ms) after enabling DFU mode (default: 1500ms).')
+    args = parser.parse_args()
+
+    main(args)

+ 166 - 0
Code/devterm_keyboard/upload/upload-reset.c

@@ -0,0 +1,166 @@
+/* Copyright (C) 2015 Roger Clark <www.rogerclark.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ *
+ * Utility to send the reset sequence on RTS and DTR and chars
+ * which resets the libmaple and causes the bootloader to be run
+ *
+ *
+ *
+ * Terminal control code by Heiko Noordhof (see copyright below)
+ */
+
+/* Copyright (C) 2003 Heiko Noordhof <heikyAusers.sf.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <termios.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <stdbool.h>
+
+/* Function prototypes (belong in a seperate header file) */
+int openserial(char *devicename);
+void closeserial(void);
+int setDTR(unsigned short level);
+int setRTS(unsigned short level);
+
+/* Two globals for use by this module only */
+static int fd;
+static struct termios oldterminfo;
+
+void closeserial(void)
+{
+    tcsetattr(fd, TCSANOW, &oldterminfo);
+    close(fd);
+}
+
+int openserial(char *devicename)
+{
+    struct termios attr;
+
+    if ((fd = open(devicename, O_RDWR)) == -1)
+        return 0; /* Error */
+    atexit(closeserial);
+
+    if (tcgetattr(fd, &oldterminfo) == -1)
+        return 0; /* Error */
+    attr = oldterminfo;
+    attr.c_cflag |= CRTSCTS | CLOCAL;
+    attr.c_oflag = 0;
+    if (tcflush(fd, TCIOFLUSH) == -1)
+        return 0; /* Error */
+    if (tcsetattr(fd, TCSANOW, &attr) == -1)
+        return 0; /* Error */
+
+    /* Set the lines to a known state, and */
+    /* finally return non-zero is successful. */
+    return setRTS(0) && setDTR(0);
+}
+
+/* For the two functions below:
+ *     level=0 to set line to LOW
+ *     level=1 to set line to HIGH
+ */
+
+int setRTS(unsigned short level)
+{
+    int status;
+
+    if (ioctl(fd, TIOCMGET, &status) == -1)
+    {
+        perror("setRTS(): TIOCMGET");
+        return 0;
+    }
+    if (level)
+        status |= TIOCM_RTS;
+    else
+        status &= ~TIOCM_RTS;
+    if (ioctl(fd, TIOCMSET, &status) == -1)
+    {
+        perror("setRTS(): TIOCMSET");
+        return 0;
+    }
+    return 1;
+}
+
+int setDTR(unsigned short level)
+{
+    int status;
+
+    if (ioctl(fd, TIOCMGET, &status) == -1)
+    {
+        perror("setDTR(): TIOCMGET");
+        return 0;
+    }
+    if (level)
+        status |= TIOCM_DTR;
+    else
+        status &= ~TIOCM_DTR;
+    if (ioctl(fd, TIOCMSET, &status) == -1)
+    {
+        perror("setDTR: TIOCMSET");
+        return 0;
+    }
+    return 1;
+}
+
+/* This portion of code was written by Roger Clark
+ * It was informed by various other pieces of code written by Leaflabs to reset their
+ * Maple and Maple mini boards
+ */
+
+main(int argc, char *argv[])
+{
+
+    if (argc < 2 || argc > 3)
+    {
+        printf("Usage upload-reset <serial_device> <Optional_delay_in_milliseconds>\n\r");
+        return;
+    }
+
+    if (openserial(argv[1]))
+    {
+        // Send magic sequence of DTR and RTS followed by the magic word "1EAF"
+        setRTS(false);
+        setDTR(false);
+        setDTR(true);
+
+        usleep(50000L);
+
+        setDTR(false);
+        setRTS(true);
+        setDTR(true);
+
+        usleep(50000L);
+
+        setDTR(false);
+
+        usleep(50000L);
+
+        write(fd, "1EAF", 4);
+
+        closeserial();
+        if (argc == 3)
+        {
+            usleep(atol(argv[2]) * 1000L);
+        }
+    }
+    else
+    {
+        printf("Failed to open serial device.\n\r");
+    }
+}

+ 30 - 0
Code/devterm_keyboard/upload/upload_bin.sh

@@ -0,0 +1,30 @@
+#!/bin/bash
+
+altID="2"
+usbID="1EAF:0003"
+serial_port="/dev/ttyACM0"
+
+# Default export location
+# binfile="../devterm_keyboard.ino.generic_stm32f103r8.bin"
+binfile="devterm_keyboard.ino.bin"
+
+# Send magic numbers via serial to enter DFU mode.
+# This needs pyserial, so it's installed quietly:
+# pip install -q pyserial
+./dfumode.py -p $serial_port -b 9600 -s 1500
+
+# Alternatively you can compile the C program that does the same thing:
+# gcc upload-reset.c -o upload-reset
+# ./upload-reset $serial_port 1500
+
+# Upload binary file
+dfu-util -d ${usbID} -a ${altID} -D ${binfile} -R
+
+echo "Waiting for $serial_port serial..."
+COUNTER=0
+while [ ! -c $serial_port ] && ((COUNTER++ < 40)); do
+    sleep 0.1
+done
+echo Done
+
+

Vissa filer visades inte eftersom för många filer har ändrats