From 451e79519637fdcf33f220f7dae9a28b15e014ba Mon Sep 17 00:00:00 2001
From: Paul Guenette <Wetmelon@users.noreply.github.com>
Date: Thu, 31 Jan 2019 20:41:46 +0100
Subject: [PATCH 01/31] Fix break -> brake

---
 docs/troubleshooting.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md
index 369f9d39..a357e1cc 100644
--- a/docs/troubleshooting.md
+++ b/docs/troubleshooting.md
@@ -49,9 +49,9 @@ You can monitor your PUS voltage using liveplotter in odrive tool by entering `s
 
 * `ERROR_DC_BUS_OVER_VOLTAGE = 0x04`
 
-Confirm that you have a break resistor of the correct value connected securly and that `odrv0.config.brake_resistance` is set to the value of your break resistor. 
+Confirm that you have a brake resistor of the correct value connected securly and that `odrv0.config.brake_resistance` is set to the value of your brake resistor. 
 
-You can monitor your PUS voltage using liveplotter in odrive tool by entering `start_liveplotter(lambda: [odrv0.vbus_voltage])`. If during a move you see the voltage rise above your PSU's nominal set voltage then you have your break resistance set too low. This may happen if you are using long wires or small gauge wires to connect your break resistor to your odrive which will added extra resistance. This extra resistance needs to be accounted for to prevent this voltage spike. If you have checked all your connections you can also try increasing your break resistance by ~ 0.01 Ohm at a time to a maximum of 0.05 greater than your break resistor value.
+You can monitor your PUS voltage using liveplotter in odrive tool by entering `start_liveplotter(lambda: [odrv0.vbus_voltage])`. If during a move you see the voltage rise above your PSU's nominal set voltage then you have your brake resistance set too low. This may happen if you are using long wires or small gauge wires to connect your brake resistor to your odrive which will added extra resistance. This extra resistance needs to be accounted for to prevent this voltage spike. If you have checked all your connections you can also try increasing your brake resistance by ~ 0.01 Ohm at a time to a maximum of 0.05 greater than your brake resistor value.
 
 ## Common Motor Errors 
 

From b64b3b56252477d84d24e7f915c6ba62cf29c5db Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 8 Feb 2019 19:25:19 -0800
Subject: [PATCH 02/31] Update Gemfile.lock

---
 docs/Gemfile.lock | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/docs/Gemfile.lock b/docs/Gemfile.lock
index 1c6d972e..860837ad 100644
--- a/docs/Gemfile.lock
+++ b/docs/Gemfile.lock
@@ -70,7 +70,7 @@ GEM
       listen (= 3.1.5)
       mercenary (~> 0.3)
       minima (= 2.4.0)
-      nokogiri (>= 1.8.1, < 2.0)
+      nokogiri (>= 1.8.5, < 2.0)
       rouge (= 2.2.1)
       terminal-table (~> 1.4)
     github-pages-health-check (1.4.0)
@@ -81,7 +81,7 @@ GEM
       typhoeus (~> 1.3)
     html-pipeline (2.7.1)
       activesupport (>= 2)
-      nokogiri (>= 1.4)
+      nokogiri (>= 1.8.5)
     http_parser.rb (0.6.0)
     i18n (0.9.5)
       concurrent-ruby (~> 1.0)
@@ -207,7 +207,7 @@ GEM
     minitest (5.11.3)
     multipart-post (2.0.0)
     net-dns (0.8.0)
-    nokogiri (1.8.2)
+    nokogiri (>= 1.8.5)
       mini_portile2 (~> 2.3.0)
     octokit (4.8.0)
       sawyer (~> 0.8.0, >= 0.5.3)

From 40c16ef49499b421a108b8d994ebae83c35accbe Mon Sep 17 00:00:00 2001
From: csann <1936653+csann@users.noreply.github.com>
Date: Fri, 22 Feb 2019 21:22:19 -0600
Subject: [PATCH 03/31] -

Revise Mac bash commands to install arm-gcc-bin library.
---
 docs/developer-guide.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/docs/developer-guide.md b/docs/developer-guide.md
index 8083e16b..04671b48 100644
--- a/docs/developer-guide.md
+++ b/docs/developer-guide.md
@@ -70,7 +70,8 @@ sudo pacman -S tup
 #### Mac
 First install [Homebrew](https://brew.sh/). Then you can run these commands in Terminal:
 ```bash
-brew cask install gcc-arm-embedded
+brew tap osx-cross/arm
+brew install arm-gcc-bin
 brew cask install osxfuse && brew install tup
 brew install openocd
 ```

From bcf3270a9c7091e97aa5647f62cf5c552e6182b8 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 25 Feb 2019 20:07:03 -0800
Subject: [PATCH 04/31] update changelog to released state

---
 CHANGELOG.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index bab57274..71d320cd 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,6 +1,8 @@
 # Unreleased Features
 Please add a note of your changes below this heading if you make a Pull Request.
 
+# Releases
+## [0.4.8] - 2019-02-25
 ### Added
 * `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors.
 * `f` command to ascii protocol to get encoder position and velocity feedback.
@@ -16,7 +18,6 @@ Please add a note of your changes below this heading if you make a Pull Request.
 ### Fixed
 * Added required 1.5 cycle phase shift between ADC and PWM, lack thereof caused unstable current controller at high eRPM.
 
-# Releases
 ## [0.4.7] - 2018-11-28
 ### Added
 * Overspeed fault

From cb7c0d842fd8f655ed41826647e4aed747ce9557 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Tue, 26 Feb 2019 20:52:05 -0800
Subject: [PATCH 05/31] fix dump errors axis mismatch

---
 tools/odrive/utils.py | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py
index 28d1d737..e74376cf 100755
--- a/tools/odrive/utils.py
+++ b/tools/odrive/utils.py
@@ -32,9 +32,10 @@ class OperationAbortedException(Exception):
     pass
 
 def dump_errors(odrv, clear=False):
-    axes = [axis for name, axis in odrv._remote_attributes.items() if 'axis' in name]
-    for num, axis in enumerate(axes):
-        print('Axis{}:'.format(num))
+    axes = [(name, axis) for name, axis in odrv._remote_attributes.items() if 'axis' in name]
+    axes.sort()
+    for name, axis in axes:
+        print(name)
 
         # Flatten axis and submodules
         # (name, remote_obj, errorcode)

From 40f24fd30b60b0d81fe7670671b2fca1b98f4503 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sun, 3 Mar 2019 15:45:10 -0800
Subject: [PATCH 06/31] Update odrivetool.md

---
 docs/odrivetool.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/docs/odrivetool.md b/docs/odrivetool.md
index c00d5bc3..52b648bb 100644
--- a/docs/odrivetool.md
+++ b/docs/odrivetool.md
@@ -116,6 +116,7 @@ You can use the DfuSe app from ST.
 1. Force the ODrive into DFU mode, as per the instructions above "How to force DFU mode".
 1. In the top left it should now be connected to "STM Device in DFU Mode".
    1. If it doesn't appear, it may be because the driver is set to libusb by Zadig. We need to set it back to the original driver. Follow [these instructions](https://github.com/pbatard/libwdi/wiki/FAQ#Help_Zadig_replaced_the_driver_for_the_wrong_device_How_do_I_restore_it).
+   2. If, after doing the above step, the ODrive still installs itself as a libusb device in Device Manager, you can try to delete the libusb driver (this is OK, since we can use Zadig to install it again). You can simply delete the file `C:\Windows\System32\drivers\libusb0.sys`.
 1. In the bottom right section called "Upgrade or Verify Action" click the button "Choose...".
 1. Locate the `ODriveFirmware.dfu` we made before.
 1. Click button "Upgrade".

From 42384f3ef9b8b7eeec8f3e91818a30673e16414b Mon Sep 17 00:00:00 2001
From: Paul Belanger <paulirino@gmail.com>
Date: Wed, 6 Mar 2019 14:00:47 -0500
Subject: [PATCH 07/31] Initial implementation of comms watchdog

- Added watchdog timeout property to Axis::Config_t:
  axis.config.watchdog_timeout

- Added axis protocol function to feed watchdog timer:
  axis.watchdog_feed()

- Axis::run_control_loop now checks for watchdog expiration.

- Ascii protocol: add support for watchdog

  - The following ASCII commands now automatically update the watchdog:
    p, v, t, c, q

  - Added a 'u' command to update the watchdog of a motor without
    modifying setpoints.

  - Updated ascii protocol documentation to reflect new commands and
    effects.

- Updated getting started guide to mention watchdog settings and
  functions in protocol.

Please note: due to unavailability of hardware at this time, I have been
unable to test this code on an Odrive.
---
 Firmware/MotorControl/axis.cpp            | 33 +++++++++++++++++++++++
 Firmware/MotorControl/axis.hpp            | 22 +++++++++++++--
 Firmware/communication/ascii_protocol.cpp | 29 +++++++++++++++++---
 docs/ascii-protocol.md                    | 18 +++++++++++++
 docs/getting-started.md                   | 12 +++++++++
 5 files changed, 108 insertions(+), 6 deletions(-)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 8be43d15..759d237b 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -28,6 +28,7 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
     trap_.axis_ = this;
 
     decode_step_dir_pins();
+    update_watchdog_settings();
 }
 
 static void step_cb_wrapper(void* ctx) {
@@ -88,6 +89,18 @@ void Axis::decode_step_dir_pins() {
     dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
 }
 
+// @brief: Setup the watchdog reset value from the configuration watchdog timeout interval. 
+void Axis::update_watchdog_settings() {
+
+    if(config_.watchdog_timeout <= 0.0f) { // watchdog disabled 
+        watchdog_reset_value_ = 0;
+    } else if(config_.watchdog_timeout >= UINT32_MAX / (current_meas_hz+1)) { //overflow! 
+        watchdog_reset_value_ = UINT32_MAX;
+    } else {
+        watchdog_reset_value_ = static_cast<uint32_t>(config_.watchdog_timeout * current_meas_hz);
+    }
+}
+
 // @brief (de)activates step/dir input
 void Axis::set_step_dir_active(bool active) {
     if (active) {
@@ -141,6 +154,26 @@ bool Axis::do_updates() {
     return check_for_errors();
 }
 
+// @brief Feed the watchdog to prevent watchdog timeouts.
+void Axis::watchdog_feed() {
+    watchdog_current_value_ = watchdog_reset_value_;
+}
+
+// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired. 
+bool Axis::watchdog_check() {
+    // reset value = 0 means watchdog disabled. 
+    if(watchdog_reset_value_ == 0) return true;
+
+    // explicit check here to ensure that we don't underflow back to UINT32_MAX
+    if(watchdog_current_value_ > 0) {
+        watchdog_current_value_--;
+        return true;
+    } else {
+        error_ |= ERROR_WATCHDOG_TIMER_EXPIRED;
+        return false;
+    }
+}
+
 bool Axis::run_sensorless_spin_up() {
     // Early Spin-up: spiral up current
     float x = 0.0f;
diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index a6b6bdae..759f4d8d 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -20,6 +20,7 @@ public:
         ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
         ERROR_CONTROLLER_FAILED = 0x200,
         ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
+        ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
     };
 
     // Warning: Do not reorder these enum values.
@@ -47,6 +48,8 @@ public:
                                     //   For M0 this has no effect if enable_uart is true
         float counts_per_step = 2.0f;
 
+        float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)
+
         // Defaults loaded from hw_config in load_configuration in main.cpp
         uint16_t step_gpio_pin = 0;
         uint16_t dir_gpio_pin = 0;
@@ -79,6 +82,8 @@ public:
     void step_cb();
     void set_step_dir_active(bool enable);
     void decode_step_dir_pins();
+    void update_watchdog_settings();
+
     static void load_default_step_dir_pin_config(
         const AxisHardwareConfig_t& hw_config, Config_t* config);
 
@@ -87,6 +92,9 @@ public:
     bool do_checks();
     bool do_updates();
 
+    void watchdog_feed();
+    bool watchdog_check();
+
 
     // True if there are no errors
     bool inline check_for_errors() {
@@ -121,8 +129,11 @@ public:
             // Update all estimators
             // Note: updates run even if checks fail
             bool updates_ok = do_updates(); 
+
+            // make sure the watchdog is being fed. 
+            bool watchdog_ok = watchdog_check();
             
-            if (!checks_ok || !updates_ok) {
+            if (!checks_ok || !updates_ok || !watchdog_ok) {
                 // It's not useful to quit idle since that is the safe action
                 // Also leaving idle would rearm the motors
                 if (current_state_ != AXIS_STATE_IDLE)
@@ -185,6 +196,10 @@ public:
     State_t& current_state_ = task_chain_[0];
     uint32_t loop_counter_ = 0;
 
+    // watchdog
+    uint32_t watchdog_reset_value_ = 0; //computed from config_.watchdog_timeout in update_watchdog_settings()
+    uint32_t watchdog_current_value_= 0;
+
     // Communication protocol definitions
     auto make_protocol_definitions() {
         return make_protocol_member_list(
@@ -201,6 +216,8 @@ public:
                 make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
                 make_protocol_property("enable_step_dir", &config_.enable_step_dir),
                 make_protocol_property("counts_per_step", &config_.counts_per_step),
+                make_protocol_property("watchdog_timeout", &config_.watchdog_timeout,
+                    [](void* ctx) { static_cast<Axis*>(ctx)->update_watchdog_settings(); }, this),
                 make_protocol_property("step_gpio_pin", &config_.step_gpio_pin,
                     [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
                 make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
@@ -215,7 +232,8 @@ public:
             make_protocol_object("controller", controller_.make_protocol_definitions()),
             make_protocol_object("encoder", encoder_.make_protocol_definitions()),
             make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()),
-            make_protocol_object("trap_traj", trap_.make_protocol_definitions())
+            make_protocol_object("trap_traj", trap_.make_protocol_definitions()),
+            make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed)
         );
     }
 };
diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp
index 8a0d3287..1e1c9ba4 100644
--- a/Firmware/communication/ascii_protocol.cpp
+++ b/Firmware/communication/ascii_protocol.cpp
@@ -99,7 +99,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
                 vel_feed_forward = 0.0f;
             if (numscan < 4)
                 current_feed_forward = 0.0f;
-            axes[motor_number]->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward);
+            Axis* axis = axes[motor_number];
+            axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward);
+            axis->watchdog_feed();
         }
 
     } else if (cmd[0] == 'q') { // position control with limits
@@ -117,6 +119,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
                 axis->controller_.config_.vel_limit = vel_limit;
             if (numscan >= 4)
                 axis->motor_.config_.current_lim = current_lim;
+
+            axis->watchdog_feed();
         }
 
     } else if (cmd[0] == 'v') { // velocity control
@@ -130,7 +134,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         } else {
             if (numscan < 3)
                 current_feed_forward = 0.0f;
-            axes[motor_number]->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward);
+            Axis* axis = axes[motor_number];
+            axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward);
+            axis->watchdog_feed();
         }
 
     } else if (cmd[0] == 'c') { // current control
@@ -142,7 +148,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         } else if (motor_number >= AXIS_COUNT) {
             respond(response_channel, use_checksum, "invalid motor %u", motor_number);
         } else {
-            axes[motor_number]->controller_.set_current_setpoint(current_setpoint);
+            Axis* axis = axes[motor_number];
+            axis->controller_.set_current_setpoint(current_setpoint);
+            axis->watchdog_feed();
         }
 
     } else if (cmd[0] == 't') { // trapezoidal trajectory
@@ -154,7 +162,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         } else if (motor_number >= AXIS_COUNT) {
             respond(response_channel, use_checksum, "invalid motor %u", motor_number);
         } else {
-            axes[motor_number]->controller_.move_to_pos(goal_point);
+            Axis* axis = axes[motor_number];
+            axis->controller_.move_to_pos(goal_point);
+            axis->watchdog_feed();
         }
 
     } else if (cmd[0] == 'f') { // feedback
@@ -240,6 +250,17 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
             }
         }
 
+    }else if (cmd[0] == 'u') { // Update axis watchdog. 
+        unsigned motor_number;
+        int numscan = sscanf(cmd, "u %u", &motor_number);
+        if(numscan < 1){
+            respond(response_channel, use_checksum, "invalid command format");
+        } else if (motor_number >= AXIS_COUNT) {
+            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
+        }else {
+            axes[motor_number]->watchdog_feed();
+        }
+
     } else if (cmd[0] != 0) {
         respond(response_channel, use_checksum, "unknown command");
     }
diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md
index 0c510fe6..d782aa79 100644
--- a/docs/ascii-protocol.md
+++ b/docs/ascii-protocol.md
@@ -36,6 +36,8 @@ Example: `t 0 -20000`
 
 For general moving around of the axis, this is the recommended command.
 
+This command updates the watchdog timer for the motor. 
+
 #### Motor Position command
 For basic use where you send one setpoint at at a time, use the `q` command.
 If you have a realtime controller that is streaming setpoints and tracking a trajectory, use the `p` command.
@@ -64,6 +66,7 @@ Example: `p 0 -20000 0 0`
 
 Note that if you don't know what feed-forward is or what it's used for, simply omit it.
 
+This command updates the watchdog timer for the motor. 
 
 #### Motor Velocity command
 ```
@@ -78,6 +81,8 @@ Example: `v 0 1000 0`
 
 Note that if you don't know what feed-forward is or what it's used for, simply omit it.
 
+This command updates the watchdog timer for the motor. 
+
 #### Motor Current command
 ```
 c motor current
@@ -86,6 +91,19 @@ c motor current
 * `motor` is the motor number, `0` or `1`.
 * `current` is the desired current in A.
 
+This command updates the watchdog timer for the motor. 
+
+
+#### Update motor watchdog
+```
+u motor
+```
+* `u` for /u/pdate.
+* `motor` is the motor number, `0` or `1`.
+
+This command updates the watchdog timer for the motor, without changing any
+setpoints. 
+
 #### Parameter reading/writing
 
 Not all parameters can be accessed via the ASCII protocol but at least all parameters with float and integer type are supported.
diff --git a/docs/getting-started.md b/docs/getting-started.md
index 982ec861..8060bbb3 100644
--- a/docs/getting-started.md
+++ b/docs/getting-started.md
@@ -331,6 +331,18 @@ You can now control the current with `axis.controller.current_setpoint = 3` [A].
 
 *Note: There is no velocity limiting in current control mode. Make sure that you don't overrev the motor, or exceed the max speed for your encoder.*
 
+
+## Watchdog Timer
+Each axis has a configurable watchdog timer that can stop the motors if the
+control connection to the ODrive is interrupted.
+
+Each axis has a configurable watchdog timeout: `axis.config.watchdog_timeout`,
+measured in seconds. A value of `0` disables the watchdog functionality. Any value
+`> 0` will stop the motors if the watchdog has not been fed in the configured
+time interval. 
+
+The watchdog is fed using the `axis.watchdog_feed()` method of each axis. 
+
 ## What's next?
 You can now:
 * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance.

From 412f7962f4e584ac3b06224ed21610be381b6e35 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 11 Mar 2019 14:35:19 -0700
Subject: [PATCH 08/31] reject precalibration unless encoder is ready

---
 Firmware/MotorControl/encoder.cpp | 30 ++++++++++++++++++------------
 Firmware/MotorControl/encoder.hpp | 11 +++++++----
 2 files changed, 25 insertions(+), 16 deletions(-)

diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index 0cf13549..49605017 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -39,9 +39,8 @@ bool Encoder::do_checks(){
 // Triggered when an encoder passes over the "Index" pin
 // TODO: only arm index edge interrupt when we know encoder has powered up
 // (maybe by attaching the interrupt on start search, synergistic with following)
-// TODO: disable interrupt once we found the index
 void Encoder::enc_index_cb() {
-    if (config_.use_index && !index_found_) {
+    if (config_.use_index) {
         set_circular_count(0, false);
         if (config_.zero_count_on_find_idx)
             set_linear_count(0); // Avoid position control transient after search
@@ -71,6 +70,23 @@ void Encoder::set_idx_subscribe(bool override_enable) {
     }
 }
 
+void Encoder::update_pll_gains() {
+    pll_kp_ = 2.0f * config_.bandwidth;  // basic conversion to discrete time
+    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped
+
+    // Check that we don't get problems with discrete time approximation
+    if (!(current_meas_period * pll_kp_ < 1.0f)) {
+        set_error(ERROR_UNSTABLE_GAIN);
+    }
+}
+
+void Encoder::check_pre_calibrated() {
+    if (!is_ready_)
+        config_.pre_calibrated = false;
+    if (config_.mode == MODE_INCREMENTAL && !index_found_)
+        config_.pre_calibrated = false;
+}
+
 // Function that sets the current encoder count to a desired 32-bit value.
 void Encoder::set_linear_count(int32_t count) {
     // Disable interrupts to make a critical section to avoid race condition
@@ -261,16 +277,6 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
     }
 }
 
-void Encoder::update_pll_gains() {
-    pll_kp_ = 2.0f * config_.bandwidth;  // basic conversion to discrete time
-    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped
-
-    // Check that we don't get problems with discrete time approximation
-    if (!(current_meas_period * pll_kp_ < 1.0f)) {
-        set_error(ERROR_UNSTABLE_GAIN);
-    }
-}
-
 void Encoder::sample_now() {
     switch (config_.mode) {
         case MODE_INCREMENTAL: {
diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp
index dff98f28..b3a0e3bd 100644
--- a/Firmware/MotorControl/encoder.hpp
+++ b/Firmware/MotorControl/encoder.hpp
@@ -52,6 +52,8 @@ public:
 
     void enc_index_cb();
     void set_idx_subscribe(bool override_enable = false);
+    void update_pll_gains();
+    void check_pre_calibrated();
 
     void set_linear_count(int32_t count);
     void set_circular_count(int32_t count, bool update_offset);
@@ -63,7 +65,7 @@ public:
     void sample_now();
     bool update();
 
-    void update_pll_gains();
+
 
     const EncoderHardwareConfig_t& hw_config_;
     Config_t& config_;
@@ -92,8 +94,8 @@ public:
     auto make_protocol_definitions() {
         return make_protocol_member_list(
             make_protocol_property("error", &error_),
-            make_protocol_ro_property("is_ready", &is_ready_),
-            make_protocol_ro_property("index_found", const_cast<bool*>(&index_found_)),
+            make_protocol_property("is_ready", &is_ready_),
+            make_protocol_property("index_found", const_cast<bool*>(&index_found_)),
             make_protocol_property("shadow_count", &shadow_count_),
             make_protocol_property("count_in_cpr", &count_in_cpr_),
             make_protocol_property("interpolation", &interpolation_),
@@ -110,7 +112,8 @@ public:
                     [](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
                 make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
                     [](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
-                make_protocol_property("pre_calibrated", &config_.pre_calibrated),
+                make_protocol_property("pre_calibrated", &config_.pre_calibrated,
+                    [](void* ctx) { static_cast<Encoder*>(ctx)->check_pre_calibrated(); }, this),
                 make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx),
                 make_protocol_property("cpr", &config_.cpr),
                 make_protocol_property("offset", &config_.offset),

From 3400d60fa347855376c7fc8c67a3ace721d59093 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sat, 16 Mar 2019 00:30:32 +0100
Subject: [PATCH 09/31] Fix intellisense includePath, add flylint paths to
 workspace settings

---
 Firmware/.vscode/c_cpp_properties.json |  7 ++++---
 ODrive_Workspace.code-workspace        | 10 +++++++++-
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index 570c0f29..5e30003f 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -6,6 +6,7 @@
                 "${workspaceRoot}",
                 "${workspaceRoot}/fibre/cpp/include/**",
                 "${workspaceRoot}/MotorControl",
+                "${workspaceRoot}/communication",
                 "${workspaceRoot}/Drivers/DRV8301",
                 "${workspaceRoot}/Board/v3/Inc",
                 "${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
@@ -16,9 +17,7 @@
                 "${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc",
                 "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS",
                 "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include",
-                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
-                "${ARM_GCC_ROOT}/arm-none-eabi/include/**",
-                "${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**"
+                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"
             ],
             "defines": [
                 "STM32F405xx",
@@ -49,6 +48,7 @@
                 "${workspaceRoot}",
                 "${workspaceRoot}/fibre/cpp/include/**",
                 "${workspaceRoot}/MotorControl",
+                "${workspaceRoot}/communication",
                 "${workspaceRoot}/Drivers/DRV8301",
                 "${workspaceRoot}/Board/v3/Inc",
                 "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
@@ -92,6 +92,7 @@
                 "${workspaceRoot}",
                 "${workspaceRoot}/fibre/cpp/include/**",
                 "${workspaceRoot}/MotorControl",
+                "${workspaceRoot}/communication",
                 "${workspaceRoot}/Drivers/DRV8301",
                 "${workspaceRoot}/Board/v3/Inc",
                 "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace
index d858dcc3..1670d342 100644
--- a/ODrive_Workspace.code-workspace
+++ b/ODrive_Workspace.code-workspace
@@ -11,6 +11,14 @@
 		}
 	],
 	"settings": {
+
+		"c-cpp-flylint.cppcheck.includePaths": [
+			"${workspaceRoot}",
+			"${workspaceRoot}/fibre/cpp/include/fibre",
+			"${workspaceRoot}/communication",
+			"${workspaceRoot}/MotorControl",
+		],
+
 		"files.associations": {
 			"memory": "cpp",
 			"utility": "cpp",
@@ -47,5 +55,5 @@
 			"future": "cpp",
 			"arm_math.h": "c"
 		}
-	}
+	
 }

From e7af78d2b02f0ae209bed453452f2e1a14a5318e Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sat, 16 Mar 2019 00:35:08 +0100
Subject: [PATCH 10/31] Wrap gcc.exe in compilerPath with quotes

---
 Firmware/.vscode/c_cpp_properties.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index 5e30003f..98ddcaba 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -38,7 +38,7 @@
                 ],
                 "limitSymbolsToIncludedHeaders": true
             },
-            "compilerPath": "${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
+            "compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
             "cStandard": "c11",
             "cppStandard": "c++14"
         },

From 61632acce7205fd81855de3ba1019da415db9116 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sat, 16 Mar 2019 23:30:45 +0100
Subject: [PATCH 11/31] Fix missing curly brace

---
 ODrive_Workspace.code-workspace | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace
index 1670d342..65ed1095 100644
--- a/ODrive_Workspace.code-workspace
+++ b/ODrive_Workspace.code-workspace
@@ -55,5 +55,5 @@
 			"future": "cpp",
 			"arm_math.h": "c"
 		}
-	
+	}
 }

From 37427fcdc201d57726eca5ec58dc4e8d87c50f7a Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:17:29 +0100
Subject: [PATCH 12/31] Fix protocol templates

---
 Firmware/fibre/cpp/include/fibre/protocol.hpp | 127 ++++--------------
 1 file changed, 23 insertions(+), 104 deletions(-)

diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp
index 4b503e68..552dbd9e 100644
--- a/Firmware/fibre/cpp/include/fibre/protocol.hpp
+++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp
@@ -84,59 +84,17 @@ typedef struct {
 
 
 template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
-inline size_t write_le(T value, uint8_t* buffer);
+inline size_t write_le(T value, uint8_t* buffer){
+    for(size_t i = 0; i < sizeof(value); ++i){
+        buffer[i] = (value >> 8*i) & 0xff;
+    }
+    return sizeof(value);
+}
 
 template<typename T>
-inline size_t read_le(T* value, const uint8_t* buffer);
-
-template<>
-inline size_t write_le<bool>(bool value, uint8_t* buffer) {
-    buffer[0] = value ? 1 : 0;
-    return 1;
-}
-
-template<>
-inline size_t write_le<uint8_t>(uint8_t value, uint8_t* buffer) {
-    buffer[0] = value;
-    return 1;
-}
-
-template<>
-inline size_t write_le<uint16_t>(uint16_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    return 2;
-}
-
-template<>
-inline size_t write_le<uint32_t>(uint32_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    buffer[2] = (value >> 16) & 0xff;
-    buffer[3] = (value >> 24) & 0xff;
-    return 4;
-}
-
-template<>
-inline size_t write_le<int32_t>(int32_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    buffer[2] = (value >> 16) & 0xff;
-    buffer[3] = (value >> 24) & 0xff;
-    return 4;
-}
-
-template<>
-inline size_t write_le<uint64_t>(uint64_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    buffer[2] = (value >> 16) & 0xff;
-    buffer[3] = (value >> 24) & 0xff;
-    buffer[4] = (value >> 32) & 0xff;
-    buffer[5] = (value >> 40) & 0xff;
-    buffer[6] = (value >> 48) & 0xff;
-    buffer[7] = (value >> 56) & 0xff;
-    return 8;
+typename std::enable_if_t<std::is_const<T>::value, size_t>
+write_le(T value, uint8_t* buffer) {
+    return write_le<std::remove_const_t<T>>(value, buffer);
 }
 
 template<>
@@ -148,59 +106,12 @@ inline size_t write_le<float>(float value, uint8_t* buffer) {
 }
 
 template<typename T>
-typename std::enable_if_t<std::is_const<T>::value, size_t>
-write_le(T value, uint8_t* buffer) {
-    return write_le<std::remove_const_t<T>>(value, buffer);
-}
-
-template<>
-inline size_t read_le<bool>(bool* value, const uint8_t* buffer) {
-    *value = buffer[0];
-    return 1;
-}
-
-template<>
-inline size_t read_le<uint8_t>(uint8_t* value, const uint8_t* buffer) {
-    *value = buffer[0];
-    return 1;
-}
-
-template<>
-inline size_t read_le<uint16_t>(uint16_t* value, const uint8_t* buffer) {
-    *value = (static_cast<uint16_t>(buffer[0]) << 0) |
-             (static_cast<uint16_t>(buffer[1]) << 8);
-    return 2;
-}
-
-template<>
-inline size_t read_le<int32_t>(int32_t* value, const uint8_t* buffer) {
-    *value = (static_cast<int32_t>(buffer[0]) << 0) |
-             (static_cast<int32_t>(buffer[1]) << 8) |
-             (static_cast<int32_t>(buffer[2]) << 16) |
-             (static_cast<int32_t>(buffer[3]) << 24);
-    return 4;
-}
-
-template<>
-inline size_t read_le<uint32_t>(uint32_t* value, const uint8_t* buffer) {
-    *value = (static_cast<uint32_t>(buffer[0]) << 0) |
-             (static_cast<uint32_t>(buffer[1]) << 8) |
-             (static_cast<uint32_t>(buffer[2]) << 16) |
-             (static_cast<uint32_t>(buffer[3]) << 24);
-    return 4;
-}
-
-template<>
-inline size_t read_le<uint64_t>(uint64_t* value, const uint8_t* buffer) {
-    *value = (static_cast<uint64_t>(buffer[0]) << 0) |
-             (static_cast<uint64_t>(buffer[1]) << 8) |
-             (static_cast<uint64_t>(buffer[2]) << 16) |
-             (static_cast<uint64_t>(buffer[3]) << 24) |
-             (static_cast<uint64_t>(buffer[4]) << 32) |
-             (static_cast<uint64_t>(buffer[5]) << 40) |
-             (static_cast<uint64_t>(buffer[6]) << 48) |
-             (static_cast<uint64_t>(buffer[7]) << 56);
-    return 8;
+inline size_t read_le(T* value, const uint8_t* buffer){
+    *value = static_cast<T>(buffer[0]);
+    for(size_t i = 1; i < sizeof(*value); ++i){
+        *value |= static_cast<T>(buffer[i]) << i*8;
+    }
+    return sizeof(*value);
 }
 
 template<>
@@ -499,6 +410,14 @@ inline constexpr const char* get_default_json_modifier<float>() {
     return "\"type\":\"float\",\"access\":\"rw\"";
 }
 template<>
+inline constexpr const char* get_default_json_modifier<const int64_t>() {
+    return "\"type\":\"int64\",\"access\":\"r\"";
+}
+template<>
+inline constexpr const char* get_default_json_modifier<int64_t>() {
+    return "\"type\":\"int64\",\"access\":\"rw\"";
+}
+template<>
 inline constexpr const char* get_default_json_modifier<const uint64_t>() {
     return "\"type\":\"uint64\",\"access\":\"r\"";
 }

From 4d1e8152079f748975590264dfe69e129b91309d Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sat, 16 Mar 2019 15:21:32 +0100
Subject: [PATCH 13/31] Fix include paths not being found by tup

---
 Firmware/build.lua | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/build.lua b/Firmware/build.lua
index 8962e403..bf1730a2 100644
--- a/Firmware/build.lua
+++ b/Firmware/build.lua
@@ -162,7 +162,7 @@ function build(args)
 
         outputs.includes = {}
         for _,inc in pairs(args.includes) do
-            table.insert(outputs.includes, tup.nodevariable(inc))
+            table.insert(outputs.includes, inc)
         end
         if args.name != nil then
             all_packages[args.name] = outputs

From d7a067e87dbc13708bb84628240b620a56e0d3c1 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sat, 16 Mar 2019 15:23:29 +0100
Subject: [PATCH 14/31] Build objects to /obj folder

---
 Firmware/build.lua | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/build.lua b/Firmware/build.lua
index bf1730a2..d8a4a0f2 100644
--- a/Firmware/build.lua
+++ b/Firmware/build.lua
@@ -63,7 +63,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags)
             inc_flags += "-I"..inc
         end
         -- todo: vary build directory
-        obj_file = builddir.."/"..src:gsub("/","_")..".o"
+        obj_file = builddir.."/obj/"..src:gsub("/","_")..".o"
         outputs.object_files += obj_file
         if gen_su_file then
             su_file = builddir.."/"..src:gsub("/","_")..".su"

From 68259cd8f175026cfbdbbb2aeb6f8b187f6e9722 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sat, 16 Mar 2019 15:23:42 +0100
Subject: [PATCH 15/31] gcc_generic_compiler must be declared local

---
 Firmware/build.lua                | 2 +-
 Firmware/fibre/tupfiles/build.lua | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Firmware/build.lua b/Firmware/build.lua
index d8a4a0f2..d4c7aad4 100644
--- a/Firmware/build.lua
+++ b/Firmware/build.lua
@@ -56,7 +56,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags)
         compiler_flags += '-fstack-usage'
     end
 
-    gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
+    local gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
         -- convert include list to flags
         inc_flags = {}
         for _,inc in pairs(includes) do
diff --git a/Firmware/fibre/tupfiles/build.lua b/Firmware/fibre/tupfiles/build.lua
index 4a0ea105..8b4a4510 100644
--- a/Firmware/fibre/tupfiles/build.lua
+++ b/Firmware/fibre/tupfiles/build.lua
@@ -5,7 +5,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags)
     -- add some default compiler flags
     compiler_flags += '-fstack-usage'
 
-    gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
+    local gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
         -- resolve source path
         src = tostring(src)
 

From b049e6f0ebcee8ed91dbde734f8ee607702aafb1 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:25:38 +0100
Subject: [PATCH 16/31] Add a config flag for debug builds.  Build with -O2 by
 default

---
 Firmware/Tupfile.lua        | 10 ++++++++--
 Firmware/tup.config.default |  1 +
 2 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua
index 94cb080e..2b1d5997 100644
--- a/Firmware/Tupfile.lua
+++ b/Firmware/Tupfile.lua
@@ -92,7 +92,12 @@ FLAGS += '-mfloat-abi=hard'
 FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'}
 
 -- debug build
-FLAGS += '-g -gdwarf-2'
+if tup.getconfig("DEBUG") == "true" then
+    FLAGS += '-g -gdwarf-2'
+    OPT += '-Og'
+else
+    OPT += '-O2'
+end
 
 
 -- linker flags
@@ -104,7 +109,8 @@ LDFLAGS += '-Wl,--undefined=uxTopUsedPriority'
 
 
 -- common flags for ASM, C and C++
-OPT += '-Og'
+-- OPT += '-Og'
+-- OPT += '-O2'
 -- OPT += '-O0'
 OPT += '-ffast-math -fno-finite-math-only'
 tup.append_table(FLAGS, OPT)
diff --git a/Firmware/tup.config.default b/Firmware/tup.config.default
index 60d2500d..5c2c4822 100644
--- a/Firmware/tup.config.default
+++ b/Firmware/tup.config.default
@@ -3,6 +3,7 @@
 #CONFIG_BOARD_VERSION=v3.5-24V
 CONFIG_USB_PROTOCOL=native
 CONFIG_UART_PROTOCOL=ascii
+CONFIG_DEBUG=false
 
 # Uncomment this to error on compilation warnings
 #CONFIG_STRICT=true

From 2e71e81dd12360ab89f3472f0a1f10a2afce7ba6 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:33:14 +0100
Subject: [PATCH 17/31] Call load_configuration before any threads start

---
 Firmware/Board/v3/Src/freertos.c | 3 +++
 Firmware/MotorControl/main.cpp   | 4 ++--
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c
index 11154587..3c34849b 100644
--- a/Firmware/Board/v3/Src/freertos.c
+++ b/Firmware/Board/v3/Src/freertos.c
@@ -60,6 +60,7 @@
 #include "usb_device.h"
 extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
 int odrive_main(void);
+int load_configuration(void);
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -187,6 +188,8 @@ void MX_FREERTOS_Init(void) {
   sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1);
 
   init_deferred_interrupts();
+
+  load_configuration();
   /* USER CODE END RTOS_SEMAPHORES */
 
   /* USER CODE BEGIN RTOS_TIMERS */
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index 7ffeb5e3..1a1f331d 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -45,7 +45,7 @@ void save_configuration(void) {
     }
 }
 
-void load_configuration(void) {
+extern "C" int load_configuration(void) {
     // Try to load configs
     if (NVM_init() ||
         ConfigFormat::safe_load_config(
@@ -71,6 +71,7 @@ void load_configuration(void) {
     } else {
         user_config_loaded_ = true;
     }
+    return user_config_loaded_;
 }
 
 void erase_configuration(void) {
@@ -117,7 +118,6 @@ void vApplicationIdleHook(void) {
 
 int odrive_main(void) {
     // Load persistent configuration (or defaults)
-    load_configuration();
 
 #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
     if (board_config.enable_i2c_instead_of_can) {

From b9a23a118722d6dd19f1c9a328deaeea8c127945 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:38:29 +0100
Subject: [PATCH 18/31] Add a couple settings for cppcheck

---
 ODrive_Workspace.code-workspace | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace
index 65ed1095..3d990eaf 100644
--- a/ODrive_Workspace.code-workspace
+++ b/ODrive_Workspace.code-workspace
@@ -18,6 +18,8 @@
 			"${workspaceRoot}/communication",
 			"${workspaceRoot}/MotorControl",
 		],
+		"c-cpp-flylint.cppcheck.platform": "avr8",
+		"c-cpp-flylint.cppcheck.standard": ["c99","c++14"],
 
 		"files.associations": {
 			"memory": "cpp",

From b97a91ad94b288630a63426c9b31f20cd95719b2 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:42:47 +0100
Subject: [PATCH 19/31] Remove browse_path locations

---
 Firmware/.vscode/c_cpp_properties.json | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index 98ddcaba..68884c82 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -28,16 +28,10 @@
                 "USB_PROTOCOL_NATIVE",
                 "__weak=\"__attribute__((weak))\"",
                 "__packed=\"__attribute__((__packed__))\"",
-                "__GNUC__"
+                "__GNUC__",
+                "__ODRIVE_MAIN_H"
             ],
             "intelliSenseMode": "clang-x64",
-            "browse": {
-                "path": [
-                    "${workspaceRoot}",
-                    "${ARM_GCC_ROOT}"
-                ],
-                "limitSymbolsToIncludedHeaders": true
-            },
             "compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
             "cStandard": "c11",
             "cppStandard": "c++14"

From b5f5b0396d5ce0f24c8bd32d4301254875bb8291 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:43:18 +0100
Subject: [PATCH 20/31] Declare single-argument constructors explicit

---
 Firmware/MotorControl/controller.hpp           | 2 +-
 Firmware/MotorControl/sensorless_estimator.hpp | 2 +-
 Firmware/MotorControl/trapTraj.hpp             | 3 ++-
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index 7ffa25b6..020f34d0 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -34,7 +34,7 @@ public:
         bool setpoints_in_cpr = false;
     };
 
-    Controller(Config_t& config);
+    explicit Controller(Config_t& config);
     void reset();
     void set_error(Error_t error);
 
diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp
index 6d15820f..719a3227 100644
--- a/Firmware/MotorControl/sensorless_estimator.hpp
+++ b/Firmware/MotorControl/sensorless_estimator.hpp
@@ -14,7 +14,7 @@ public:
         float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)]  { 5.51328895422 / (<pole pairs> * <rpm/v>) }
     };
 
-    SensorlessEstimator(Config_t& config);
+    explicit SensorlessEstimator(Config_t& config);
 
     bool update();
 
diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp
index 42dac0ef..fe5f3fec 100644
--- a/Firmware/MotorControl/trapTraj.hpp
+++ b/Firmware/MotorControl/trapTraj.hpp
@@ -9,13 +9,14 @@ public:
         float decel_limit = 5000.0f; // [count/s^2]
         float A_per_css = 0.0f;      // [A/(count/s^2)]
     };
+    
     struct Step_t {
         float Y;
         float Yd;
         float Ydd;
     };
 
-    TrapezoidalTrajectory(Config_t& config);
+    explicit TrapezoidalTrajectory(Config_t& config);
     bool planTrapezoidal(float Xf, float Xi, float Vi,
                          float Vmax, float Amax, float Dmax);
     Step_t eval(float t);

From 7fbbe2abfff910e4df18b76f79c14ee02b378f84 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:56:17 +0100
Subject: [PATCH 21/31] Use memcpy to put data into the buffer

---
 Firmware/fibre/cpp/include/fibre/protocol.hpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp
index 552dbd9e..ff3e6237 100644
--- a/Firmware/fibre/cpp/include/fibre/protocol.hpp
+++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp
@@ -82,12 +82,11 @@ typedef struct {
     uint16_t endpoint_id;
 } endpoint_ref_t;
 
+#include <cstring>
 
 template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
 inline size_t write_le(T value, uint8_t* buffer){
-    for(size_t i = 0; i < sizeof(value); ++i){
-        buffer[i] = (value >> 8*i) & 0xff;
-    }
+    std::memcpy(&buffer[0], &value, sizeof(value));
     return sizeof(value);
 }
 
@@ -118,6 +117,7 @@ template<>
 inline size_t read_le<float>(float* value, const uint8_t* buffer) {
     static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
     static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
+
     return read_le(reinterpret_cast<uint32_t*>(value), buffer);
 }
 

From b8c144905beb94ee7106cec9923107020cf7e4f7 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 20:59:37 +0100
Subject: [PATCH 22/31] Use memcpy in read to copy data from buffer to value

---
 Firmware/fibre/cpp/include/fibre/protocol.hpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp
index ff3e6237..664ba20d 100644
--- a/Firmware/fibre/cpp/include/fibre/protocol.hpp
+++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp
@@ -106,10 +106,7 @@ inline size_t write_le<float>(float value, uint8_t* buffer) {
 
 template<typename T>
 inline size_t read_le(T* value, const uint8_t* buffer){
-    *value = static_cast<T>(buffer[0]);
-    for(size_t i = 1; i < sizeof(*value); ++i){
-        *value |= static_cast<T>(buffer[i]) << i*8;
-    }
+    std::memcpy(value, buffer, sizeof(*value));
     return sizeof(*value);
 }
 

From b5609a8da0ea23be90c765c058871308aee3a266 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 17 Mar 2019 21:02:40 +0100
Subject: [PATCH 23/31] Remove dumb define in intellisense

---
 Firmware/.vscode/c_cpp_properties.json | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index 68884c82..4b93d26e 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -28,8 +28,7 @@
                 "USB_PROTOCOL_NATIVE",
                 "__weak=\"__attribute__((weak))\"",
                 "__packed=\"__attribute__((__packed__))\"",
-                "__GNUC__",
-                "__ODRIVE_MAIN_H"
+                "__GNUC__"
             ],
             "intelliSenseMode": "clang-x64",
             "compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",

From 901311b582cc6904c4ea76322a8df33d5b961ca6 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 18 Mar 2019 17:57:44 -0700
Subject: [PATCH 24/31] add ODrive v3.6

---
 Firmware/.vscode/c_cpp_properties.json | 4 ++--
 Firmware/Board/v3/Inc/main.h           | 2 +-
 Firmware/MotorControl/odrive_main.h    | 2 +-
 Firmware/Tupfile.lua                   | 8 ++++++++
 4 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index 570c0f29..29d5fa77 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -24,8 +24,8 @@
                 "STM32F405xx",
                 "USE_HAL_DRIVER",
                 "HW_VERSION_MAJOR=3",
-                "HW_VERSION_MINOR=5",
-                "HW_VERSION_VOLTAGE=24",
+                "HW_VERSION_MINOR=6",
+                "HW_VERSION_VOLTAGE=56",
                 "USB_PROTOCOL_NATIVE",
                 "__weak=\"__attribute__((weak))\"",
                 "__packed=\"__attribute__((__packed__))\"",
diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h
index d41b19ee..bfd9888c 100644
--- a/Firmware/Board/v3/Inc/main.h
+++ b/Firmware/Board/v3/Inc/main.h
@@ -171,7 +171,7 @@
 #define CURRENT_MEAS_PERIOD ( (float)2*TIM_1_8_PERIOD_CLOCKS*(TIM_1_8_RCR+1) / (float)TIM_1_8_CLOCK_HZ )
 #define CURRENT_MEAS_HZ ( (float)(TIM_1_8_CLOCK_HZ) / (float)(2*TIM_1_8_PERIOD_CLOCKS*(TIM_1_8_RCR+1)) )
 
-#if HW_VERSION_VOLTAGE == 48
+#if HW_VERSION_VOLTAGE >= 48
 #define VBUS_S_DIVIDER_RATIO 19.0f
 #define VBUS_OVERVOLTAGE_LEVEL 52.0f
 #elif HW_VERSION_VOLTAGE == 24
diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h
index df1bc14b..677fb996 100644
--- a/Firmware/MotorControl/odrive_main.h
+++ b/Firmware/MotorControl/odrive_main.h
@@ -77,7 +77,7 @@ struct BoardConfig_t {
     float brake_resistance = 0.47f;     // [ohm]
 #endif
     float dc_bus_undervoltage_trip_level = 8.0f;                        //<! [V] minimum voltage below which the motor stops operating
-    float dc_bus_overvoltage_trip_level = 1.08f * HW_VERSION_VOLTAGE;   //<! [V] maximum voltage above which the motor stops operating.
+    float dc_bus_overvoltage_trip_level = 1.07f * HW_VERSION_VOLTAGE;   //<! [V] maximum voltage above which the motor stops operating.
                                                                         //<! This protects against cases in which the power supply fails to dissipate
                                                                         //<! the brake power if the brake resistor is disabled.
                                                                         //<! The default is 26V for the 24V board version and 52V for the 48V board version.
diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua
index 94cb080e..daf18dcc 100644
--- a/Firmware/Tupfile.lua
+++ b/Firmware/Tupfile.lua
@@ -31,6 +31,14 @@ elseif boardversion == "v3.5-48V" then
     boarddir = 'Board/v3'
     FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=5"
     FLAGS += "-DHW_VERSION_VOLTAGE=48"
+elseif boardversion == "v3.6-24V" then
+    boarddir = 'Board/v3'
+    FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=6"
+    FLAGS += "-DHW_VERSION_VOLTAGE=24"
+elseif boardversion == "v3.6-56V" then
+    boarddir = 'Board/v3'
+    FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=6"
+    FLAGS += "-DHW_VERSION_VOLTAGE=56"
 elseif boardversion == "" then
     error("board version not specified - take a look at tup.config.default")
 else

From 2b6626a76d85c2bce06cf55e0c0d07630171d478 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 18 Mar 2019 18:36:08 -0700
Subject: [PATCH 25/31] update default OTP in makefile

---
 Firmware/Makefile       | 4 ++--
 tools/odrive/version.py | 4 ++++
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/Firmware/Makefile b/Firmware/Makefile
index 82ae758c..263ba433 100644
--- a/Firmware/Makefile
+++ b/Firmware/Makefile
@@ -70,8 +70,8 @@ ifeq ($(OTP_CONFIRM),TRUE)
 		-c 'mwb 0x1fff7801 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7802 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7803 3' -c 'sleep 10' \
-		-c 'mwb 0x1fff7804 5' -c 'sleep 10' \
-		-c 'mwb 0x1fff7805 48' -c 'sleep 10' \
+		-c 'mwb 0x1fff7804 6' -c 'sleep 10' \
+		-c 'mwb 0x1fff7805 56' -c 'sleep 10' \
 		-c 'reset run' \
 		-c exit
 	@echo "OK"
diff --git a/tools/odrive/version.py b/tools/odrive/version.py
index 5a2827a3..64742ff0 100644
--- a/tools/odrive/version.py
+++ b/tools/odrive/version.py
@@ -70,6 +70,10 @@ if __name__ == '__main__':
     args = parser.parse_args()
 
     git_name, major, minor, revision, unreleased = get_version_from_git()
+
+    #TODO temporary override to get around makefile editing for OTP
+    unreleased = False
+
     print('Firmware version {}.{}.{}{} ({})'.format(
         major, minor, revision, '-dev' if unreleased else '',
         git_name))

From c399187edd3085732422bf05d74a3461ec43a5d9 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 18 Mar 2019 18:36:35 -0700
Subject: [PATCH 26/31] Revert "update default OTP in makefile"

This reverts commit 2b6626a76d85c2bce06cf55e0c0d07630171d478.
---
 Firmware/Makefile       | 4 ++--
 tools/odrive/version.py | 4 ----
 2 files changed, 2 insertions(+), 6 deletions(-)

diff --git a/Firmware/Makefile b/Firmware/Makefile
index 263ba433..82ae758c 100644
--- a/Firmware/Makefile
+++ b/Firmware/Makefile
@@ -70,8 +70,8 @@ ifeq ($(OTP_CONFIRM),TRUE)
 		-c 'mwb 0x1fff7801 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7802 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7803 3' -c 'sleep 10' \
-		-c 'mwb 0x1fff7804 6' -c 'sleep 10' \
-		-c 'mwb 0x1fff7805 56' -c 'sleep 10' \
+		-c 'mwb 0x1fff7804 5' -c 'sleep 10' \
+		-c 'mwb 0x1fff7805 48' -c 'sleep 10' \
 		-c 'reset run' \
 		-c exit
 	@echo "OK"
diff --git a/tools/odrive/version.py b/tools/odrive/version.py
index 64742ff0..5a2827a3 100644
--- a/tools/odrive/version.py
+++ b/tools/odrive/version.py
@@ -70,10 +70,6 @@ if __name__ == '__main__':
     args = parser.parse_args()
 
     git_name, major, minor, revision, unreleased = get_version_from_git()
-
-    #TODO temporary override to get around makefile editing for OTP
-    unreleased = False
-
     print('Firmware version {}.{}.{}{} ({})'.format(
         major, minor, revision, '-dev' if unreleased else '',
         git_name))

From e7953744343dfa84f5bc848ef3511d600ef9b77f Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 18 Mar 2019 18:38:18 -0700
Subject: [PATCH 27/31] update default OTP in makefile

---
 Firmware/Makefile | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Firmware/Makefile b/Firmware/Makefile
index 82ae758c..263ba433 100644
--- a/Firmware/Makefile
+++ b/Firmware/Makefile
@@ -70,8 +70,8 @@ ifeq ($(OTP_CONFIRM),TRUE)
 		-c 'mwb 0x1fff7801 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7802 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7803 3' -c 'sleep 10' \
-		-c 'mwb 0x1fff7804 5' -c 'sleep 10' \
-		-c 'mwb 0x1fff7805 48' -c 'sleep 10' \
+		-c 'mwb 0x1fff7804 6' -c 'sleep 10' \
+		-c 'mwb 0x1fff7805 56' -c 'sleep 10' \
 		-c 'reset run' \
 		-c exit
 	@echo "OK"

From e94230275eae8865b0aea40bdf16347297f4552c Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 18 Mar 2019 21:09:46 -0700
Subject: [PATCH 28/31] add python side enum, fix instant timout on 1st
 iteration

---
 Firmware/MotorControl/axis.cpp | 3 +++
 tools/odrive/enums.py          | 1 +
 2 files changed, 4 insertions(+)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index d20f292a..80987c00 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -99,6 +99,9 @@ void Axis::update_watchdog_settings() {
     } else {
         watchdog_reset_value_ = static_cast<uint32_t>(config_.watchdog_timeout * current_meas_hz);
     }
+
+    // Do a feed to avoid instant timeout
+    watchdog_feed();
 }
 
 // @brief (de)activates step/dir input
diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py
index fc5439bd..d4425a21 100644
--- a/tools/odrive/enums.py
+++ b/tools/odrive/enums.py
@@ -27,6 +27,7 @@ class errors:
         ERROR_ENCODER_FAILED = 0x100 # Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
         ERROR_CONTROLLER_FAILED = 0x200
         ERROR_POS_CTRL_DURING_SENSORLESS = 0x400
+        ERROR_WATCHDOG_TIMER_EXPIRED = 0x800
 
     class motor:
         ERROR_NONE = 0

From dc968c8f3d5ef7710c81b1765a0b932a82a2059a Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 18 Mar 2019 21:12:51 -0700
Subject: [PATCH 29/31] update changelog

---
 CHANGELOG.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 975a62d9..e0d2f219 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,6 +1,9 @@
 # Unreleased Features
 Please add a note of your changes below this heading if you make a Pull Request.
 
+### Added
+* Communication watchdog feature.
+
 # Releases
 ## [0.4.8] - 2019-02-25
 ### Added

From 8695f1fc6e69ef8fdb9c02b2d5ddd279169ca2e7 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Tue, 19 Mar 2019 07:10:04 +0100
Subject: [PATCH 30/31] Apply comment suggestions from code review

Co-Authored-By: Wetmelon <Wetmelon@users.noreply.github.com>
---
 Firmware/Board/v3/Src/freertos.c              | 1 +
 Firmware/MotorControl/main.cpp                | 1 -
 Firmware/fibre/cpp/include/fibre/protocol.hpp | 2 ++
 3 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c
index 3c34849b..ac6c6de9 100644
--- a/Firmware/Board/v3/Src/freertos.c
+++ b/Firmware/Board/v3/Src/freertos.c
@@ -189,6 +189,7 @@ void MX_FREERTOS_Init(void) {
 
   init_deferred_interrupts();
 
+  // Load persistent configuration (or defaults)
   load_configuration();
   /* USER CODE END RTOS_SEMAPHORES */
 
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index 1a1f331d..427792b3 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -117,7 +117,6 @@ void vApplicationIdleHook(void) {
 }
 
 int odrive_main(void) {
-    // Load persistent configuration (or defaults)
 
 #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
     if (board_config.enable_i2c_instead_of_can) {
diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp
index 664ba20d..498d5172 100644
--- a/Firmware/fibre/cpp/include/fibre/protocol.hpp
+++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp
@@ -86,6 +86,7 @@ typedef struct {
 
 template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
 inline size_t write_le(T value, uint8_t* buffer){
+    //TODO: add static_assert that this is still a little endian machine
     std::memcpy(&buffer[0], &value, sizeof(value));
     return sizeof(value);
 }
@@ -106,6 +107,7 @@ inline size_t write_le<float>(float value, uint8_t* buffer) {
 
 template<typename T>
 inline size_t read_le(T* value, const uint8_t* buffer){
+    // TODO: add static_assert that this is still a little endian machine
     std::memcpy(value, buffer, sizeof(*value));
     return sizeof(*value);
 }

From b8bcdb8fe36edc9059f09788aa38a9e6fd5e0f1c Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Tue, 19 Mar 2019 14:13:51 -0700
Subject: [PATCH 31/31] reorder some stuff

---
 Firmware/Tupfile.lua | 19 +++++++------------
 1 file changed, 7 insertions(+), 12 deletions(-)

diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua
index 9f59f34f..994fd746 100644
--- a/Firmware/Tupfile.lua
+++ b/Firmware/Tupfile.lua
@@ -99,6 +99,13 @@ FLAGS += '-mfpu=fpv4-sp-d16'
 FLAGS += '-mfloat-abi=hard'
 FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'}
 
+-- linker flags
+LDFLAGS += '-T'..boarddir..'/STM32F405RGTx_FLASH.ld'
+LDFLAGS += '-L'..boarddir..'/Drivers/CMSIS/Lib' -- lib dir
+LDFLAGS += '-lc -lm -lnosys -larm_cortexM4lf_math' -- libs
+LDFLAGS += '-mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float -Wl,--cref -Wl,--gc-sections'
+LDFLAGS += '-Wl,--undefined=uxTopUsedPriority'
+
 -- debug build
 if tup.getconfig("DEBUG") == "true" then
     FLAGS += '-g -gdwarf-2'
@@ -107,19 +114,7 @@ else
     OPT += '-O2'
 end
 
-
--- linker flags
-LDFLAGS += '-T'..boarddir..'/STM32F405RGTx_FLASH.ld'
-LDFLAGS += '-L'..boarddir..'/Drivers/CMSIS/Lib' -- lib dir
-LDFLAGS += '-lc -lm -lnosys -larm_cortexM4lf_math' -- libs
-LDFLAGS += '-mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float -Wl,--cref -Wl,--gc-sections'
-LDFLAGS += '-Wl,--undefined=uxTopUsedPriority'
-
-
 -- common flags for ASM, C and C++
--- OPT += '-Og'
--- OPT += '-O2'
--- OPT += '-O0'
 OPT += '-ffast-math -fno-finite-math-only'
 tup.append_table(FLAGS, OPT)
 tup.append_table(LDFLAGS, OPT)