diff --git a/CHANGELOG.md b/CHANGELOG.md
index 75d2ef41..7d6fa030 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,8 +1,6 @@
 ### Changes this version:
-- Reformatted USB serial string as hex and added command to request UID as hex string
-- Added device name to USB Product name
-- Added support for F407 OTP section
-- Added support for MagnTek MT6835 via SPI (SPI3 port, MagnTek encoder class)
+- Added SPI speed selector to MagnTek encoders
+- Added "reg" and "save" commands to MagnTek encoder. Allows programming MT6835 encoders (debug=1 mode required!)
 
 ### Changes in 1.16:
 
@@ -19,4 +17,8 @@ Internal changes:
 - Using analog VREF for voltage sensing (better accuracy with unstable 3.3V)
 - Added chip temperature readout
 - Added remote CAN button/analog source mainclass
-- Added exponential torque postprocessing for game effects
\ No newline at end of file
+- Added exponential torque postprocessing for game effects
+- Reformatted USB serial string as hex and added command to request UID as hex string
+- Added device name to USB Product name
+- Added support for F407 OTP section
+- Added support for MagnTek MT6835 via SPI (SPI3 port, MagnTek encoder class)
\ No newline at end of file
diff --git a/Configurator b/Configurator
index a3bc7f11..9eee2b5f 160000
--- a/Configurator
+++ b/Configurator
@@ -1 +1 @@
-Subproject commit a3bc7f11aeb04853ae8bbc1446b6761d2a739588
+Subproject commit 9eee2b5ffe44f2dec1c6d2eae4a0ad987d53b5c8
diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
index 05e87fdc..bed8e6eb 100644
--- a/Firmware/FFBoard/Inc/Axis.h
+++ b/Firmware/FFBoard/Inc/Axis.h
@@ -239,6 +239,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	int32_t effectTorque = 0;
 	int32_t axisEffectTorque = 0;
 	uint8_t fx_ratio_i = 204; // Reduce effects to a certain ratio of the total power to have a margin for the endstop. 80% = 204
+	uint16_t timeSincePosChange = 1;
 	uint16_t power = 5000;
 	float torqueScaler = 0; // power * fx_ratio as a ratio between 0 & 1
 	float effect_margin_scaler = 0;
diff --git a/Firmware/FFBoard/Inc/CAN.h b/Firmware/FFBoard/Inc/CAN.h
index d58a3184..06357e6e 100644
--- a/Firmware/FFBoard/Inc/CAN.h
+++ b/Firmware/FFBoard/Inc/CAN.h
@@ -11,6 +11,7 @@
 #ifdef CANBUS
 //#include "CanHandler.h"
 #include "main.h"
+#include <algorithm>
 #include <vector>
 #include "semaphore.hpp"
 #include <GPIOPin.h>
diff --git a/Firmware/FFBoard/Inc/constants.h b/Firmware/FFBoard/Inc/constants.h
index 8f6724a9..e18918af 100644
--- a/Firmware/FFBoard/Inc/constants.h
+++ b/Firmware/FFBoard/Inc/constants.h
@@ -8,7 +8,7 @@
  * For more settings see target_constants.h in a target specific folder
  */
 
-static const uint8_t SW_VERSION_INT[3] = {1,16,5}; // Version as array. 8 bit each!
+static const uint8_t SW_VERSION_INT[3] = {1,16,6}; // Version as array. 8 bit each!
 #ifndef MAX_AXIS
 #define MAX_AXIS 2 // ONLY USE 2 for now else screws HID Reports
 #endif
diff --git a/Firmware/FFBoard/Inc/ringbufferwrapper.h b/Firmware/FFBoard/Inc/ringbufferwrapper.h
index 2ba46b8c..99eef8ca 100644
--- a/Firmware/FFBoard/Inc/ringbufferwrapper.h
+++ b/Firmware/FFBoard/Inc/ringbufferwrapper.h
@@ -57,7 +57,7 @@ T RingBufferWrapper::peek_as(bool* ok) noexcept
    T data;
    // Only POD types can be trivially copied from
    // the ring buffer.
-   if (!std::is_pod<T>::value) {
+   if (!std::is_standard_layout<T>::value && !std::is_trivial<T>::value) {
       *ok = false;
       return data;
    }
diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
index f5394a29..c700caf9 100644
--- a/Firmware/FFBoard/Src/Axis.cpp
+++ b/Firmware/FFBoard/Src/Axis.cpp
@@ -13,6 +13,7 @@
 #include "ODriveCAN.h"
 #include "MotorSimplemotion.h"
 #include "RmdMotorCAN.h"
+#include "critical.hpp"
 
 //////////////////////////////////////////////
 /*
@@ -282,8 +283,9 @@ uint8_t Axis::getEncType(){
 void Axis::setPos(uint16_t val)
 {
 	startForceFadeIn(0.25,0.5);
-	if(this->drv != nullptr){
-		drv->getEncoder()->setPos(val);
+	Encoder* enc_p = getEncoder();
+	if(enc_p != nullptr){
+		enc_p->setPos(val);
 	}
 }
 
@@ -292,6 +294,7 @@ MotorDriver* Axis::getDriver(){
 }
 
 Encoder* Axis::getEncoder(){
+	if(!drv) return nullptr;
 	return drv->getEncoder();
 }
 
@@ -306,7 +309,7 @@ void Axis::prepareForUpdate(){
 
 	//if (!drv->motorReady()) return;
 
-	float angle = getEncAngle(this->drv->getEncoder());
+	float angle = getEncAngle(getEncoder());
 
 	// Scale encoder value to set rotation range
 	// Update a change of range only when new range is within valid range
@@ -394,13 +397,13 @@ void Axis::setDrvType(uint8_t drvtype)
 	{
 		return;
 	}
-	this->drv.reset(nullptr);
-	MotorDriver* drv = drv_chooser.Create((uint16_t)drvtype);
+	cpp_freertos::CriticalSection::Enter();
+	this->drv.reset(drv_chooser.Create((uint16_t)drvtype));
 	if (drv == nullptr)
 	{
+		cpp_freertos::CriticalSection::Exit();
 		return;
 	}
-	this->drv = std::unique_ptr<MotorDriver>(drv);
 	this->conf.drvtype = drvtype;
 
 	// Pass encoder to driver again
@@ -408,7 +411,7 @@ void Axis::setDrvType(uint8_t drvtype)
 		this->drv->setEncoder(this->enc);
 	}
 #ifdef TMC4671DRIVER
-	if (dynamic_cast<TMC4671 *>(drv))
+	if (dynamic_cast<TMC4671 *>(drv.get()))
 	{
 		setupTMC4671();
 	}
@@ -422,6 +425,7 @@ void Axis::setDrvType(uint8_t drvtype)
 	{
 		drv->startMotor();
 	}
+	cpp_freertos::CriticalSection::Exit();
 }
 
 #ifdef TMC4671DRIVER
@@ -435,7 +439,7 @@ void Axis::setupTMC4671()
 	tmclimits.pid_torque_flux = getPower();
 	drv->setLimits(tmclimits);
 	//drv->setBiquadTorque(TMC4671Biquad(tmcbq_500hz_07q_25k));
-	
+
 
 	// Enable driver
 
@@ -461,7 +465,7 @@ void Axis::setEncType(uint8_t enctype)
 		this->conf.enctype = 0; // None encoder
 	}
 
-	float angle = getEncAngle(this->drv->getEncoder());
+	float angle = getEncAngle(this->getEncoder());
 	//int32_t scaledEnc = scaleEncValue(angle, degreesOfRotation);
 	// reset metrics
 	this->resetMetrics(angle);
@@ -652,12 +656,32 @@ void Axis::updateMetrics(float new_pos) { // pos is degrees
 	std::tie(metric.current.pos,metric.current.pos_f) = scaleEncValue(new_pos, degreesOfRotation);
 
 
-	// compute speed and accel from raw instant speed normalized
-	float currentSpeed = (new_pos - metric.previous.posDegrees) * 1000.0; // deg/s
-	metric.current.speed = speedFilter.process(currentSpeed);
-	metric.current.accel = accelFilter.process((currentSpeed - _lastSpeed))* 1000.0; // deg/s/s
-	_lastSpeed = currentSpeed;
-
+	// compute speed and accel from time between enocder changes
+	const float pos_change = new_pos - metric.previous.posDegrees;
+	timeSincePosChange += 1;
+	const float tick_rate = 1000.0f;
+	const uint16_t update_timeout = 1000; // 1 second
+	const float update_time_inv = (tick_rate/timeSincePosChange);
+	if ((fabsf(pos_change) > 1e-8f)) {
+		const float currentSpeed = pos_change * update_time_inv; // deg/s
+		metric.current.speed = speedFilter.process(currentSpeed);
+		metric.current.accel = accelFilter.process((currentSpeed - _lastSpeed) * tick_rate); // deg/s/s
+		_lastSpeed = currentSpeed;
+		timeSincePosChange = 0;
+	} else if (timeSincePosChange >= update_timeout) {
+		metric.current.speed = speedFilter.process(0.0f);
+		metric.current.accel = accelFilter.process(0.0f);
+	} else {
+		const float speed_dir = copysign(1.0f,metric.previous.speed);
+		// Encoder* enc = getEncoder();
+		const uint32_t cpr = 10000;//enc->getCpr();
+		const float angle_inc = 360.0f / cpr; // deg
+		const float speed_bound = angle_inc/update_time_inv; // deg/sec
+		const float speed_est = speed_dir * std::min(fabsf(metric.previous.speed), speed_bound); // deg/sec
+		metric.current.speed = speedFilter.process(speed_est);
+		metric.current.accel = accelFilter.process((speed_est - _lastSpeed) * tick_rate); // deg/s/s
+		_lastSpeed = speed_est;
+	}
 }
 
 
@@ -939,14 +963,14 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 		break;
 
 	case Axis_commands::pos:
-		if (cmd.type == CMDtype::get && this->drv->getEncoder() != nullptr)
+		if (cmd.type == CMDtype::get && getEncoder() != nullptr)
 		{
-			int32_t pos = this->drv->getEncoder()->getPos();
+			int32_t pos = getEncoder()->getPos();
 			replies.emplace_back(isInverted() ? -pos : pos);
 		}
-		else if (cmd.type == CMDtype::set && this->drv->getEncoder() != nullptr)
+		else if (cmd.type == CMDtype::set && getEncoder() != nullptr)
 		{
-			this->drv->getEncoder()->setPos(isInverted() ? -cmd.val : cmd.val);
+			getEncoder()->setPos(isInverted() ? -cmd.val : cmd.val);
 		}
 		else
 		{
@@ -1024,8 +1048,8 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 		if (cmd.type == CMDtype::get)
 		{
 			uint32_t cpr = 0;
-			if(this->drv->getEncoder() != nullptr){
-				cpr = this->drv->getEncoder()->getCpr();
+			if(this->getEncoder() != nullptr){
+				cpr = this->getEncoder()->getCpr();
 			}
 //#ifdef TMC4671DRIVER // CPR should be consistent with position. Maybe change TMC to prescale to encoder count or correct readout in UI
 //			TMC4671 *tmcdrv = dynamic_cast<TMC4671 *>(this->drv.get()); // Special case for TMC. Get the actual encoder resolution
diff --git a/Firmware/FFBoard/Src/CommandHandler.cpp b/Firmware/FFBoard/Src/CommandHandler.cpp
index ae7c3fcb..1c488a83 100644
--- a/Firmware/FFBoard/Src/CommandHandler.cpp
+++ b/Firmware/FFBoard/Src/CommandHandler.cpp
@@ -12,6 +12,7 @@
 #include "CDCcomm.h"
 //#include <set>
 #include "ChoosableClass.h"
+#include <algorithm>
 
 //std::vector<CommandHandler*> CommandHandler::cmdHandlers;
 //std::set<uint16_t> CommandHandler::cmdHandlerIDs;
diff --git a/Firmware/FFBoard/Src/ErrorHandler.cpp b/Firmware/FFBoard/Src/ErrorHandler.cpp
index 94f6eaf2..dc7ff60e 100644
--- a/Firmware/FFBoard/Src/ErrorHandler.cpp
+++ b/Firmware/FFBoard/Src/ErrorHandler.cpp
@@ -10,6 +10,7 @@
 #include "FFBoardMain.h"
 #include "cppmain.h"
 #include "critical.hpp"
+#include <algorithm>
 #include <span>
 
 std::vector<ErrorHandler*> ErrorHandler::errorHandlers;
diff --git a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h
index 38163662..981dea2d 100644
--- a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h
+++ b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h
@@ -19,11 +19,12 @@
 
 class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage, public CommandHandler,cpp_freertos::Thread{
 	enum class MtEncoderSPI_commands : uint32_t{
-		cspin,pos,errors,mode
+		cspin,pos,errors,mode,speed,reg,save
 	};
 	enum class MtEncoderSPI_mode : uint8_t{
 		mt6825,mt6835
 	};
+	const std::array<float,3> spispeeds = {10e6,5e6,2.5e6}; // Target speeds. Must double each entry
 public:
 	MtEncoderSPI();
 	virtual ~MtEncoderSPI();
@@ -57,10 +58,13 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage,
 
 	//bool useDMA = false; // if true uses DMA for angle updates instead of polling SPI. TODO when used with tmc external encoder using DMA will hang the interrupt randomly
 
+	void setSpiSpeed(uint8_t preset);
+
 private:
 	uint8_t readSpi(uint16_t addr);
 	void writeSpi(uint16_t addr,uint8_t data);
 	void spiTxRxCompleted(SPIPort* port);
+	bool saveEeprom();
 
 
 	bool nomag = false; // Magnet lost in last report
@@ -85,6 +89,8 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage,
 
 	static std::array<uint8_t,256> tableCRC;
 	const uint8_t POLY = 0x07;
+
+	uint8_t spiSpeedPreset = 0;
 };
 
 #endif /* USEREXTENSIONS_SRC_MTENCODERSPI_H_ */
diff --git a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp
index b0ef5bf1..54148bad 100644
--- a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp
+++ b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp
@@ -23,7 +23,7 @@ std::array<uint8_t,256> MtEncoderSPI::tableCRC __attribute__((section (".ccmram"
 
 MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFreeCsPins()[0]), CommandHandler("mtenc",CLSID_ENCODER_MTSPI,0),cpp_freertos::Thread("MTENC",256,42) {
 	MtEncoderSPI::inUse = true;
-	this->spiConfig.peripheral.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4; // 4 = 10MHz 8 = 5MHz
+	this->spiConfig.peripheral.BaudRatePrescaler = spiPort.getClosestPrescaler(10e6).first; // 10MHz max
 	this->spiConfig.peripheral.FirstBit = SPI_FIRSTBIT_MSB;
 	this->spiConfig.peripheral.CLKPhase = SPI_PHASE_2EDGE;
 	this->spiConfig.peripheral.CLKPolarity = SPI_POLARITY_HIGH;
@@ -32,7 +32,7 @@ MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFr
 	//Init CRC-8 table
 	makeCrcTable(tableCRC,POLY,8); // Mt6825, Poly X8+X2+X (+1)
 
-	restoreFlash();
+	restoreFlash(); // Also configures SPI port
 	spiPort.reserveCsPin(this->spiConfig.cs);
 
 	CommandHandler::registerCommands();
@@ -40,6 +40,9 @@ MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFr
 	registerCommand("pos", MtEncoderSPI_commands::pos, "Position",CMDFLAG_GET | CMDFLAG_SET);
 	registerCommand("errors", MtEncoderSPI_commands::errors, "Parity error count",CMDFLAG_GET);
 	registerCommand("mode", MtEncoderSPI_commands::mode, "Encoder mode (MT6825=0;MT6835=1)",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING);
+	registerCommand("speed", MtEncoderSPI_commands::speed, "SPI speed preset",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING);
+	registerCommand("reg", MtEncoderSPI_commands::reg, "Read/Write register",CMDFLAG_GETADR | CMDFLAG_SETADR | CMDFLAG_DEBUG);
+	registerCommand("save", MtEncoderSPI_commands::save, "Save to memory",CMDFLAG_GET | CMDFLAG_DEBUG);
 	this->Start();
 }
 
@@ -51,14 +54,16 @@ MtEncoderSPI::~MtEncoderSPI() {
 void MtEncoderSPI::restoreFlash(){
 	uint16_t conf_int = Flash_ReadDefault(ADR_MTENC_CONF1, 0);
 	offset = Flash_ReadDefault(ADR_MTENC_OFS, 0) << 2;
-	uint8_t cspin = conf_int & 0xF;
+	uint8_t cspin = conf_int & 0x3;
 	MtEncoderSPI_mode mode = static_cast<MtEncoderSPI_mode>(conf_int >> 8);
 	setMode(mode);
 	setCsPin(cspin);
+	setSpiSpeed((conf_int >> 2) & 0x3);
 }
 
 void MtEncoderSPI::saveFlash(){
-	uint16_t conf_int = this->cspin & 0xF;
+	uint16_t conf_int = this->cspin & 0x3;
+	conf_int |= (this->spiSpeedPreset & 0x3) << 2;
 	conf_int |= ((uint8_t)mode & 0xf) << 8;
 	Flash_Write(ADR_MTENC_CONF1, conf_int);
 	Flash_Write(ADR_MTENC_OFS, offset >> 2);
@@ -114,13 +119,13 @@ uint8_t MtEncoderSPI::readSpi(uint16_t addr){
 		uint8_t txbuf[2] = {(uint8_t)(addr | 0x80),0};
 		uint8_t rxbuf[2] = {0,0};
 		spiPort.transmitReceive(txbuf, rxbuf, 2, this,100);
+		return rxbuf[1];
 	}else if(mode == MtEncoderSPI_mode::mt6835){
 		uint8_t txbuf[3] = {(uint8_t)((addr & 0xf00) | 0x30),(uint8_t)(addr & 0xff),0};
 		uint8_t rxbuf[3] = {0,0,0};
 		spiPort.transmitReceive(txbuf, rxbuf, 3, this,100);
+		return rxbuf[2];
 	}
-
-	return rxbuf[1];
 }
 
 void MtEncoderSPI::writeSpi(uint16_t addr,uint8_t data){
@@ -134,6 +139,22 @@ void MtEncoderSPI::writeSpi(uint16_t addr,uint8_t data){
 
 }
 
+/**
+ * Saves current configuration to permanent storage
+ */
+bool MtEncoderSPI::saveEeprom(){
+	if(mode != MtEncoderSPI_mode::mt6835){
+		return false;
+	}
+	if(mode == MtEncoderSPI_mode::mt6835){
+		uint8_t txbuf[3] = {0xC0,0x00,0x00};
+		uint8_t rxbuf[3] = {0,0,0};
+		spiPort.transmitReceive(txbuf, rxbuf, 3, this,100);
+		return rxbuf[2] == 0x55;
+	}
+	return false;
+}
+
 void MtEncoderSPI::setPos(int32_t pos){
 	offset = curPos - pos;
 }
@@ -154,8 +175,6 @@ void MtEncoderSPI::spiTxRxCompleted(SPIPort* port){
  */
 void MtEncoderSPI::updateAngleStatus(){
 
-
-
 	if(mode == MtEncoderSPI_mode::mt6825){
 		uint8_t txbufNew[5] = {0x03 | 0x80,0,0,0,0};
 		memcpy(this->txbuf,txbufNew,5);
@@ -262,6 +281,15 @@ void MtEncoderSPI::setMode(MtEncoderSPI::MtEncoderSPI_mode mode){
 	this->overspeed = false;
 }
 
+void MtEncoderSPI::setSpiSpeed(uint8_t preset){
+	if(preset == spiSpeedPreset){
+		return; // Ignore if no change
+	}
+	spiSpeedPreset = clip<uint8_t,uint8_t>(preset,0,spispeeds.size());
+	this->spiConfig.peripheral.BaudRatePrescaler = spiPort.getClosestPrescaler(spispeeds[spiSpeedPreset]).first;
+	initSPI();
+}
+
 
 CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vector<CommandReply>& replies){
 	switch(static_cast<MtEncoderSPI_commands>(cmd.cmdId)){
@@ -298,6 +326,40 @@ CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vector<Command
 			return CommandStatus::ERR;
 		}
 		break;
+
+	case MtEncoderSPI_commands::speed:
+	{
+		if(cmd.type == CMDtype::get){
+			replies.emplace_back(spiSpeedPreset);
+		}else if(cmd.type == CMDtype::set){
+			setSpiSpeed(cmd.val);
+		}else if(cmd.type == CMDtype::info){
+			for(uint8_t i = 0; i<spispeeds.size();i++){
+				replies.emplace_back(std::to_string(this->spiPort.getClosestPrescaler(spispeeds[i]).second)  + ":" + std::to_string(i)+"\n");
+			}
+		}else{
+			return CommandStatus::ERR;
+		}
+		break;
+	}
+	case MtEncoderSPI_commands::reg:
+	{
+		if(cmd.type == CMDtype::getat){
+			replies.emplace_back(readSpi(cmd.adr));
+		}else if(cmd.type == CMDtype::setat){
+			writeSpi(cmd.adr, cmd.val);
+		}else{
+			return CommandStatus::ERR;
+		}
+		break;
+	}
+	case MtEncoderSPI_commands::save:
+	{
+		if(cmd.type == CMDtype::get){
+			replies.emplace_back(saveEeprom() ? 1 : 0);
+		}
+		break;
+	}
 	default:
 		return CommandStatus::NOT_FOUND;
 	}
diff --git a/Firmware/Makefile b/Firmware/Makefile
index 4c50b75b..b5e4fb33 100644
--- a/Firmware/Makefile
+++ b/Firmware/Makefile
@@ -26,7 +26,7 @@ BUILD_DIR = build
 ######################################
 
 # Choose your board mcu F407VG or F411RE
-MCU_TARGET=F407VG
+MCU_TARGET ?= F407VG
 # The directory of mcu target
 TARGET_DIR = Targets/$(MCU_TARGET)
 
@@ -36,7 +36,7 @@ TARGET_DIR = Targets/$(MCU_TARGET)
 TARGET = OpenFFBoard_$(MCU_TARGET)
 
 # Output directory for hex/bin files
-OUTPUT_DIR = $(BUILD_DIR)
+OUTPUT_DIR ?= $(BUILD_DIR)
 
 # C sources
 C_SOURCES = $(wildcard $(TARGET_DIR)/Core/Src/*.c)
@@ -229,15 +229,15 @@ $(BUILD_DIR)/$(TARGET).elf: $(OBJECTS) Makefile
 
 $(OUTPUT_DIR)/%.hex: $(BUILD_DIR)/%.elf | $(OUTPUT_DIR)
 	$(HEX) $< $@
-	
+
 $(OUTPUT_DIR)/%.bin: $(BUILD_DIR)/%.elf | $(OUTPUT_DIR)
-	$(BIN) $< $@	
-	
+	$(BIN) $< $@
+
 $(BUILD_DIR):
-	-mkdir $@	
-	
+	-mkdir -p $@
+
 $(OUTPUT_DIR): $(BUILD_DIR)
-	-mkdir $@
+	-mkdir -p $@
 
 #######################################
 # clean up
@@ -256,4 +256,4 @@ upload: $(BUILD_DIR)/$(TARGET).bin
 #######################################
 -include $(wildcard $(BUILD_DIR)/*.d)
 
-# *** EOF ***
\ No newline at end of file
+# *** EOF ***