From 8b51e02f6c58807aaccf5cb0489fc479115ccc31 Mon Sep 17 00:00:00 2001 From: Michael Balzer Date: Sun, 25 Jun 2017 22:10:11 +0200 Subject: [PATCH] V2.0: support OVMS tuning macro commands & profile management --- README.md | 49 +- TwizyCfg/CANopen.ino | 32 +- TwizyCfg/Tuning.h | 99 +++ TwizyCfg/Tuning.ino | 1359 ++++++++++++++++++++++++++++++++++++ TwizyCfg/TwizyCfg.ino | 632 +++++++++++++++-- TwizyCfg/TwizyCfg_config.h | 4 + TwizyCfg/base64.ino | 145 ++++ TwizyCfg/utils.h | 15 + 8 files changed, 2284 insertions(+), 51 deletions(-) create mode 100644 TwizyCfg/Tuning.h create mode 100644 TwizyCfg/Tuning.ino create mode 100644 TwizyCfg/base64.ino diff --git a/README.md b/README.md index 69464ea..5360a38 100644 --- a/README.md +++ b/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 ` | | Write-only register | `wo ` | + - Standard OVMS command syntax (i.e. `pre`, `read` etc.) is also accepted - `` & `` define the SDO register to access, need to be given as hexadecimal numbers - `` 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 ` | +| Reset profile | `reset ` | +| Get profile base64 | `get ` | +| Show main profile values | `info` | +| Save config to profile | `save ` | +| Load config from profile | `load ` | +| Set drive level | `drive ` | +| Set recuperation levels neutral & brake | `recup ` | +| Set ramp levels | `ramps
` | +| Set ramp limits | `rampl ` | +| Set smoothing | `smooth ` | +| Set max & warn speed | `speed ` | +| Set torque, power & current levels | `power ` | +| Set torque speed maps | `tsmap ` | +| Set brakelight accel levels | `brakelight ` | + + - 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. diff --git a/TwizyCfg/CANopen.ino b/TwizyCfg/CANopen.ino index b92b9d0..739d185 100644 --- a/TwizyCfg/CANopen.ino +++ b/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) { @@ -364,7 +394,7 @@ UINT configmode(bool on) } } } - + return 0; } diff --git a/TwizyCfg/Tuning.h b/TwizyCfg/Tuning.h new file mode 100644 index 0000000..4f88bb0 --- /dev/null +++ b/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 + * + * 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 + diff --git a/TwizyCfg/Tuning.ino b/TwizyCfg/Tuning.ino new file mode 100644 index 0000000..c69b966 --- /dev/null +++ b/TwizyCfg/Tuning.ino @@ -0,0 +1,1359 @@ +/** + * ========================================================================== + * 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 + * + * License: + * This is free software under GNU Lesser General Public License (LGPL) + * https://www.gnu.org/licenses/lgpl.html + * + */ + +#include + +#include "utils.h" +#include "CANopen.h" +#include "Tuning.h" + + +// SEVCON configuration status: + +cfg_status twizy_cfg; + + +// SEVCON macro configuration profile: + +cfg_profile twizy_cfg_profile; + + +unsigned int twizy_max_rpm; // CFG: max speed (RPM: 0..11000) +unsigned long twizy_max_trq; // CFG: max torque (mNm: 0..70125) +unsigned int twizy_max_pwr_lo; // CFG: max power low speed (W: 0..17000) +unsigned int twizy_max_pwr_hi; // CFG: max power high speed (W: 0..17000) + +UINT8 twizy_autorecup_checkpoint; // change detection for autorecup function +UINT twizy_autorecup_level; // autorecup: current recup level (per mille) + +UINT8 twizy_autodrive_checkpoint; // change detection for autopower function +UINT twizy_autodrive_level; // autopower: current drive level (per mille) + + +/*********************************************************************** + * COMMAND CLASS: SEVCON CONTROLLER CONFIGURATION + * + * MSG: ...todo... + * SMS: CFG [cmd] + * + */ + +struct twizy_cfg_params { + UINT8 DefaultKphMax; + UINT DefaultRpmMax; + UINT DefaultRpmRev; + UINT DeltaBrkStart; + UINT DeltaBrkEnd; + UINT DeltaBrkDown; + + UINT8 DefaultKphWarn; + UINT DefaultRpmWarn; + UINT DeltaWarnOff; + + UINT DefaultTrq; + UINT DefaultTrqRated; + UINT32 DefaultTrqLim; + UINT DeltaMapTrq; + + UINT32 DefaultCurrLim; + UINT DefaultCurrStatorMax; + UINT BoostCurr; + UINT DefaultFMAP[4]; // only upper 2 points needed + UINT ExtendedFMAP[4]; // only upper 2 points needed + + UINT DefaultPwrLo; + UINT DefaultPwrLoLim; + UINT DefaultPwrHi; + UINT DefaultPwrHiLim; + UINT DefaultMaxMotorPwr; + + UINT8 DefaultRecup; + UINT8 DefaultRecupPrc; + + UINT DefaultRampStart; + UINT8 DefaultRampStartPrm; + UINT DefaultRampAccel; + UINT8 DefaultRampAccelPrc; + + UINT DefaultPMAP[18]; + + UINT DefaultMapSpd[4]; +}; + +struct twizy_cfg_params twizy_cfg_params[2] = +{ + { + // + // CFG[0] = TWIZY80: + // + // 55 Nm (0x4611.0x01) = default max power map (PMAP) torque + // 55 Nm (0x6076.0x00) = default peak torque + // 57 Nm (0x2916.0x01) = rated torque (??? should be equal to 0x6076...) + // 70.125 Nm (0x4610.0x11) = max motor torque according to flux map + // + // 7250 rpm (0x2920.0x05) = default max fwd speed = ~80 kph + // 8050 rpm = default overspeed warning trigger (STOP lamp ON) = ~89 kph + // 8500 rpm = default overspeed brakedown trigger = ~94 kph + // 10000 rpm = max neutral speed (0x3813.2d) = ~110 kph + // 11000 rpm = severe overspeed fault (0x4624.00) = ~121 kph + + 80, // DefaultKphMax + 7250, // DefaultRpmMax + 900, // DefaultRpmMaxRev + 400, // DeltaBrkStart + 800, // DeltaBrkEnd + 1250, // DeltaBrkDown + + 89, // DefaultKphWarn + 8050, // DefaultRpmWarn + 550, // DeltaWarnOff + + 55000, // DefaultTrq + 57000, // DefaultTrqRated + 70125, // DefaultTrqLim + 0, // DeltaMapTrq + + 450000, // DefaultCurrLim + 450, // DefaultCurrStatorMax + 540, // BoostCurr + { 964, 9728, 1122, 9984 }, // DefaultFMAP + { 1122, 10089, 2240, 11901 }, // ExtendedFMAP + + 12182, // DefaultPwrLo + 17000, // DefaultPwrLoLim + 13000, // DefaultPwrHi + 17000, // DefaultPwrHiLim + 4608, // DefaultMaxMotorPwr [1/256 kW] + + 182, // DefaultRecup + 18, // DefaultRecupPrc + + 400, // DefaultRampStart + 40, // DefaultRampStartPrm + 2500, // DefaultRampAccel + 25, // DefaultRampAccelPrc + + // DefaultPMAP: + { 880,0, 880,2115, 659,2700, 608,3000, 516,3500, + 421,4500, 360,5500, 307,6500, 273,7250 }, + + // DefaultMapSpd: + { 3000, 3500, 4500, 6000 } + }, + { + // + // CFG[1] = TWIZY45: + // + // 32.5 Nm (0x4611.0x01) = default max power map (PMAP) torque + // 33 Nm (0x6076.0x00) = default peak torque + // 33 Nm (0x2916.0x01) = rated torque (??? should be equal to 0x6076...) + // 36 Nm (0x4610.0x11) = max motor torque according to flux map + // + // 5814 rpm (0x2920.0x05) = default max fwd speed = ~45 kph + // 7200 rpm = default overspeed warning trigger (STOP lamp ON) = ~56 kph + // 8500 rpm = default overspeed brakedown trigger = ~66 kph + // 10000 rpm = max neutral speed (0x3813.2d) = ~77 kph + // 11000 rpm = severe overspeed fault (0x4624.00) = ~85 kph + + 45, // DefaultKphMax + 5814, // DefaultRpmMax + 1307, // DefaultRpmMaxRev + 686, // DeltaBrkStart + 1386, // DeltaBrkEnd + 2686, // DeltaBrkDown + + 56, // DefaultKphWarn + 7200, // DefaultRpmWarn + 900, // DeltaWarnOff + + 32500, // DefaultTrq + 33000, // DefaultTrqRated + 36000, // DefaultTrqLim + 500, // DeltaMapTrq + + 270000, // DefaultCurrLim + 290, // DefaultCurrStatorMax + 330, // BoostCurr + { 480, 8192, 576, 8960 }, // DefaultFMAP + { 656, 9600, 1328, 11901 }, // ExtendedFMAP + + 7050, // DefaultPwrLo + 10000, // DefaultPwrLoLim + 7650, // DefaultPwrHi + 10000, // DefaultPwrHiLim + 2688, // DefaultMaxMotorPwr [1/256 kW] + + 209, // DefaultRecup + 21, // DefaultRecupPrc + + 300, // DefaultRampStart + 30, // DefaultRampStartPrm + 2083, // DefaultRampAccel + 21, // DefaultRampAccelPrc + + // DefaultPMAP: + { 520,0, 520,2050, 437,2500, 363,3000, 314,3500, + 279,4000, 247,4500, 226,5000, 195,6000 }, + + // DefaultMapSpd: + { 4357, 5083, 6535, 8714 } + } +}; + +// ROM parameter access macro: +#define CFG twizy_cfg_params[twizy_cfg.type] + + +// scale utility: +// returns value scaled limited by and +UINT32 scale(UINT32 deflt, UINT32 from, UINT32 to, UINT32 min, UINT32 max) +{ + UINT32 val; + + if (to == from) + return deflt; + + val = (deflt * to) / from; + + if (val < min) + return min; + else if (val > max) + return max; + else + return val; +} + + +// vehicle_twizy_cfg_makepowermap(): +// Internal utility for cfg_speed() and cfg_power() +// builds torque/speed curve (SDO 0x4611) for current max values... +// - twizy_max_rpm +// - twizy_max_trq +// - twizy_max_pwr_lo +// - twizy_max_pwr_hi +// See "Twizy Powermap Calculator" spreadsheet for details. + +UINT8 pmap_fib[7] = { 1, 2, 3, 5, 8, 13, 21 }; + +UINT vehicle_twizy_cfg_makepowermap(void) + /* twizy_max_rpm, twizy_max_trq, twizy_max_pwr_lo, twizy_max_pwr_hi */ +{ + UINT err; + UINT8 i; + UINT rpm_0, rpm; + UINT trq; + UINT pwr; + int rpm_d, pwr_d; + + if (twizy_max_rpm == CFG.DefaultRpmMax && twizy_max_trq == CFG.DefaultTrq + && twizy_max_pwr_lo == CFG.DefaultPwrLo && twizy_max_pwr_hi == CFG.DefaultPwrHi) { + + // restore default torque map: + for (i=0; i<18; i++) { + if (err = writesdo(0x4611,0x01+i,CFG.DefaultPMAP[i])) + return err; + } + + // restore default flux map (only last 2 points): + for (i=0; i<4; i++) { + if (err = writesdo(0x4610,0x0f+i,CFG.DefaultFMAP[i])) + return err; + } + } + + else { + + // calculate constant torque part: + + rpm_0 = (((UINT32)twizy_max_pwr_lo * 9549 + (twizy_max_trq>>1)) / twizy_max_trq); + trq = (twizy_max_trq * 16 + 500) / 1000; + + if (err = writesdo(0x4611,0x01,trq)) + return err; + if (err = writesdo(0x4611,0x02,0)) + return err; + if (err = writesdo(0x4611,0x03,trq)) + return err; + if (err = writesdo(0x4611,0x04,rpm_0)) + return err; + + // adjust flux map (only last 2 points): + + if (trq > CFG.DefaultFMAP[2]) { + for (i=0; i<4; i++) { + if (err = writesdo(0x4610,0x0f+i,CFG.ExtendedFMAP[i])) + return err; + } + } + else { + for (i=0; i<4; i++) { + if (err = writesdo(0x4610,0x0f+i,CFG.DefaultFMAP[i])) + return err; + } + } + + // calculate constant power part: + + if (twizy_max_rpm > rpm_0) + rpm_d = (twizy_max_rpm - rpm_0 + (pmap_fib[6]>>1)) / pmap_fib[6]; + else + rpm_d = 0; + + pwr_d = ((int)twizy_max_pwr_hi - (int)twizy_max_pwr_lo + (pmap_fib[5]>>1)) / pmap_fib[5]; + + for (i=0; i<7; i++) { + + if (i<6) + rpm = rpm_0 + (pmap_fib[i] * rpm_d); + else + rpm = twizy_max_rpm; + + if (i<5) + pwr = twizy_max_pwr_lo + (pmap_fib[i] * pwr_d); + else + pwr = twizy_max_pwr_hi; + + trq = ((((UINT32)pwr * 9549 + (rpm>>1)) / rpm) * 16 + 500) / 1000; + + if (err = writesdo(0x4611,0x05+(i<<1),trq)) + return err; + if (err = writesdo(0x4611,0x06+(i<<1),rpm)) + return err; + + } + + } + + // commit map changes: + if (err = writesdo(0x4641,0x01,1)) + return err; + delay5(10); + + return 0; +} + + +// vehicle_twizy_cfg_readmaxpwr: +// read twizy_max_pwr_lo & twizy_max_pwr_hi +// from SEVCON registers (only if necessary / undefined) +UINT vehicle_twizy_cfg_readmaxpwr(void) +{ + UINT err; + UINT rpm; + + // the controller does not store max power, derive it from rpm/trq: + + if (twizy_max_pwr_lo == 0) { + if (err = readsdo(0x4611,0x04)) + return err; + rpm = twizy_sdo.data; + if (err = readsdo(0x4611,0x03)) + return err; + if (twizy_sdo.data == CFG.DefaultPMAP[2] && rpm == CFG.DefaultPMAP[3]) + twizy_max_pwr_lo = CFG.DefaultPwrLo; + else + twizy_max_pwr_lo = (UINT)((((twizy_sdo.data*1000)>>4) * rpm + (9549>>1)) / 9549); + } + + if (twizy_max_pwr_hi == 0) { + if (err = readsdo(0x4611,0x12)) + return err; + rpm = twizy_sdo.data; + if (err = readsdo(0x4611,0x11)) + return err; + if (twizy_sdo.data == CFG.DefaultPMAP[16] && rpm == CFG.DefaultPMAP[17]) + twizy_max_pwr_hi = CFG.DefaultPwrHi; + else + twizy_max_pwr_hi = (UINT)((((twizy_sdo.data*1000)>>4) * rpm + (9549>>1)) / 9549); + } + + return 0; +} + + +UINT vehicle_twizy_cfg_speed(int max_kph, int warn_kph) +// max_kph: 6..?, -1=reset to default (80) +// warn_kph: 6..?, -1=reset to default (89) +{ + UINT err; + UINT rpm; + + CHECKPOINT (41) + + // parameter validation: + + if (max_kph == -1) + max_kph = CFG.DefaultKphMax; + else if (max_kph < 6) + return ERR_Range + 1; + + if (warn_kph == -1) + warn_kph = CFG.DefaultKphWarn; + else if (warn_kph < 6) + return ERR_Range + 2; + + + // get max torque for map scaling: + + if (twizy_max_trq == 0) { + if (err = readsdo(0x6076,0x00)) + return err; + twizy_max_trq = twizy_sdo.data - CFG.DeltaMapTrq; + } + + // get max power for map scaling: + + if (err = vehicle_twizy_cfg_readmaxpwr()) + return err; + + // set overspeed warning range (STOP lamp): + rpm = scale(CFG.DefaultRpmWarn,CFG.DefaultKphWarn,warn_kph,400,65535); + if (err = writesdo(0x3813,0x34,rpm)) // lamp ON + return err; + if (err = writesdo(0x3813,0x3c,rpm-CFG.DeltaWarnOff)) // lamp OFF + return err; + + // calc fwd rpm: + rpm = scale(CFG.DefaultRpmMax,CFG.DefaultKphMax,max_kph,400,65535); + + // set fwd rpm: + err = writesdo(0x2920,0x05,rpm); + if (err) + return err; + + // set rev rpm: + err = writesdo(0x2920,0x06,LIMIT_MAX(rpm,CFG.DefaultRpmRev)); + if (err) + return err; + + // adjust overspeed braking points (using fixed offsets): + if (err = writesdo(0x3813,0x33,rpm+CFG.DeltaBrkStart)) // neutral braking start + return err; + if (err = writesdo(0x3813,0x35,rpm+CFG.DeltaBrkEnd)) // neutral braking end + return err; + if (err = writesdo(0x3813,0x3b,rpm+CFG.DeltaBrkDown)) // drive brakedown trigger + return err; + + // adjust overspeed limits: + if (err = writesdo(0x3813,0x2d,rpm+CFG.DeltaBrkDown+1500)) // neutral max speed + return err; + if (err = writesdo(0x4624,0x00,rpm+CFG.DeltaBrkDown+2500)) // severe overspeed fault + return err; + + twizy_max_rpm = rpm; + + return 0; +} + + +UINT vehicle_twizy_cfg_power(int trq_prc, int pwr_lo_prc, int pwr_hi_prc, int curr_prc) +// See "Twizy Powermap Calculator" spreadsheet. +// trq_prc: 10..130, -1=reset to default (100%) +// i.e. TWIZY80: +// 100% = 55.000 Nm +// 128% = 70.125 Nm (130 allowed for easy handling) +// pwr_lo_prc: 10..139, -1=reset to default (100%) +// 100% = 12182 W (mechanical) +// 139% = 16933 W (mechanical) +// pwr_hi_prc: 10..130, -1=reset to default (100%) +// 100% = 13000 W (mechanical) +// 130% = 16900 W (mechanical) +// curr_prc: 10..123, -1=reset to default current limits +// 100% = 450 A (Twizy 45: 270 A) +// 120% = 540 A (Twizy 45: 324 A) +// 123% = 540 A (Twizy 45: 330 A) +// setting this to any value disables high limits of trq & pwr +// (=> flux map extended to enable higher torque) +// safety max limits = SEVCON model specific boost current level +{ + UINT err; + BOOL limited = FALSE; + + // parameter validation: + + if (curr_prc == -1) { + curr_prc = 100; + limited = TRUE; + } + else if (curr_prc < 10 || curr_prc > 123) + return ERR_Range + 4; + + if (trq_prc == -1) + trq_prc = 100; + else if (trq_prc < 10 || (limited && trq_prc > 130)) + return ERR_Range + 1; + + if (pwr_lo_prc == -1) + pwr_lo_prc = 100; + else if (pwr_lo_prc < 10 || (limited && pwr_lo_prc > 139)) + return ERR_Range + 2; + + if (pwr_hi_prc == -1) + pwr_hi_prc = 100; + else if (pwr_hi_prc < 10 || (limited && pwr_hi_prc > 130)) + return ERR_Range + 3; + + // get max fwd rpm for map scaling: + if (twizy_max_rpm == 0) { + if (err = readsdo(0x2920,0x05)) + return err; + twizy_max_rpm = twizy_sdo.data; + } + + // set current limits: + if (err = writesdo(0x4641,0x02,scale( + CFG.DefaultCurrStatorMax,100,curr_prc,0,CFG.BoostCurr))) + return err; + if (err = writesdo(0x6075,0x00,scale( + CFG.DefaultCurrLim,100,curr_prc,0,CFG.BoostCurr*1000L))) + return err; + + // calc peak use torque: + twizy_max_trq = scale(CFG.DefaultTrq,100,trq_prc,10000, + (limited) ? CFG.DefaultTrqLim : 200000); + + // set peak use torque: + if (err = writesdo(0x6076,0x00,twizy_max_trq + CFG.DeltaMapTrq)) + return err; + + // set rated torque: + if (err = writesdo(0x2916,0x01,(trq_prc==100) + ? CFG.DefaultTrqRated + : (twizy_max_trq + CFG.DeltaMapTrq))) + return err; + + // calc peak use power: + twizy_max_pwr_lo = scale(CFG.DefaultPwrLo,100,pwr_lo_prc,500, + (limited) ? CFG.DefaultPwrLoLim : 200000); + twizy_max_pwr_hi = scale(CFG.DefaultPwrHi,100,pwr_hi_prc,500, + (limited) ? CFG.DefaultPwrHiLim : 200000); + + // set motor max power: + if (err = writesdo(0x3813,0x23,(pwr_lo_prc==100 && pwr_hi_prc==100) + ? CFG.DefaultMaxMotorPwr + : MAX(twizy_max_pwr_lo,twizy_max_pwr_hi)*0.353)) + return err; + + return 0; +} + + +UINT vehicle_twizy_cfg_tsmap( + char map, + INT8 t1_prc, INT8 t2_prc, INT8 t3_prc, INT8 t4_prc, + int t1_spd, int t2_spd, int t3_spd, int t4_spd) +// map: 'D'=Drive 'N'=Neutral 'B'=Footbrake +// +// t1_prc: 0..100, -1=reset to default (D=100, N/B=100) +// t2_prc: 0..100, -1=reset to default (D=100, N/B=80) +// t3_prc: 0..100, -1=reset to default (D=100, N/B=50) +// t4_prc: 0..100, -1=reset to default (D=100, N/B=20) +// +// t1_spd: 0..?, -1=reset to default (D/N/B=33) +// t2_spd: 0..?, -1=reset to default (D/N/B=39) +// t3_spd: 0..?, -1=reset to default (D/N/B=50) +// t4_spd: 0..?, -1=reset to default (D/N/B=66) +{ + UINT err; + UINT8 base; + UINT val, bndlo, bndhi; + UINT8 todo; + + // parameter validation: + + if (map != 'D' && map != 'N' && map != 'B') + return ERR_Range + 1; + + // torque points: + + if ((t1_prc != -1) && (t1_prc < 0 || t1_prc > 100)) + return ERR_Range + 2; + if ((t2_prc != -1) && (t2_prc < 0 || t2_prc > 100)) + return ERR_Range + 3; + if ((t3_prc != -1) && (t3_prc < 0 || t3_prc > 100)) + return ERR_Range + 4; + if ((t4_prc != -1) && (t4_prc < 0 || t4_prc > 100)) + return ERR_Range + 5; + + // speed points: + + if ((t1_spd != -1) && (t1_spd < 0)) + return ERR_Range + 6; + if ((t2_spd != -1) && (t2_spd < 0)) + return ERR_Range + 7; + if ((t3_spd != -1) && (t3_spd < 0)) + return ERR_Range + 8; + if ((t4_spd != -1) && (t4_spd < 0)) + return ERR_Range + 9; + + // get map base subindex in SDO 0x3813: + + if (map=='B') + base = 0x07; + else if (map=='N') + base = 0x1b; + else // 'D' + base = 0x24; + + // set: + // we need to adjust point by point to avoid the "Param dyn range" alert, + // ensuring a new speed has no conflict with the previous surrounding points + + todo = 0x0f; + + while (todo) { + + // point 1: + if (todo & 0x01) { + + // get speed boundaries: + bndlo = 0; + if (err = readsdo(0x3813,base+3)) + return err; + bndhi = twizy_sdo.data; + + // calc new speed: + if (t1_spd >= 0) + val = scale(CFG.DefaultMapSpd[0],33,t1_spd,0,65535); + else + val = (map=='D') ? 3000 : CFG.DefaultMapSpd[0]; + + if (val >= bndlo && val <= bndhi) { + // ok, change: + if (err = writesdo(0x3813,base+1,val)) + return err; + + if (t1_prc >= 0) + val = scale(32767,100,t1_prc,0,32767); + else + val = 32767; + if (err = writesdo(0x3813,base+0,val)) + return err; + + todo &= ~0x01; + } + } + + // point 2: + if (todo & 0x02) { + + // get speed boundaries: + if (err = readsdo(0x3813,base+1)) + return err; + bndlo = twizy_sdo.data; + if (err = readsdo(0x3813,base+5)) + return err; + bndhi = twizy_sdo.data; + + // calc new speed: + if (t2_spd >= 0) + val = scale(CFG.DefaultMapSpd[1],39,t2_spd,0,65535); + else + val = (map=='D') ? 3500 : CFG.DefaultMapSpd[1]; + + if (val >= bndlo && val <= bndhi) { + // ok, change: + if (err = writesdo(0x3813,base+3,val)) + return err; + + if (t2_prc >= 0) + val = scale(32767,100,t2_prc,0,32767); + else + val = (map=='D') ? 32767 : 26214; + if (err = writesdo(0x3813,base+2,val)) + return err; + + todo &= ~0x02; + } + } + + // point 3: + if (todo & 0x04) { + + // get speed boundaries: + if (err = readsdo(0x3813,base+3)) + return err; + bndlo = twizy_sdo.data; + if (err = readsdo(0x3813,base+7)) + return err; + bndhi = twizy_sdo.data; + + // calc new speed: + if (t3_spd >= 0) + val = scale(CFG.DefaultMapSpd[2],50,t3_spd,0,65535); + else + val = (map=='D') ? 4500 : CFG.DefaultMapSpd[2]; + + if (val >= bndlo && val <= bndhi) { + // ok, change: + if (err = writesdo(0x3813,base+5,val)) + return err; + + if (t3_prc >= 0) + val = scale(32767,100,t3_prc,0,32767); + else + val = (map=='D') ? 32767 : 16383; + if (err = writesdo(0x3813,base+4,val)) + return err; + + todo &= ~0x04; + } + } + + // point 4: + if (todo & 0x08) { + + // get speed boundaries: + if (err = readsdo(0x3813,base+5)) + return err; + bndlo = twizy_sdo.data; + bndhi = 65535; + + // calc new speed: + if (t4_spd >= 0) + val = scale(CFG.DefaultMapSpd[3],66,t4_spd,0,65535); + else + val = (map=='D') ? 6000 : CFG.DefaultMapSpd[3]; + + if (val >= bndlo && val <= bndhi) { + // ok, change: + if (err = writesdo(0x3813,base+7,val)) + return err; + + if (t4_prc >= 0) + val = scale(32767,100,t4_prc,0,32767); + else + val = (map=='D') ? 32767 : 6553; + if (err = writesdo(0x3813,base+6,val)) + return err; + + todo &= ~0x08; + } + } + + } // while (todo) + + + return 0; +} + + +UINT vehicle_twizy_cfg_drive(int max_prc, int autodrive_ref, int autodrive_minprc) +// max_prc: 10..100, -1=reset to default (100) +// autodrive_ref: 0..250, -1=default (off) (not a direct SEVCON control) +// sets power 100% ref point to * 100 W +// autodrive_minprc: 0..100, -1=default (off) (not a direct SEVCON control) +// sets lower limit on auto power adjustment +{ + UINT err; + + // parameter validation: + + if (max_prc == -1) + max_prc = 100; + else if (max_prc < 10 || max_prc > 100) + return ERR_Range + 1; + +#ifdef OVMS_TWIZY_BATTMON + + if (autodrive_ref == -1) + ; + else if (autodrive_ref < 0 || autodrive_ref > 250) + return ERR_Range + 2; + + if (autodrive_minprc == -1) + autodrive_minprc = 0; + else if (autodrive_minprc < 0 || autodrive_minprc > 100) + return ERR_Range + 3; + + if ((autodrive_ref > 0) && (!sys_can.DisableAutoPower)) + { + // calculate max drive level: + twizy_autodrive_level = LIMIT_MAX(((long) twizy_batt[0].max_drive_pwr * 5L * 1000L) + / autodrive_ref, 1000); + twizy_autodrive_level = LIMIT_MIN(twizy_autodrive_level, autodrive_minprc * 10); + + // set autopower checkpoint: + twizy_autodrive_checkpoint = (twizy_batt[0].max_drive_pwr + autodrive_ref + autodrive_minprc); + } + else + { + twizy_autodrive_level = 1000; + } + +#else + + twizy_autodrive_level = 1000; + +#endif //OVMS_TWIZY_BATTMON + + // set: + if (err = writesdo(0x2920,0x01,LIMIT_MAX(max_prc*10, twizy_autodrive_level))) + return err; + + return 0; +} + + +UINT vehicle_twizy_cfg_recup(int neutral_prc, int brake_prc, int autorecup_ref, int autorecup_minprc) +// neutral_prc: 0..100, -1=reset to default (18) +// brake_prc: 0..100, -1=reset to default (18) +// autorecup_ref: 0..250, -1=default (off) (not a direct SEVCON control) +// ATT: parameter function changed in V3.6.0! +// now sets power 100% ref point to * 100 W +// autorecup_minprc: 0..100, -1=default (off) (not a direct SEVCON control) +// sets lower limit on auto power adjustment +{ + UINT err; + UINT level; + + // parameter validation: + + if (neutral_prc == -1) + neutral_prc = CFG.DefaultRecupPrc; + else if (neutral_prc < 0 || neutral_prc > 100) + return ERR_Range + 1; + + if (brake_prc == -1) + brake_prc = CFG.DefaultRecupPrc; + else if (brake_prc < 0 || brake_prc > 100) + return ERR_Range + 2; + +#ifdef OVMS_TWIZY_BATTMON + + if (autorecup_ref == -1) + ; + else if (autorecup_ref < 0 || autorecup_ref > 250) + return ERR_Range + 3; + + if (autorecup_minprc == -1) + autorecup_minprc = 0; + else if (autorecup_minprc < 0 || autorecup_minprc > 100) + return ERR_Range + 4; + + if ((autorecup_ref > 0) && (!sys_can.DisableAutoPower)) + { + // calculate max recuperation level: + twizy_autorecup_level = LIMIT_MAX(((long) twizy_batt[0].max_recup_pwr * 5L * 1000L) + / autorecup_ref, 1000); + twizy_autorecup_level = LIMIT_MIN(twizy_autorecup_level, autorecup_minprc * 10); + + // set autopower checkpoint: + twizy_autorecup_checkpoint = (twizy_batt[0].max_recup_pwr + autorecup_ref + autorecup_minprc); + } + else + { + twizy_autorecup_level = 1000; + } + +#else + + twizy_autorecup_level = 1000; + +#endif //OVMS_TWIZY_BATTMON + + // set neutral recup level: + level = scale(CFG.DefaultRecup,CFG.DefaultRecupPrc,neutral_prc,0,1000); + if (twizy_autorecup_level != 1000) + level = (((long) level) * twizy_autorecup_level) / 1000; + if (err = writesdo(0x2920,0x03,level)) + return err; + + // set brake recup level: + level = scale(CFG.DefaultRecup,CFG.DefaultRecupPrc,brake_prc,0,1000); + if (twizy_autorecup_level != 1000) + level = (((long) level) * twizy_autorecup_level) / 1000; + if (err = writesdo(0x2920,0x04,level)) + return err; + + return 0; +} + + +#ifdef OVMS_TWIZY_BATTMON + +// Auto recup & drive power update function: +// check for BMS max pwr change, update SEVCON settings accordingly +// this is called by vehicle_twizy_state_ticker1() = approx. once per second +void vehicle_twizy_cfg_autopower(void) +{ + int ref, minprc; + + // check for SEVCON write access: + if ((!sys_can.EnableWrite) || (sys_can.DisableAutoPower) + || ((twizy_status & CAN_STATUS_KEYON) == 0)) + return; + + // adjust recup levels? + ref = cfgparam(autorecup_ref); + minprc = cfgparam(autorecup_minprc); + if ((ref > 0) + && ((twizy_batt[0].max_recup_pwr + ref + minprc) != twizy_autorecup_checkpoint)) + { + vehicle_twizy_cfg_recup(cfgparam(neutral), cfgparam(brake), ref, minprc); + } + + // adjust drive level? + ref = cfgparam(autodrive_ref); + minprc = cfgparam(autodrive_minprc); + if ((ref > 0) + && ((twizy_batt[0].max_drive_pwr + ref + minprc) != twizy_autodrive_checkpoint) + && (twizy_kickdown_hold == 0)) + { + vehicle_twizy_cfg_drive(cfgparam(drive), ref, minprc); + } +} + +#endif //OVMS_TWIZY_BATTMON + + +UINT vehicle_twizy_cfg_ramps(int start_prm, int accel_prc, int decel_prc, int neutral_prc, int brake_prc) +// start_prm: 1..250, -1=reset to default (40) (Att! changed in V3.4 from prc to prm!) +// accel_prc: 1..100, -1=reset to default (25) +// decel_prc: 0..100, -1=reset to default (20) +// neutral_prc: 0..100, -1=reset to default (40) +// brake_prc: 0..100, -1=reset to default (40) +{ + UINT err; + + // parameter validation: + + if (start_prm == -1) + start_prm = CFG.DefaultRampStartPrm; + else if (start_prm < 1 || start_prm > 250) + return ERR_Range + 1; + + if (accel_prc == -1) + accel_prc = CFG.DefaultRampAccelPrc; + else if (accel_prc < 1 || accel_prc > 100) + return ERR_Range + 2; + + if (decel_prc == -1) + decel_prc = 20; + else if (decel_prc < 0 || decel_prc > 100) + return ERR_Range + 3; + + if (neutral_prc == -1) + neutral_prc = 40; + else if (neutral_prc < 0 || neutral_prc > 100) + return ERR_Range + 4; + + if (brake_prc == -1) + brake_prc = 40; + else if (brake_prc < 0 || brake_prc > 100) + return ERR_Range + 5; + + // set: + + if (err = writesdo(0x291c,0x02,scale(CFG.DefaultRampStart,CFG.DefaultRampStartPrm,start_prm,10,10000))) + return err; + if (err = writesdo(0x2920,0x07,scale(CFG.DefaultRampAccel,CFG.DefaultRampAccelPrc,accel_prc,10,10000))) + return err; + if (err = writesdo(0x2920,0x0b,scale(2000,20,decel_prc,10,10000))) + return err; + if (err = writesdo(0x2920,0x0d,scale(4000,40,neutral_prc,10,10000))) + return err; + if (err = writesdo(0x2920,0x0e,scale(4000,40,brake_prc,10,10000))) + return err; + + return 0; +} + + +UINT vehicle_twizy_cfg_rampl(int accel_prc, int decel_prc) +// accel_prc: 1..100, -1=reset to default (30) +// decel_prc: 0..100, -1=reset to default (30) +{ + UINT err; + + // parameter validation: + + if (accel_prc == -1) + accel_prc = 30; + else if (accel_prc < 1 || accel_prc > 100) + return ERR_Range + 1; + + if (decel_prc == -1) + decel_prc = 30; + else if (decel_prc < 0 || decel_prc > 100) + return ERR_Range + 2; + + // set: + + if (err = writesdo(0x2920,0x0f,scale(6000,30,accel_prc,0,20000))) + return err; + if (err = writesdo(0x2920,0x10,scale(6000,30,decel_prc,0,20000))) + return err; + + return 0; +} + + +UINT vehicle_twizy_cfg_smoothing(int prc) +// prc: 0..100, -1=reset to default (70) +{ + UINT err; + + // parameter validation: + + if (prc == -1) + prc = 70; + else if (prc < 0 || prc > 100) + return ERR_Range + 1; + + // set: + if (err = writesdo(0x290a,0x01,1+(prc/10))) + return err; + if (err = writesdo(0x290a,0x03,scale(800,70,prc,0,1000))) + return err; + + return 0; +} + + +#ifdef OVMS_TWIZY_CFG_BRAKELIGHT + +UINT vehicle_twizy_cfg_brakelight(int on_lev, int off_lev) +// *** NOT FUNCTIONAL WITHOUT HARDWARE MODIFICATION *** +// *** SEVCON cannot control Twizy brake lights *** +// on_lev: 0..100, -1=reset to default (100=off) +// off_lev: 0..100, -1=reset to default (100=off) +// on_lev must be >= off_lev +// ctrl bit in 0x2910.1 will be set/cleared accordingly +{ + UINT err; + + // parameter validation: + + if (on_lev == -1) + on_lev = 100; + else if (on_lev < 0 || on_lev > 100) + return ERR_Range + 1; + + if (off_lev == -1) + off_lev = 100; + else if (off_lev < 0 || off_lev > 100) + return ERR_Range + 2; + + if (on_lev < off_lev) + return ERR_Range + 3; + + // set range: + if (err = writesdo(0x3813,0x05,scale(1024,100,off_lev,64,1024))) + return err; + if (err = writesdo(0x3813,0x06,scale(1024,100,on_lev,64,1024))) + return err; + + // set ctrl bit: + if (err = readsdo(0x2910,0x01)) + return err; + if (on_lev != 100 || off_lev != 100) + twizy_sdo.data |= 0x2000; + else + twizy_sdo.data &= ~0x2000; + if (err = writesdo(0x2910,0x01,twizy_sdo.data)) + return err; + + return 0; +} + +#endif // OVMS_TWIZY_CFG_BRAKELIGHT + + +// vehicle_twizy_cfg_calc_checksum: get checksum for twizy_cfg_profile +// +// Note: for extendability of struct twizy_cfg_profile, 0-Bytes will +// not affect the checksum, so new fields can simply be added at the end +// without losing version compatibility. +// (0-bytes translate to value -1 = default) +BYTE vehicle_twizy_cfg_calc_checksum(BYTE *profile) +{ + UINT checksum; + UINT8 i; + + checksum = 0x0101; // version tag + + for (i=1; i>= 8; + + return (checksum & 0x0ff); +} + + +// vehicle_twizy_cfg_readprofile: read profile from params to twizy_cgf_profile +// with checksum validation +// it invalid checksum initialize to default config & return FALSE +BOOL vehicle_twizy_cfg_readprofile(UINT8 key) +{ + if (key >= 1 && key <= 3) { + // read custom cfg from params: + //par_getbin(PARAM_PROFILE_S + ((key-1)<<1), &twizy_cfg_profile, sizeof twizy_cfg_profile); + EEPROM.get(key * 64, twizy_cfg_profile); + + // check consistency: + if (twizy_cfg_profile.checksum == vehicle_twizy_cfg_calc_checksum((BYTE *)&twizy_cfg_profile)) + return TRUE; + else { + // init to defaults: + memset(&twizy_cfg_profile, 0, sizeof(twizy_cfg_profile)); + return FALSE; + } + } + else { + // no custom cfg: load defaults + memset(&twizy_cfg_profile, 0, sizeof(twizy_cfg_profile)); + return TRUE; + } +} + + +// vehicle_twizy_cfg_writeprofile: write from twizy_cgf_profile to params +// with checksum calculation +BOOL vehicle_twizy_cfg_writeprofile(UINT8 key) +{ + if (key >= 1 && key <= 3) { + twizy_cfg_profile.checksum = vehicle_twizy_cfg_calc_checksum((BYTE *)&twizy_cfg_profile); + //par_setbin(PARAM_PROFILE_S + ((key-1)<<1), &twizy_cfg_profile, sizeof twizy_cfg_profile); + EEPROM.put(key * 64, twizy_cfg_profile); + return TRUE; + } + else { + return FALSE; + } +} + + +// vehicle_twizy_cfg_applyprofile: configure current profile +// return value: 0 = no error, else error code +// sets: twizy_cfg.profile_cfgmode, twizy_cfg.profile_user +UINT vehicle_twizy_cfg_applyprofile(UINT8 key) +{ + UINT err; + int pval; + + // clear success flag: + twizy_cfg.applied = 0; + + // login: + + if (err = login(1)) + return err; + + // update op (user) mode params: + + if (err = vehicle_twizy_cfg_drive(cfgparam(drive),cfgparam(autodrive_ref),cfgparam(autodrive_minprc))) + return err; + + if (err = vehicle_twizy_cfg_recup(cfgparam(neutral),cfgparam(brake),cfgparam(autorecup_ref),cfgparam(autorecup_minprc))) + return err; + + if (err = vehicle_twizy_cfg_ramps(cfgparam(ramp_start),cfgparam(ramp_accel),cfgparam(ramp_decel),cfgparam(ramp_neutral),cfgparam(ramp_brake))) + return err; + + if (err = vehicle_twizy_cfg_rampl(cfgparam(ramplimit_accel),cfgparam(ramplimit_decel))) + return err; + + if (err = vehicle_twizy_cfg_smoothing(cfgparam(smooth))) + return err; + + // update user profile status: + twizy_cfg.profile_user = key; + + + // update pre-op (admin) mode params if configmode possible at the moment: + + err = configmode(1); + + if (!err) { + + err = vehicle_twizy_cfg_speed(cfgparam(speed),cfgparam(warn)); + if (!err) + err = vehicle_twizy_cfg_power(cfgparam(torque),cfgparam(power_low),cfgparam(power_high),cfgparam(current)); + if (!err) + err = vehicle_twizy_cfg_makepowermap(); + + if (!err) + err = vehicle_twizy_cfg_tsmap('D', + cfgparam(tsmap[0].prc1),cfgparam(tsmap[0].prc2),cfgparam(tsmap[0].prc3),cfgparam(tsmap[0].prc4), + cfgparam(tsmap[0].spd1),cfgparam(tsmap[0].spd2),cfgparam(tsmap[0].spd3),cfgparam(tsmap[0].spd4)); + if (!err) + err = vehicle_twizy_cfg_tsmap('N', + cfgparam(tsmap[1].prc1),cfgparam(tsmap[1].prc2),cfgparam(tsmap[1].prc3),cfgparam(tsmap[1].prc4), + cfgparam(tsmap[1].spd1),cfgparam(tsmap[1].spd2),cfgparam(tsmap[1].spd3),cfgparam(tsmap[1].spd4)); + if (!err) + err = vehicle_twizy_cfg_tsmap('B', + cfgparam(tsmap[2].prc1),cfgparam(tsmap[2].prc2),cfgparam(tsmap[2].prc3),cfgparam(tsmap[2].prc4), + cfgparam(tsmap[2].spd1),cfgparam(tsmap[2].spd2),cfgparam(tsmap[2].spd3),cfgparam(tsmap[2].spd4)); + +#ifdef OVMS_TWIZY_CFG_BRAKELIGHT + if (!err) + err = vehicle_twizy_cfg_brakelight(cfgparam(brakelight_on),cfgparam(brakelight_off)); +#endif // OVMS_TWIZY_CFG_BRAKELIGHT + + // update cfgmode profile status: + if (err == 0) + twizy_cfg.profile_cfgmode = key; + + // switch back to op-mode (5 tries): + key = 5; + while (configmode(0) != 0) { + if (--key == 0) + break; + delay5(20); // 100 ms + } + + } else { + + // pre-op mode currently not possible; + // just set speed limit: + pval = cfgparam(speed); + if (pval == -1) + pval = CFG.DefaultKphMax; + + err = writesdo(0x2920,0x05,scale(CFG.DefaultRpmMax,CFG.DefaultKphMax,pval,0,65535)); + if (!err) + err = writesdo(0x2920,0x06,scale(CFG.DefaultRpmMax,CFG.DefaultKphMax,pval,0,CFG.DefaultRpmRev)); + + } + + // set success flag: + twizy_cfg.applied = 1; + + return err; +} + + +// vehicle_twizy_cfg_switchprofile: load and configure a profile +// return value: 0 = no error, else error code +// sets: twizy_cfg.profile_cfgmode, twizy_cfg.profile_user +UINT vehicle_twizy_cfg_switchprofile(UINT8 key) +{ + // check key: + + if (key > 3) + return ERR_Range + 1; + + // load new profile: + vehicle_twizy_cfg_readprofile(key); + twizy_cfg.unsaved = 0; + + // apply profile: + return vehicle_twizy_cfg_applyprofile(key); +} + + +#ifdef OVMS_TWIZY_DEBUG +// utility: output power map info to string: +char *vehicle_twizy_fmt_powermap(char *s) +{ + // Pt2 = Constant Torque End (max_pwr_lo / max_trq) + s = stp_i(s, " PwrLo: ", twizy_max_pwr_lo); + readsdo(0x4611,0x03); + s = stp_l2f(s, "W / ", (twizy_sdo.data * 1000) / 16, 3); + readsdo(0x4611,0x04); + s = stp_l2f(s, "Nm @ ", (twizy_sdo.data * CFG.DefaultKphMax * 10) / CFG.DefaultRpmMax, 1); + s = stp_rom(s, "kph"); + + // Pt8 = max_pwr_hi + s = stp_i(s, " PwrHi: ", twizy_max_pwr_hi); + readsdo(0x4611,0x0f); + s = stp_l2f(s, "W / ", (twizy_sdo.data * 1000) / 16, 3); + readsdo(0x4611,0x10); + s = stp_l2f(s, "Nm @ ", (twizy_sdo.data * CFG.DefaultKphMax * 10) / CFG.DefaultRpmMax, 1); + s = stp_rom(s, "kph"); + + return s; +} +#endif // OVMS_TWIZY_DEBUG + + +// utility: output profile switch result info to string +// and set new profile nr in PARAM_PROFILE +char *vehicle_twizy_fmt_switchprofileresult(char *s, INT8 profilenr, UINT err) +{ + if (err && (err != ERR_CfgModeFailed)) { + // failure: + s = vehicle_twizy_fmt_err(s, err); + } + else { + + if (profilenr == -1) { + s = stp_rom(s, "WS"); // working set + } + else { + s = stp_i(s, "#", profilenr); + //par_set(PARAM_PROFILE, s-1); + EEPROM.put(PARAM_PROFILE, profilenr); + } + + if (err == ERR_CfgModeFailed) { + // partial success: + s = stp_rom(s, " PARTIAL: retry at stop!"); + } + else { + // full success: + s = stp_i(s, " OK: 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)); + } + + return s; +} + + +void vehicle_twizy_init() { + INT8 i; + + twizy_max_rpm = 0; + twizy_max_trq = 0; + twizy_max_pwr_lo = 0; + twizy_max_pwr_hi = 0; + + twizy_cfg.type = 0; + + // reload last selected user profile on init: + + EEPROM.get(PARAM_PROFILE, i); + twizy_cfg.profile_user = constrain(i, 0, 3); + twizy_cfg.profile_cfgmode = twizy_cfg.profile_user; + vehicle_twizy_cfg_readprofile(twizy_cfg.profile_user); + + twizy_cfg.unsaved = 0; + twizy_cfg.keystate = 0; + + // Init autopower system: + twizy_autorecup_level = twizy_autodrive_level = 1000; + twizy_autorecup_checkpoint = twizy_autodrive_checkpoint = 0; + +} + diff --git a/TwizyCfg/TwizyCfg.ino b/TwizyCfg/TwizyCfg.ino index 322d147..077a513 100644 --- a/TwizyCfg/TwizyCfg.ino +++ b/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 #include #include + #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 -- write-only SDO register (numerical)\n" " p -- preop mode\n" " o -- op mode\n" + "(Hint: standard OVMS syntax also accepted)\n" "\n" + " set -- set profile from base64\n" + " reset -- reset profile\n" + " get -- get profile base64\n" + " info -- show main profile values\n" + " save -- save config to profile\n" + " load -- load config from profile\n" + "\n" + " drive -- set drive level\n" + " recup -- set recuperation levels neutral & brake\n" + " ramps
-- set ramp levels\n" + " rampl -- set ramp limits\n" + " smooth -- set smoothing\n" + "\n" + " speed -- set max & warn speed\n" + " power -- set torque, power & current levels\n" + " tsmap -- set torque speed maps\n" + " brakelight -- set brakelight accel levels\n" + "\n" + "See OVMS manual & command overview for details.\n" "Note: and are hexadecimal, 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); - s = stp_rom(s, ": "); - - // - // COMMAND DISPATCHER: - // Part 2: online commands (SEVCON access necessary) - // - PRE - // - OP - // - READ[S] - // - WRITE[O] - // - + + // + // Prepare command: + // + + err = 0; + + // 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: + // - else if (starts_with(cmd, "P")) { - // P: enter config mode + if (!err) switch (cmd.id) { + + + case cmdPreOp: + // 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")) { - // O: leave config mode + + case cmdOp: + // 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; + + 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 - else { + 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"); - } - - // go operational? - if (go_op_onexit) - configmode(0); - + break; } // - // FINISH: send command response + // FINISH: // + // go operational? + if (!err && go_op_onexit) + configmode(0); + Serial.println(net_scratchpad); return true; @@ -279,7 +816,10 @@ void setup() { CAN.setMode(MCP_NORMAL); - + // Init tuning module: + + vehicle_twizy_init(); + // Output info & prompt: exec((char *) "?"); Serial.print("\n> "); diff --git a/TwizyCfg/TwizyCfg_config.h b/TwizyCfg/TwizyCfg_config.h index dd17661..e740fed 100644 --- a/TwizyCfg/TwizyCfg_config.h +++ b/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 diff --git a/TwizyCfg/base64.ino b/TwizyCfg/base64.ino new file mode 100644 index 0000000..fce5dac --- /dev/null +++ b/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 +#include + +// 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;k0) + { + 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; + } + diff --git a/TwizyCfg/utils.h b/TwizyCfg/utils.h index 2e06096..8b753ad 100644 --- a/TwizyCfg/utils.h +++ b/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