V2.0: support OVMS tuning macro commands & profile management

README.md

@@ -4,9 +4,11 @@ This is a minimalistic SEVCON Gen4 configuration shell for Arduino.
 
 It's a port of my SEVCON core functionality from the [OVMS project](https://github.com/openvehicles/Open-Vehicle-Monitoring-System), with a simple command interface on the Arduino serial port.
 
-It currently only supports reading & writing SDO registers and entering/leaving pre-operational mode. Support for the OVMS/Twizy tuning macro commands (i.e. `power`, `speed`, `recup` etc.) has not yet been ported.
+**V1** supported only reading & writing SDO registers and entering/leaving pre-operational mode. This is now supported both by the V1 shortcut commands as well as the original OVMS command syntax.
 
-So currently you need to know the registers and value scalings for your tuning needs. Read the SEVCON Gen4 manual for some basic description of registers. The full register set is documented in the SEVCON master dictionary, which is ©SEVCON. It's contained in the SEVCON DVT package.
+**V2** now also supports the OVMS/Twizy tuning macro commands (i.e. `power`, `speed`, `recup` etc.) as well as profile management including saving to / loading from the Arduino EEPROM. Assuming this will be used by hardware hackers, the `brakelight` command has been included as well. Still missing from the OVMS command set is the `clear` command, this is planned to get included along with log access and output. Also missing are all dynamic adjustment functions, i.e. auto drive/recuperation power level following and kickdown.
+
+Read the OVMS user manual and command overview for details on all commands. You may also like to read the SEVCON Gen4 manual for some basic description of registers. The full register set is documented in the SEVCON master dictionary, which is ©SEVCON. It's contained in the SEVCON DVT package.
 
 Most registers of interest for normal tuning can be found in the [Twizy SDO list](extras/Twizy-SDO-List.ods).
 
@@ -36,6 +38,9 @@ Connect to the OBD2 port, switch on the Twizy, start the sketch & open the seria
 
 The Arduino will display a help screen, then wait for your commands:
 
+
+### Low level commands
+
 | Function | Command |
 | --- | --- |
 | Show help | `?` / `help` |
@@ -46,13 +51,49 @@ The Arduino will display a help screen, then wait for your commands:
 | Write register | `w <id> <sub> <val>` |
 | Write-only register | `wo <id> <sub> <val>` |
 
+  - Standard OVMS command syntax (i.e. `pre`, `read` etc.) is also accepted
   - `<id>` & `<sub>` define the SDO register to access, need to be given as hexadecimal numbers
   - `<val>` is the value to write, needs to be given as an unsigned decimal number (negative = two's complement, see below)
 
-**Examples**:
+
+### Macro commands
+
+| Function | Command |
+| --- | --- |
+| Set profile from base64 | `set <prf> <b64>` |
+| Reset profile | `reset <prf>` |
+| Get profile base64 | `get <prf>` |
+| Show main profile values | `info` |
+| Save config to profile | `save <prf>` |
+| Load config from profile | `load <prf>` |
+| Set drive level | `drive <prc>` |
+| Set recuperation levels neutral & brake | `recup <ntr> <brk>` |
+| Set ramp levels | `ramps <st> <ac> <dc> <nt> <br>` |
+| Set ramp limits | `rampl <ac> <dc>` |
+| Set smoothing | `smooth <prc>` |
+| Set max & warn speed | `speed <max> <warn>` |
+| Set torque, power & current levels | `power <trq> <pw1> <pw2> <cur>` |
+| Set torque speed maps | `tsmap <DNB> <p1@s1> <p2@s2> <p3@s3> <p4@s4>` |
+| Set brakelight accel levels | `brakelight <on> <off>` |
+
+  - See [OVMS user manual](https://github.com/openvehicles/Open-Vehicle-Monitoring-System/raw/master/docs/Renault-Twizy/OVMS-UserGuide-RenaultTwizy.pdf)
+  - See [OVMS command overview](https://github.com/openvehicles/Open-Vehicle-Monitoring-System/raw/master/docs/Renault-Twizy/Twizy-Command-Overview.pdf)
+  - See [Twizy profile editor](https://dexters-web.de/cfgedit)
+  - See [Twizy profile converter](https://dexters-web.de/cfgconv)
+
+
+### Examples
 
   - Get firmware version: `rs 100a 00` (if it's `0712.0003` or higher, the Twizy is locked)
-  - Set brake recuperation to 30%: `w 2920 04 300`
+  - Set vmax to 100 kph: `speed 100`
+  - Set torque to 130% and power to 120%: `power 130 120`
+  - Set neutral recuperation to 20% and brake recuperation to 30%: `recup 20 30`
+  - Set and apply a base64 profile: `set 0 3m9wg295ABozAAAAAAAAAAAuOkVbZVNFNRUrRVtlNSMbJGUAAAABAABlZQAAAAAA` (Twizy needs to be on, not in `GO`)
+  - Save current profile to EEPROM slot 1: `save 1`
+  - Reset SEVCON to default configuration: `reset`
+
+
+### Notes
 
 The shell automatically logs into the SEVCON with access level 4 (highest user level, 5 is reserved for SEVCON engineering), so all user SDOs can be written to. See SEVCON master dictionary for access levels. **Note**: all logins are logged in the SEVCON event logs. See SEVCON manual on how to access the logs.
 

TwizyCfg/CANopen.ino

@@ -17,6 +17,9 @@
  */
 
 #include "CANopen.h"
+#include "Tuning.h"
+#include "utils.h"
+
 
 // SDO TX/RX buffer:
 sdo_buffer twizy_sdo;
@@ -248,6 +251,15 @@ UINT writesdo(UINT index, UINT8 subindex, UINT32 data)
 {
   UINT8 control;
 
+#if TWIZY_DEBUG >= 1
+  Serial.print(F("W "));
+  Serial.print(index, HEX);
+  Serial.print(F(" "));
+  Serial.print(subindex, HEX);
+  Serial.print(F(" "));
+  Serial.println(data);
+#endif
+
   // request download:
   twizy_sdo.control = SDO_InitDownloadRequest | SDO_Expedited; // no size needed, server is smart
   twizy_sdo.index = index;
@@ -281,6 +293,21 @@ UINT login(bool on)
 {
   UINT err;
 
+  // get SEVCON type (Twizy 80/45):
+  if (err = readsdo(0x1018,0x02))
+    return ERR_UnknownHardware + err;
+
+  if (twizy_sdo.data == 0x0712302d)
+    twizy_cfg.type = 0; // Twizy80
+  else if (twizy_sdo.data == 0x0712301b)
+    twizy_cfg.type = 1; // Twizy45
+  else {
+    Serial.print(F("ERROR: Unknown controller type: 0x"));
+    Serial.println(twizy_sdo.data, HEX);
+    Serial.println(F("(Twizy80 = 0x0712302d, Twizy45 = 0x0712301b)"));
+    return ERR_UnknownHardware; // unknown controller type
+  }
+  
   // check login level:
   if (err = readsdo(0x5000,1))
     return ERR_LoginFailed + err;
@@ -296,6 +323,9 @@ UINT login(bool on)
       return ERR_LoginFailed + err;
     if (twizy_sdo.data != 4)
       return ERR_LoginFailed;
+
+    Serial.print(F("Logged into SEVCON, car type: Twizy"));
+    Serial.println((twizy_cfg.type==0) ? 80 : 45);
   }
 
   else if (!on && twizy_sdo.data != 0) {

TwizyCfg/Tuning.h

@@ -0,0 +1,99 @@
+/**
+ * ==========================================================================
+ * Twizy/SEVCON configuration shell
+ * ==========================================================================
+ * 
+ * Twizy/SEVCON tuning functions
+ * 
+ * Based on the OVMS Twizy firmware:
+ * https://github.com/openvehicles/Open-Vehicle-Monitoring-System
+ * 
+ * Author: Michael Balzer <dexter@dexters-web.de>
+ * 
+ * License:
+ *  This is free software under GNU Lesser General Public License (LGPL)
+ *  https://www.gnu.org/licenses/lgpl.html
+ *  
+ */
+
+#ifndef _Tuning_h
+#define _Tuning_h
+
+#define OVMS_TWIZY_CFG_BRAKELIGHT 1
+
+union cfg_status {
+  
+  UINT8 drivemode;
+  
+  struct {
+    unsigned type:1;                  // CFG: 0=Twizy80, 1=Twizy45
+    unsigned profile_user:2;          // CFG: user selected profile: 0=Default, 1..3=Custom
+    unsigned profile_cfgmode:2;       // CFG: profile, cfgmode params were last loaded from
+    unsigned unsaved:1;               // CFG: RAM profile changed & not yet saved to EEPROM
+    unsigned keystate:1;              // CFG: key state change detection
+    unsigned applied:1;               // CFG: applyprofile success flag
+  };
+  
+};
+
+extern cfg_status twizy_cfg;
+
+
+// SEVCON macro configuration profile:
+// 1 kept in RAM (working set)
+// 3 stored in binary EEPROM param slots PARAM_PROFILE1 /2 /3
+
+struct cfg_profile {
+  UINT8       checksum;
+  UINT8       speed, warn;
+  UINT8       torque, power_low, power_high;
+  UINT8       drive, neutral, brake;
+  struct tsmap {
+    UINT8       spd1, spd2, spd3, spd4;
+    UINT8       prc1, prc2, prc3, prc4;
+  }           tsmap[3]; // 0=D 1=N 2=B
+  UINT8       ramp_start, ramp_accel, ramp_decel, ramp_neutral, ramp_brake;
+  UINT8       smooth;
+  UINT8       brakelight_on, brakelight_off;
+  // V3.2.1 additions:
+  UINT8       ramplimit_accel, ramplimit_decel;
+  // V3.4.0 additions:
+  UINT8       autorecup_minprc;
+  // V3.6.0 additions:
+  UINT8       autorecup_ref;
+  UINT8       autodrive_minprc;
+  UINT8       autodrive_ref;
+  // V3.7.0 additions:
+  UINT8       current;
+};
+
+extern cfg_profile twizy_cfg_profile;
+
+// EEPROM memory usage info:
+// sizeof twizy_cfg_profile = 24 + 8x3 = 48 byte
+// Maximum size = 2 x PARAM_MAX_LENGTH = 64 byte
+
+// Macros for converting profile values:
+// shift values by 1 (-1.. => 0..) to map param range to UINT8
+#define cfgparam(NAME)  (((int)(twizy_cfg_profile.NAME))-1)
+#define cfgvalue(VAL)   ((UINT8)((VAL)+1))
+
+
+extern unsigned int twizy_max_rpm;         // CFG: max speed (RPM: 0..11000)
+extern unsigned long twizy_max_trq;        // CFG: max torque (mNm: 0..70125)
+extern unsigned int twizy_max_pwr_lo;      // CFG: max power low speed (W: 0..17000)
+extern unsigned int twizy_max_pwr_hi;      // CFG: max power high speed (W: 0..17000)
+
+extern UINT8 twizy_autorecup_checkpoint;   // change detection for autorecup function
+extern UINT twizy_autorecup_level;         // autorecup: current recup level (per mille)
+
+extern UINT8 twizy_autodrive_checkpoint;   // change detection for autopower function
+extern UINT twizy_autodrive_level;         // autopower: current drive level (per mille)
+
+
+// EEPROM addresses:
+#define PARAM_PROFILE               0x0000 // current cfg profile nr (INT8 0..3)
+#define PARAM_PROFILE_S             0x0040 // custom profiles base
+
+#endif
+

TwizyCfg/TwizyCfg.ino

@@ -16,12 +16,16 @@
  *  https://www.gnu.org/licenses/lgpl.html
  *  
  */
-#define TWIZY_CFG_VERSION "V1.0 (2017-06-13)"
+#define TWIZY_CFG_VERSION "V2.0 (2017-06-25)"
+
+#include <EEPROM.h>
 
 #include <mcp_can.h>
 #include <mcp_can_dfs.h>
+
 #include "utils.h"
 #include "CANopen.h"
+#include "Tuning.h"
 #include "TwizyCfg_config.h"
 
 
@@ -37,7 +41,68 @@ char net_msg_scratchpad[200];
 // COMMAND DISPATCHER:
 //
 
-bool exec(char *cmd)
+enum cfg_command_id {
+  cmdNone = 0,
+  cmdHelp,
+  cmdRead, cmdReadString, cmdWrite, cmdWriteOnly,
+  cmdPreOp, cmdOp,
+  cmdSet, cmdReset, cmdGet, cmdInfo, cmdSave, cmdLoad,
+  cmdDrive, cmdRecup, cmdRamps, cmdRampLimits, cmdSmooth,
+  cmdSpeed, cmdPower, cmdTSMap, cmdBrakelight
+};
+
+enum cfg_command_mode {
+  modeOffline, modeLogin, modePreOp
+};
+
+struct cfg_command {
+  char                  cmd[11];
+  cfg_command_id        id;
+  cfg_command_mode      mode;
+};
+
+const cfg_command command_table[] PROGMEM = {
+  
+  { "?", cmdHelp, modeOffline },
+  { "HELP", cmdHelp, modeOffline },
+  
+  { "READ", cmdRead, modeLogin },
+  { "R", cmdRead, modeLogin },
+  { "READS", cmdReadString, modeLogin },
+  { "RS", cmdReadString, modeLogin },
+  { "WRITE", cmdWrite, modeLogin },
+  { "W", cmdWrite, modeLogin },
+  { "WRITEO", cmdWriteOnly, modeLogin },
+  { "WO", cmdWriteOnly, modeLogin },
+  { "PRE", cmdPreOp, modeLogin },
+  { "P", cmdPreOp, modeLogin },
+  { "OP", cmdOp, modeLogin },
+  { "O", cmdOp, modeLogin },
+  
+  { "SET", cmdSet, modeOffline },
+  { "RESET", cmdReset, modeOffline },
+  { "GET", cmdGet, modeOffline },
+  { "INFO", cmdInfo, modeOffline },
+  { "SAVE", cmdSave, modeOffline },
+  { "LOAD", cmdLoad, modeLogin },
+  
+  { "DRIVE", cmdDrive, modeLogin },
+  { "RECUP", cmdRecup, modeLogin },
+  { "RAMPS", cmdRamps, modeLogin },
+  { "RAMPL", cmdRampLimits, modeLogin },
+  { "SMOOTH", cmdSmooth, modeLogin },
+  
+  { "SPEED", cmdSpeed, modePreOp },
+  { "POWER", cmdPower, modePreOp },
+  { "TSMAP", cmdTSMap, modePreOp },
+  { "BRAKELIGHT", cmdBrakelight, modePreOp },
+  
+};
+
+#define COMMAND_COUNT (sizeof(command_table)/sizeof(cfg_command))
+
+
+bool exec(char *cmdline)
 {
   UINT err;
   char *s;
@@ -51,14 +116,32 @@ bool exec(char *cmd)
   char maps[4] = {'D','N','B',0};
   UINT8 i;
 
-  arguments = net_sms_initargs(cmd);
+  arguments = net_sms_initargs(cmdline);
 
   // convert cmd to upper-case:
-  for (s=cmd; ((*s!=0)&&(*s!=' ')); s++)
+  for (s=cmdline; ((*s!=0)&&(*s!=' ')); s++)
     if ((*s > 0x60) && (*s < 0x7b)) *s=*s-0x20;
 
   
-  if (*cmd == '?' || starts_with(cmd, "HELP")) {
+  //
+  // Identify command:
+  //
+  
+  cfg_command cmd;
+  
+  for (i = 0; i < COMMAND_COUNT; i++ ) {
+    memcpy_P(&cmd, &command_table[i], sizeof(cfg_command));
+    if (strcmp(cmdline, cmd.cmd) == 0) {
+      break;
+    }
+  }
+  
+  if (i == COMMAND_COUNT) {
+    cmd.id = cmdNone;
+    cmd.mode = modeOffline;
+  }
+
+  if (cmd.id == cmdHelp) {
     Serial.print(F("\n"
       "Twizy-Cfg " TWIZY_CFG_VERSION "\n"
       "\n"
@@ -70,59 +153,90 @@ bool exec(char *cmd)
       " wo <id> <sub> <val>      -- write-only SDO register (numerical)\n"
       " p                        -- preop mode\n"
       " o                        -- op mode\n"
+      "(Hint: standard OVMS syntax also accepted)\n"
       "\n"
+      " set <prf> <b64>          -- set profile from base64\n"
+      " reset <prf>              -- reset profile\n"
+      " get <prf>                -- get profile base64\n"
+      " info                     -- show main profile values\n"
+      " save <prf>               -- save config to profile\n"
+      " load <prf>               -- load config from profile\n"
+      "\n"
+      " drive <prc>              -- set drive level\n"
+      " recup <ntr> <brk>        -- set recuperation levels neutral & brake\n"
+      " ramps <st> <ac> <dc> <nt> <br> -- set ramp levels\n"
+      " rampl <ac> <dc>          -- set ramp limits\n"
+      " smooth <prc>             -- set smoothing\n"
+      "\n"
+      " speed <max> <warn>       -- set max & warn speed\n"
+      " power <trq> <pw1> <pw2> <cur> -- set torque, power & current levels\n"
+      " tsmap <DNB> <pt1..4>     -- set torque speed maps\n"
+      " brakelight <on> <off>    -- set brakelight accel levels\n"
+      "\n"
+      "See OVMS manual & command overview for details.\n"
       "Note: <id> and <sub> are hexadecimal, <val> are decimal\n"
       "Examples:\n"
       " rs 1008 0                -- read SEVCON firmware name\n"
       " w 2920 3 325             -- set neutral recup level to 32.5%\n"
       "\n"
       ));
-    
     return false;
   }
 
-  else {
 
-    // common reply intro:
+  //
-    s = stp_ram(net_scratchpad, cmd);
+  // Prepare command:
-    s = stp_rom(s, ": ");
+  //
 
-    //
+  err = 0;
-    // COMMAND DISPATCHER:
-    //  Part 2: online commands (SEVCON access necessary)
-    //  - PRE
-    //  - OP
-    //  - READ[S]
-    //  - WRITE[O]
-    //
   
+  // common reply intro:
+  s = stp_ram(net_scratchpad, cmdline);
+  s = stp_rom(s, ": ");
+  
+  if (cmd.mode >= modeLogin) {
     // login:
     if (err = login(1)) {
       s = vehicle_twizy_fmt_err(s, err);
     }
+  }
+  
+  if (cmd.mode >= modePreOp) {
+    // enter config mode:
+    if (err = configmode(1)) {
+      s = vehicle_twizy_fmt_err(s, err);
+    }
+  }
+  
+  //
+  // Execute command:
+  //
+
+  if (!err) switch (cmd.id) {
 
 
-    else if (starts_with(cmd, "P")) {
+    case cmdPreOp:
-      // P: enter config mode
+      // enter config mode
       if (err = configmode(1))
         s = vehicle_twizy_fmt_err(s, err);
       else
         s = stp_rom(s, "OK");
       go_op_onexit = false;
-    }
+      break;
 
 
-    else if (starts_with(cmd, "O")) {
+    case cmdOp:
-      // O: leave config mode
+      // leave config mode
       if (err = configmode(0))
         s = vehicle_twizy_fmt_err(s, err);
       else
         s = stp_rom(s, "OK");
       go_op_onexit = false;
-    }
+      break;
 
 
-    else if (starts_with(cmd, "R")) {
+    case cmdRead:
+    case cmdReadString:
       // R index_hex subindex_hex
       // RS index_hex subindex_hex
       if (arguments = net_sms_nextarg(arguments))
@@ -134,7 +248,7 @@ bool exec(char *cmd)
         s = stp_rom(s, "ERROR: Too few args");
       }
       else {
-        if (cmd[1] != 'S') {
+        if (cmd.id == cmdRead) {
           // READ:
           if (err = readsdo(arg[0], arg[1])) {
             s = vehicle_twizy_fmt_err(s, err);
@@ -159,10 +273,11 @@ bool exec(char *cmd)
       }
 
       go_op_onexit = false;
-    }
+      break;
 
 
-    else if (starts_with(cmd, "W")) {
+    case cmdWrite:
+    case cmdWriteOnly:
       // W index_hex subindex_hex data_dec
       // WO index_hex subindex_hex data_dec
       if (arguments = net_sms_nextarg(arguments))
@@ -177,7 +292,7 @@ bool exec(char *cmd)
       }
       else {
 
-        if (cmd[1] == 'O') {
+        if (cmd.id == cmdWriteOnly) {
           // WRITEONLY:
 
           // write new value:
@@ -213,25 +328,447 @@ bool exec(char *cmd)
       }
 
       go_op_onexit = false;
-    }
+      break;
 
 
-    else {
+    case cmdSet:
+    case cmdReset:
+      // SET [nr] [base64data]: set complete profile
+      // RESET [nr]: reset profile (= SET without data)
+      //   nr 1..3 = update EEPROM directly (no SEVCON access)
+      //   else update working set & try to apply
+      
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+      
+      // we're a little bit out of RAM...
+      // max cmd length is "CFG SET n " + 88 b64 chars + \0 = 99 chars.
+      // use upper 90 bytes of net_scratchpad as the b64 codec buffer:
+      t = net_scratchpad + 110;
+      memset(t, 0, 90);
+      *t = 1; // checksum for cleared/reset profile
+      
+      i = 0;
+      if (arguments = net_sms_nextarg(arguments))
+        i = base64decode(arguments, (byte*)t);
+      
+      if (t[0] != vehicle_twizy_cfg_calc_checksum((BYTE *)t)) {
+        s = stp_rom(s, "ERROR: wrong checksum");
+      }
+      else if (arg[0] >= 1 && arg[0] <= 3) {
+        // update EEPROM slot:
+        //par_setbin(PARAM_PROFILE_S + ((arg[0]-1)<<1), t, 64);
+        EEPROM.put(arg[0] * 64, *((cfg_profile*)t));
+        // signal "unsaved" if WS now differs from stored profile:
+        if (arg[0] == twizy_cfg.profile_user)
+          twizy_cfg.unsaved = 1;
+        s = stp_i(s, "OK #", arg[0]);
+      }
+      else {
+        // update working set:
+        memcpy((void *)&twizy_cfg_profile, (void *)t, sizeof(twizy_cfg_profile));
+        // signal "unsaved" if profile modified or custom profile active:
+        twizy_cfg.unsaved = (i > 0) || (twizy_cfg.profile_user > 0);
+        // apply changed working set:
+        err = vehicle_twizy_cfg_applyprofile(twizy_cfg.profile_user);
+        s = vehicle_twizy_fmt_switchprofileresult(s, -1, err);
+      }
+      break;
+
+
+    case cmdGet:
+      // GET [nr]: get complete profile (base64 encoded)
+      //  nr 1..3 = directly from EEPROM
+      //  else working set
+  
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+      
+      if (arg[0] >= 1 && arg[0] <= 3) {
+        // read from EEPROM:
+        // we're a little bit out of RAM...
+        // max response length is "CFG GET: #n= " + 88 b64 chars + \0 = 102 chars.
+        // use upper 90 bytes of net_scratchpad as the b64 codec buffer:
+        t = net_scratchpad + 110;
+        //par_getbin(PARAM_PROFILE_S + ((arg[0]-1)<<1), t, sizeof(twizy_cfg_profile));
+        EEPROM.get(arg[0] * 64, *((cfg_profile*)t));
+        s = stp_i(s, "#", arg[0]);
+      }
+      else {
+        // read from working set:
+        twizy_cfg_profile.checksum = vehicle_twizy_cfg_calc_checksum((BYTE *)&twizy_cfg_profile);
+        t = (char *) &twizy_cfg_profile;
+        s = stp_rom(s, "WS");
+      }
+      
+      s = stp_rom(s, "= ");
+      s = base64encode((byte *)t, sizeof(twizy_cfg_profile), s);
+      break;
+
+
+    case cmdInfo:
+      // INFO: output main params
+  
+      s = stp_i(s, "#", twizy_cfg.profile_user);
+      if (twizy_cfg.unsaved)
+        s = stp_rom(s, "/WS");
+  
+      s = stp_i(s, " SPEED ", cfgparam(speed));
+      s = stp_i(s, " ", cfgparam(warn));
+  
+      s = stp_i(s, " POWER ", cfgparam(torque));
+      s = stp_i(s, " ", cfgparam(power_low));
+      s = stp_i(s, " ", cfgparam(power_high));
+      s = stp_i(s, " ", cfgparam(current));
+  
+      s = stp_i(s, " DRIVE ", cfgparam(drive));
+      s = stp_i(s, " ", cfgparam(autodrive_ref));
+      s = stp_i(s, " ", cfgparam(autodrive_minprc));
+      
+      s = stp_i(s, " RECUP ", cfgparam(neutral));
+      s = stp_i(s, " ", cfgparam(brake));
+      s = stp_i(s, " ", cfgparam(autorecup_ref));
+      s = stp_i(s, " ", cfgparam(autorecup_minprc));
+  
+      s = stp_i(s, " RAMPS ", cfgparam(ramp_start));
+      s = stp_i(s, " ", cfgparam(ramp_accel));
+      s = stp_i(s, " ", cfgparam(ramp_decel));
+      s = stp_i(s, " ", cfgparam(ramp_neutral));
+      s = stp_i(s, " ", cfgparam(ramp_brake));
+  
+      s = stp_i(s, " SMOOTH ", cfgparam(smooth));
+      break;
+
+
+    case cmdSave:
+      // SAVE [nr]: save profile to EEPROM
+  
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+      else
+        arg[0] = twizy_cfg.profile_user; // save as current profile
+  
+      if (vehicle_twizy_cfg_writeprofile(arg[0]) == FALSE) {
+        s = stp_i(s, "ERROR: wrong key #", arg[0]);
+      }
+      else {
+        s = stp_i(s, "OK saved as #", arg[0]);
+  
+        // make destination new current:
+        //par_set(PARAM_PROFILE, s-1);
+        i = arg[0];
+        EEPROM.put(PARAM_PROFILE, i);
+        twizy_cfg.profile_user = arg[0];
+        twizy_cfg.profile_cfgmode = arg[0];
+        twizy_cfg.unsaved = 0;
+      }
+      break;
+
+
+    case cmdLoad:
+      // LOAD [nr]: load/restore profile from EEPROM
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+      else
+        arg[0] = twizy_cfg.profile_user; // restore current profile
+
+      err = vehicle_twizy_cfg_switchprofile(arg[0]);
+      s = vehicle_twizy_fmt_switchprofileresult(s, arg[0], err);
+      break;
+
+
+    case cmdDrive:
+      // DRIVE [max_prc] [autopower_ref] [autopower_minprc]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+
+      // autopower drive 100% reference & min prc:
+      if (arguments = net_sms_nextarg(arguments))
+        arg[1] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[2] = atoi(arguments);
+
+      if (err = vehicle_twizy_cfg_drive(arg[0], arg[1], arg[2])) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.drive = cfgvalue(arg[0]);
+        twizy_cfg_profile.autodrive_ref = cfgvalue(arg[1]);
+        twizy_cfg_profile.autodrive_minprc = cfgvalue(arg[2]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK");
+      }
+      break;
+
+
+    case cmdRecup:
+      // RECUP [neutral_prc] [brake_prc] [autopower_ref] [autopower_minprc]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[1] = atoi(arguments);
+      else
+        arg[1] = arg[0];
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[2] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[3] = atoi(arguments);
+
+      if (err = vehicle_twizy_cfg_recup(arg[0], arg[1], arg[2], arg[3])) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.neutral = cfgvalue(arg[0]);
+        twizy_cfg_profile.brake = cfgvalue(arg[1]);
+        twizy_cfg_profile.autorecup_ref = cfgvalue(arg[2]);
+        twizy_cfg_profile.autorecup_minprc = cfgvalue(arg[3]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK");
+      }
+      break;
+
+
+    case cmdRamps:
+      // RAMPS [start_prc] [accel_prc] [decel_prc] [neutral_prc] [brake_prc]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[1] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[2] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[3] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[4] = atoi(arguments);
+
+      if (err = vehicle_twizy_cfg_ramps(arg[0], arg[1], arg[2], arg[3], arg[4])) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.ramp_start = cfgvalue(arg[0]);
+        twizy_cfg_profile.ramp_accel = cfgvalue(arg[1]);
+        twizy_cfg_profile.ramp_decel = cfgvalue(arg[2]);
+        twizy_cfg_profile.ramp_neutral = cfgvalue(arg[3]);
+        twizy_cfg_profile.ramp_brake = cfgvalue(arg[4]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK");
+      }
+      break;
+
+
+    case cmdRampLimits:
+      // RAMPL [accel_prc] [decel_prc]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[1] = atoi(arguments);
+
+      if (err = vehicle_twizy_cfg_rampl(arg[0], arg[1])) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.ramplimit_accel = cfgvalue(arg[0]);
+        twizy_cfg_profile.ramplimit_decel = cfgvalue(arg[1]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK");
+      }
+      break;
+
+
+    case cmdSmooth:
+      // SMOOTH [prc]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+
+      if (err = vehicle_twizy_cfg_smoothing(arg[0])) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.smooth = cfgvalue(arg[0]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK");
+      }
+      break;
+
+
+    case cmdSpeed:
+      // SPEED [max_kph] [warn_kph]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+      if (arguments = net_sms_nextarg(arguments))
+        arg[1] = atoi(arguments);
+
+      if ((err = vehicle_twizy_cfg_speed(arg[0], arg[1])) == 0)
+        err = vehicle_twizy_cfg_makepowermap();
+
+      if (err) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.speed = cfgvalue(arg[0]);
+        twizy_cfg_profile.warn = cfgvalue(arg[1]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK, power cycle to activate!");
+      }
+      break;
+
+
+    case cmdPower:
+      // POWER [trq_prc] [pwr_lo_prc] [pwr_hi_prc] [curr_prc]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[1] = atoi(arguments);
+      else
+        arg[1] = arg[0];
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[2] = atoi(arguments);
+      else
+        arg[2] = arg[1];
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[3] = atoi(arguments);
+
+      if ((err = vehicle_twizy_cfg_power(arg[0], arg[1], arg[2], arg[3])) == 0)
+        err = vehicle_twizy_cfg_makepowermap();
+
+      if (err) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.torque = cfgvalue(arg[0]);
+        twizy_cfg_profile.power_low = cfgvalue(arg[1]);
+        twizy_cfg_profile.power_high = cfgvalue(arg[2]);
+        twizy_cfg_profile.current = cfgvalue(arg[3]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK, power cycle to activate!");
+      }
+      break;
+
+
+    case cmdTSMap:
+      // TSMAP [maps] [t1_prc[@t1_spd]] [t2_prc[@t2_spd]] [t3_prc[@t3_spd]] [t4_prc[@t4_spd]]
+
+      if (arguments = net_sms_nextarg(arguments)) {
+        for (i=0; i<3 && arguments[i]; i++)
+          maps[i] = arguments[i] & ~0x20;
+        maps[i] = 0;
+      }
+      for (i=0; i<4; i++)
+      {
+        if (arguments = net_sms_nextarg(arguments))
+        {
+          arg[i] = atoi(arguments);
+          if (cmdline = strchr(arguments, '@'))
+            arg2[i] = atoi(cmdline+1);
+        }
+      }
+
+      for (i=0; maps[i]; i++) {
+        if (err = vehicle_twizy_cfg_tsmap(maps[i],
+                    arg[0], arg[1], arg[2], arg[3],
+                    arg2[0], arg2[1], arg2[2], arg2[3]))
+          break;
+
+        // update profile:
+        err = (maps[i]=='D') ? 0 : ((maps[i]=='N') ? 1 : 2);
+        twizy_cfg_profile.tsmap[err].prc1 = cfgvalue(arg[0]);
+        twizy_cfg_profile.tsmap[err].prc2 = cfgvalue(arg[1]);
+        twizy_cfg_profile.tsmap[err].prc3 = cfgvalue(arg[2]);
+        twizy_cfg_profile.tsmap[err].prc4 = cfgvalue(arg[3]);
+        twizy_cfg_profile.tsmap[err].spd1 = cfgvalue(arg2[0]);
+        twizy_cfg_profile.tsmap[err].spd2 = cfgvalue(arg2[1]);
+        twizy_cfg_profile.tsmap[err].spd3 = cfgvalue(arg2[2]);
+        twizy_cfg_profile.tsmap[err].spd4 = cfgvalue(arg2[3]);
+        err = 0;
+      }
+
+      if (err) {
+        s = stp_rom(s, "MAP ");
+        *s++ = maps[i]; *s = 0;
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        twizy_cfg.unsaved = 1;
+        s = stp_rom(s, "OK");
+      }
+      break;
+
+
+    case cmdBrakelight:
+      // BRAKELIGHT [on_lev] [off_lev]
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[0] = atoi(arguments);
+
+      if (arguments = net_sms_nextarg(arguments))
+        arg[1] = atoi(arguments);
+      else
+        arg[1] = arg[0];
+
+      if (err = vehicle_twizy_cfg_brakelight(arg[0], arg[1])) {
+        s = vehicle_twizy_fmt_err(s, err);
+      }
+      else {
+        // update profile:
+        twizy_cfg_profile.brakelight_on = cfgvalue(arg[0]);
+        twizy_cfg_profile.brakelight_off = cfgvalue(arg[1]);
+        twizy_cfg.unsaved = 1;
+
+        // success message:
+        s = stp_rom(s, "OK");
+      }
+      break;
+
+
+    default:
       // unknown command
       s = stp_rom(s, "Unknown command");
-    }
+      break;
-    
-    // go operational?
-    if (go_op_onexit)
-      configmode(0);
-    
   }
 
 
   // 
-  // FINISH: send command response
+  // FINISH:
   //
   
+  // go operational?
+  if (!err && go_op_onexit)
+    configmode(0);
+    
   Serial.println(net_scratchpad);
 
   return true;
@@ -279,6 +816,9 @@ void setup() {
   
   CAN.setMode(MCP_NORMAL);
 
+  // Init tuning module:
+
+  vehicle_twizy_init();
 
   // Output info & prompt:
   exec((char *) "?");

TwizyCfg/TwizyCfg_config.h

@@ -6,6 +6,10 @@
 #ifndef _TwizyCfg_config_h
 #define _TwizyCfg_config_h
 
+// Debug output level:
+//  1 = show every SDO write
+#define TWIZY_DEBUG 0
+
 // Set your MCP clock frequency here:
 #define TWIZY_CAN_MCP_FREQ        MCP_16MHZ
 

TwizyCfg/base64.ino

@@ -0,0 +1,145 @@
+/*********************************************************************
+MODULE NAME:    b64.c
+
+ORIGIN:         http://base64.sourceforge.net/b64.c
+
+AUTHOR:         Bob Trower 08/04/01
+
+PROJECT:        Crypt Data Packaging
+
+COPYRIGHT:      Copyright (c) Trantor Standard Systems Inc., 2001
+
+NOTE:           This source code may be used as you wish, subject to
+                the MIT license.  See the LICENCE section below.
+
+LICENCE:        Copyright (c) 2001 Bob Trower, Trantor Standard Systems Inc.
+
+                Permission is hereby granted, free of charge, to any person
+                obtaining a copy of this software and associated
+                documentation files (the "Software"), to deal in the
+                Software without restriction, including without limitation
+                the rights to use, copy, modify, merge, publish, distribute,
+                sublicense, and/or sell copies of the Software, and to
+                permit persons to whom the Software is furnished to do so,
+                subject to the following conditions:
+
+                The above copyright notice and this permission notice shall
+                be included in all copies or substantial portions of the
+                Software.
+
+                THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY
+                KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+                WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+                PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
+                OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+                OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+                OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+                SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+VERSION HISTORY:
+                Bob Trower 08/04/01 -- Create Version 0.00.00B
+/*********************************************************************/
+
+#include <string.h>
+#include <stdlib.h>
+
+// Translation Table as described in RFC1113
+const unsigned char cb64[] PROGMEM = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+
+// Translation Table to decode (created by author)
+const unsigned char cd64[] PROGMEM = "|$$$}rstuvwxyz{$$$$$$$>?@ABCDEFGHIJKLMNOPQRSTUVW$$$$$$XYZ[\\]^_`abcdefghijklmnopq";
+
+#define CB64(n) pgm_read_byte_near(cb64 + (n))
+#define CD64(n) pgm_read_byte_near(cd64 + (n))
+
+unsigned char in[4], out[4];
+
+void encodeblock( unsigned char in[3], unsigned char out[4], int len )
+  {
+  out[0] = CB64( in[0] >> 2 );
+  out[1] = CB64( ((in[0] & 0x03) << 4) | ((in[1] & 0xf0) >> 4) );
+  out[2] = (unsigned char) (len > 1 ? CB64( ((in[1] & 0x0f) << 2) | ((in[2] & 0xc0) >> 6) ) : '=');
+  out[3] = (unsigned char) (len > 2 ? CB64( in[2] & 0x3f ) : '=');
+  }
+
+char *base64encode(byte *inputData, int inputLen, char *outputData)
+  {
+  int len = 0;
+  int k;
+  for (k=0;k<inputLen;k++)
+    {
+    in[len++] = inputData[k];
+    if (len==3)
+      {
+      // Block is full
+      encodeblock(in, out, 3);
+      for (len=0;len<4;len++) *outputData++ = out[len];
+      len = 0;
+      }
+    }
+  if (len>0)
+    {
+    for (k=len;k<3;k++) in[k]=0;
+    encodeblock(in, out, len);
+    for (len=0;len<4;len++) *outputData++ = out[len];
+    }
+  *outputData = 0;
+  return outputData;
+  }
+
+
+void decodeblock( unsigned char in[4], unsigned char out[3] )
+  {
+  out[ 0 ] = (unsigned char ) (in[0] << 2 | in[1] >> 4);
+  out[ 1 ] = (unsigned char ) (in[1] << 4 | in[2] >> 2);
+  out[ 2 ] = (unsigned char ) (((in[2] << 6) & 0xc0) | in[3]);
+  }
+
+int base64decode(char *inputData, byte *outputData)
+  {
+  BYTE c = 1;
+  unsigned char v;
+  int i, len;
+  int written = 0;
+
+  while( c != 0 )
+    {
+    for( len = 0, i = 0; (i < 4) && (c != 0); i++ )
+      {
+      v = 0;
+      while( (c != 0) && (v == 0) )
+        {
+        c = (*inputData) ? *inputData++ : 0;
+        v = (unsigned char) ((c < 43 || c > 122) ? 0 : CD64( c - 43 ));
+        if( v )
+          {
+          v = (unsigned char) ((v == '$') ? 0 : v - 61);
+          }
+        }
+      if( c != 0 )
+        {
+        len++;
+        if( v )
+          {
+          in[ i ] = (unsigned char) (v - 1);
+          }
+        }
+      else
+        {
+        in[i] = 0;
+        }
+      }
+    if( len )
+      {
+      decodeblock( in, out );
+      for( i = 0; i < len - 1; i++ )
+        {
+        *outputData++ = out[i];
+        written++;
+        }
+      }
+    }
+  *outputData = 0;
+  return written;
+  }
+

TwizyCfg/utils.h

@@ -23,10 +23,16 @@
 
 typedef unsigned char UINT8;
 typedef unsigned int UINT;
+typedef unsigned int WORD;
 typedef unsigned long UINT32;
 
 typedef signed char INT8;
 
+typedef byte BYTE;
+typedef bool BOOL;
+#define TRUE true
+#define FALSE false
+
 
 // Useful macros:
 
@@ -37,6 +43,15 @@ typedef signed char INT8;
 #define LIMIT_MIN(n,lim) ((n) < (lim) ? (lim) : (n))
 #define LIMIT_MAX(n,lim) ((n) > (lim) ? (lim) : (n))
 
+#define delay5(n) delay(5*(n))
+#define delay100(n) delay(100*(n))
+#define CHECKPOINT(n) ;
+
+
+// PROGMEM string helpers:
+#define FLASHSTRING   const __FlashStringHelper
+#define FS(x)         (__FlashStringHelper*)(x)
+
 
 #endif // _utils_h