diff --git a/Makefile b/Makefile
index 8127eb6..5ab6f04 100644
--- a/Makefile
+++ b/Makefile
@@ -51,7 +51,7 @@ OBJSL		= stm32_sine.o hwinit.o stm32scheduler.o params.o terminal.o terminal_prj
            my_string.o digio.o sine_core.o my_fp.o fu.o inc_encoder.o printf.o anain.o \
            temp_meas.o param_save.o throttle.o errormessage.o pwmgeneration.o \
            picontroller.o terminalcommands.o vehiclecontrol.o \
-           stm32_can.o canmap.o canhardware.o cansdo.o
+           stm32_can.o canmap.o canhardware.o cansdo.o sdocommands.o
 
 ifeq ($(CONTROL), SINE)
 	OBJSL += pwmgeneration-sine.o
diff --git a/libopeninv b/libopeninv
index 91b26e2..d3d67d2 160000
--- a/libopeninv
+++ b/libopeninv
@@ -1 +1 @@
-Subproject commit 91b26e2c1fa3bae78f3fa8bd88238d7e698d6bd5
+Subproject commit d3d67d2643e51ca8dcc36293021a6bbfe89d422a
diff --git a/sinus.cbp b/sinus.cbp
index 94cd215..1669052 100644
--- a/sinus.cbp
+++ b/sinus.cbp
@@ -102,6 +102,7 @@
 		<Unit filename="libopeninv/include/params.h" />
 		<Unit filename="libopeninv/include/picontroller.h" />
 		<Unit filename="libopeninv/include/printf.h" />
+		<Unit filename="libopeninv/include/sdocommands.h" />
 		<Unit filename="libopeninv/include/sine_core.h" />
 		<Unit filename="libopeninv/include/stm32_can.h" />
 		<Unit filename="libopeninv/include/stm32_loader.h" />
@@ -131,6 +132,7 @@
 		<Unit filename="libopeninv/src/printf.cpp">
 			<Option compilerVar="CC" />
 		</Unit>
+		<Unit filename="libopeninv/src/sdocommands.cpp" />
 		<Unit filename="libopeninv/src/sine_core.cpp" />
 		<Unit filename="libopeninv/src/stm32_can.cpp" />
 		<Unit filename="libopeninv/src/stm32scheduler.cpp" />
diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp
index 2c3b2eb..f02c855 100644
--- a/src/stm32_sine.cpp
+++ b/src/stm32_sine.cpp
@@ -46,8 +46,11 @@
 #include "stm32_can.h"
 #include "canmap.h"
 #include "cansdo.h"
+#include "sdocommands.h"
 
 #define PRINT_JSON 0
+#define START_COMMAND_SUBINDEX 4
+#define STOP_COMMAND_SUBINDEX 5
 
 HWREV hwRev; //Hardware variant of board we are running on
 
@@ -220,13 +223,13 @@ static void Ms10Task(void)
       PwmGeneration::SetOpmode(MOD_OFF);
       Throttle::cruiseSpeed = -1;
       TerminalCommands::EnableSaving();
-      canSdo->EnableSaving();
+      SdoCommands::EnableSaving();
    }
    else if (0 == initWait)
    {
       //Disable saving in Run mode
       TerminalCommands::DisableSaving();
-      canSdo->DisableSaving();
+      SdoCommands::DisableSaving();
       PwmGeneration::SetTorquePercent(0);
       Throttle::RampThrottle(0); //Restart ramp
       Encoder::Reset();
@@ -356,6 +359,29 @@ void Param::Change(Param::PARAM_NUM paramNum)
    }
 }
 
+static void ProcessCustomSdoCommands(CanSdo::SdoFrame* sdoFrame)
+{
+   if (sdoFrame->index == SDO_INDEX_COMMANDS && sdoFrame->cmd == SDO_WRITE)
+   {
+      switch (sdoFrame->subIndex)
+      {
+      case START_COMMAND_SUBINDEX:
+         if (sdoFrame->data < MOD_LAST)
+         {
+            Param::SetInt(Param::opmode, sdoFrame->data);
+            sdoFrame->cmd = SDO_WRITE_REPLY;
+            sdoFrame->data = 0;
+         }
+         break;
+      case STOP_COMMAND_SUBINDEX:
+         Param::SetInt(Param::opmode, 0);
+         sdoFrame->cmd = SDO_WRITE_REPLY;
+         sdoFrame->data = 0;
+         break;
+      }
+   }
+}
+
 static void UpgradeParameters()
 {
    Param::SetInt(Param::version, 4); //backward compatibility
@@ -441,11 +467,22 @@ extern "C" int main(void)
    while(1)
    {
       char c = 0;
+      CanSdo::SdoFrame* sdoFrame = sdo.GetPendingUserspaceSdo();
       t.Run();
+
       if (canSdo->GetPrintRequest() == PRINT_JSON)
       {
          TerminalCommands::PrintParamsJson(canSdo, &c);
       }
+      if (0 != sdoFrame)
+      {
+         SdoCommands::ProcessStandardCommands(sdoFrame);
+
+         if (sdoFrame->cmd == SDO_ABORT)
+            ProcessCustomSdoCommands(sdoFrame);
+
+         sdo.SendSdoReply(sdoFrame);
+      }
    }
 
    return 0;