github-mirrors/Twizy-Cfg
1
0
Fork 0
mirror of https://github.com/dexterbg/Twizy-Cfg.git synced 2024-11-18 11:53:59 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions
Twizy-Cfg/TwizyCfg/TwizyCfg.ino
Michael Balzer 5686de5daa Allow CRLF terminal configuration
2023-03-05 08:42:12 +01:00

954 lines
26 KiB
C++
Raw Blame History

/**
* ==========================================================================
* Twizy/SEVCON configuration shell
* ==========================================================================
*
* Based on the OVMS Twizy firmware:
* https://github.com/openvehicles/Open-Vehicle-Monitoring-System
*
* Author: Michael Balzer <dexter@dexters-web.de>
*
* Libraries used:
* - MCP_CAN: https://github.com/coryjfowler/MCP_CAN_lib
* - iso-tp: https://github.com/dexterbg/iso-tp [https://github.com/altelch/iso-tp]
*
* License:
* This is free software under GNU Lesser General Public License (LGPL)
* https://www.gnu.org/licenses/lgpl.html
*
*/
#define TWIZY_CFG_VERSION "V2.1.0 (2017-07-08)"
#include <EEPROM.h>
#include <mcp_can.h>
#include <mcp_can_dfs.h>
#include <iso-tp.h>
#include "utils.h"
#include "CANopen.h"
#include "Tuning.h"
#include "TwizyCfg_config.h"
// CAN interface:
MCP_CAN CAN(TWIZY_CAN_CS_PIN);
// ISO-TP:
IsoTp isotp(&CAN, TWIZY_CAN_IRQ_PIN);
struct Message_t tpMsg;
// Output buffers:
char net_scratchpad[200];
char net_msg_scratchpad[200];
// --------------------------------------------------------------------
// COMMAND DISPATCHER:
//
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,
cmdDiagAddress, cmdDiagRequest
};
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 },
{ "DA", cmdDiagAddress, modeOffline },
{ "DR", cmdDiagRequest, modeOffline },
};
#define COMMAND_COUNT (sizeof(command_table)/sizeof(cfg_command))
bool exec(char *cmdline)
{
UINT err;
char *s;
char *t;
bool go_op_onexit = true;
char *arguments;
int arg[5] = {-1,-1,-1,-1,-1};
INT8 arg2[4] = {-1,-1,-1,-1};
long data;
char maps[4] = {'D','N','B',0};
UINT8 i;
arguments = net_sms_initargs(cmdline);
// convert cmd to upper-case:
for (s=cmdline; ((*s!=0)&&(*s!=' ')); s++)
if ((*s > 0x60) && (*s < 0x7b)) *s=*s-0x20;
//
// 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"
"Commands:\n"
" ?, help -- output this info\n"
" r <id> <sub> -- read SDO register (numerical)\n"
" rs <id> <sub> -- read SDO register (string)\n"
" w <id> <sub> <val> -- write SDO register (numerical) & show old value\n"
" 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"
" da <sendid> <recvid> -- set OBD2 device address\n"
" dr <hexstring> -- send OBD2 request\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;
}
//
// 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 (!err && cmd.mode >= modePreOp) {
// enter config mode:
if (err = configmode(1)) {
s = vehicle_twizy_fmt_err(s, err);
}
}
//
// Execute command:
//
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;
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;
case cmdRead:
case cmdReadString:
// R index_hex subindex_hex
// RS index_hex subindex_hex
if (arguments = net_sms_nextarg(arguments))
arg[0] = (int)axtoul(arguments);
if (arguments = net_sms_nextarg(arguments))
arg[1] = (int)axtoul(arguments);
if (!arguments) {
s = stp_rom(s, "ERROR: Too few args");
}
else {
if (cmd.id == cmdRead) {
// READ:
if (err = readsdo(arg[0], arg[1])) {
s = vehicle_twizy_fmt_err(s, err);
}
else {
s = vehicle_twizy_fmt_sdo(s);
s = stp_ul(s, " = ", twizy_sdo.data);
}
}
else {
// READS: SMS intro 'CFG READS: 0x1234.56=' = 21 chars, 139 remaining
if (err = readsdo_buf(arg[0], arg[1], (byte*)net_msg_scratchpad, (i=139, &i))) {
s = vehicle_twizy_fmt_err(s, err);
}
else {
net_msg_scratchpad[139-i] = 0;
s = stp_x(s, "0x", arg[0]);
s = stp_sx(s, ".", arg[1]);
s = stp_s(s, "=", net_msg_scratchpad);
}
}
}
go_op_onexit = false;
break;
case cmdWrite:
case cmdWriteOnly:
// W index_hex subindex_hex data_dec
// WO index_hex subindex_hex data_dec
if (arguments = net_sms_nextarg(arguments))
arg[0] = (int)axtoul(arguments);
if (arguments = net_sms_nextarg(arguments))
arg[1] = (int)axtoul(arguments);
if (arguments = net_sms_nextarg(arguments))
data = atol(arguments);
if (!arguments) {
s = stp_rom(s, "ERROR: Too few args");
}
else {
if (cmd.id == cmdWriteOnly) {
// WRITEONLY:
// write new value:
if (err = writesdo(arg[0], arg[1], data)) {
s = vehicle_twizy_fmt_err(s, err);
}
else {
s = stp_rom(s, "OK: ");
s = vehicle_twizy_fmt_sdo(s);
}
}
else {
// READ-WRITE:
// read old value:
if (err = readsdo(arg[0], arg[1])) {
s = vehicle_twizy_fmt_err(s, err);
}
else {
// read ok:
s = stp_rom(s, "OLD:");
s = vehicle_twizy_fmt_sdo(s);
s = stp_ul(s, " = ", twizy_sdo.data);
// write new value:
if (err = writesdo(arg[0], arg[1], data))
s = vehicle_twizy_fmt_err(s, err);
else
s = stp_ul(s, " => NEW: ", data);
}
}
}
go_op_onexit = false;
break;
case cmdDiagAddress:
// DA tx_id_hex rx_id_hex
if (arguments = net_sms_nextarg(arguments))
arg[0] = (int)axtoul(arguments);
if (arguments = net_sms_nextarg(arguments))
arg[1] = (int)axtoul(arguments);
if (!arguments) {
s = stp_rom(s, "ERROR: Too few args");
}
else {
tpMsg.tx_id = arg[0];
tpMsg.rx_id = arg[1];
CAN.init_Filt(1, 0, tpMsg.rx_id << 16);
s = stp_rom(s, "OK");
}
go_op_onexit = false;
break;
case cmdDiagRequest:
// DR hexstring
if (arguments = net_sms_nextarg(arguments)) {
if (strlen(arguments) & 1) {
// input length is odd:
s = stp_rom(s, "ERROR: invalid hex string length");
}
else {
// parse hexstring into scratchpad:
t = (char *) net_msg_scratchpad;
maps[2] = 0;
while (arguments[0]) {
maps[0] = arguments[0];
maps[1] = arguments[1];
*t++ = axtoul(maps);
arguments += 2;
}
}
}
if (!arguments) {
s = stp_rom(s, "ERROR: Too few args");
}
else {
// send request:
tpMsg.Buffer = (uint8_t *)net_msg_scratchpad;
tpMsg.len = (t - (char *)net_msg_scratchpad);
if (err = isotp.send(&tpMsg)) {
s = stp_i(s, "ERROR: isotp.send error code ", err);
}
else {
// read response into scratchpad:
tpMsg.Buffer = (uint8_t *)net_msg_scratchpad;
tpMsg.len = 0;
isotp.receive(&tpMsg);
if (tpMsg.tp_state != ISOTP_FINISHED) {
s = stp_i(s, "ERROR: isotp.receive error code ", tpMsg.tp_state);
}
else {
// output response as hexstring:
Serial.print(net_scratchpad);
net_scratchpad[0] = 0;
for (i=0; i<tpMsg.len; i++) {
if ((byte)net_msg_scratchpad[i] < 0x10)
Serial.print(F("0"));
Serial.print((byte)net_msg_scratchpad[i], HEX);
}
break;
}
}
}
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 ((BYTE) 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;
}
//
// FINISH:
//
// go operational?
if (!err && go_op_onexit)
configmode(0);
Serial.println(net_scratchpad);
return true;
}
// --------------------------------------------------------------------
// MAIN
//
String inputString = ""; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
void setup() {
Serial.begin(1000000);
while (!Serial) {
; // wait for serial port to connect. Needed for native USB port only
}
// reserve 200 bytes for the inputString:
inputString.reserve(200);
//
// Init Twizy CAN interface
//
while (CAN.begin(MCP_STDEXT, CAN_500KBPS, TWIZY_CAN_MCP_FREQ) != CAN_OK) {
Serial.println(F("setup: waiting for CAN connection..."));
delay(1000);
}
// Set filters:
CAN.init_Mask(0, 0, 0x07FF0000);
CAN.init_Filt(0, 0, 0x05810000); // CANopen response node 1
CAN.init_Filt(1, 0, 0x00000000);
CAN.init_Mask(1, 0, 0x07FF0000);
CAN.init_Filt(2, 0, 0x00000000);
CAN.init_Filt(3, 0, 0x00000000);
CAN.init_Filt(4, 0, 0x00000000);
CAN.init_Filt(5, 0, 0x00000000);
CAN.setMode(MCP_NORMAL);
// Init tuning module:
vehicle_twizy_init();
// Output info & prompt:
exec((char *) "?");
Serial.print("\n> ");
}
void loop() {
if (stringComplete) {
// execute command:
Serial.println(inputString);
exec((char *) inputString.c_str());
Serial.print("\n> ");
// clear the string:
inputString = "";
stringComplete = false;
}
}
/*
SerialEvent occurs whenever a new data comes in the
hardware serial RX. This routine is run between each
time loop() runs, so using delay inside loop can delay
response. Multiple bytes of data may be available.
*/
void serialEvent() {
while (Serial.available()) {
char inChar = (char)Serial.read();
if (inChar == '\r' || inChar == '\n') {
stringComplete = (inputString.length() > 0);
}
else if (inChar >= 32) {
inputString += inChar;
}
}
}
Reference in a new issue View git blame Copy permalink