File content as of revision 2:cfc39b0843ae:

#include "mbed.h"
#include "CommandProcessor.h"
#include "PreferenceWriter.h"
#include "globals.h"

#include "config.h"
#include "config_driving.h"
#include "config_inverter.h"
#include "config_logging.h"
#include "config_loop.h"
#include "config_motor.h"
#include "config_pins.h"
#include "config_table.h"

void cmd_ls(Serial *pc) {
    DPRINT(TORQUE_MODE);
    FPRINT(MAX_TQPCT_PLUS);
    FPRINT(MAX_TQPCT_MINUS);
    FPRINT(TORQUE_MAX);
    FPRINT(W_MAX);
    FPRINT(BUS_VOLTAGE);
    FPRINT(F_SW);
    FPRINT(K_LOOP_D);
    FPRINT(KI_BASE_D);
    FPRINT(K_LOOP_Q);
    FPRINT(KI_BASE_Q);
    FPRINT(F_SLOW_LOOP);
    FPRINT(INTEGRAL_MAX);
    FPRINT(W_FILTER_STRENGTH);
    FPRINT(DQ_FILTER_STRENGTH);
    FPRINT(THROTTLE_FILTER_STRENGTH);
    FPRINT(KP_D);
    FPRINT(KI_D);
    FPRINT(KP_Q);
    FPRINT(KI_Q);
    FPRINT(SLOW_LOOP_COUNTER);
    FPRINT(POLE_PAIRS);
    FPRINT(POS_OFFSET);
    FPRINT(RESOLVER_LOBES);
    DPRINT(CPR);
    FPRINT(Ld);
    FPRINT(Lq);
    FPRINT(FLUX_LINKAGE);
    FPRINT(Rs);
    FPRINT(KT);
    FPRINT(W_SAFE);
    FPRINT(W_CRAZY);
    DPRINT(TH_LIMIT_LOW);
    DPRINT(TH_LIMIT_HIGH);
    DPRINT(TH_LIMIT_CRAZY);
    DPRINT(ROWS);
    DPRINT(COLUMNS);
    FPRINT(W_STEP);
}

void cmd_ls2(Serial *pc, char *buf) {
    float *fptr = checkf(buf);
    if (fptr != NULL) pc->printf("%s: %f\n", buf, *fptr);
    int *nptr = NULL;
    if (fptr == NULL) nptr = checkn(buf);
    if (nptr != NULL) pc->printf("%s: %d\n", buf, *nptr);
    if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
}

void cmd_defaults(Serial *pc) {
    DEFAULT(TORQUE_MODE);
    DEFAULT(MAX_TQPCT_PLUS);
    DEFAULT(MAX_TQPCT_MINUS);
    DEFAULT(TORQUE_MAX);
    DEFAULT(W_MAX);
    DEFAULT(BUS_VOLTAGE);
    DEFAULT(F_SW);
    DEFAULT(K_LOOP_D);
    DEFAULT(KI_BASE_D);
    DEFAULT(K_LOOP_Q);
    DEFAULT(KI_BASE_Q);
    DEFAULT(F_SLOW_LOOP);
    DEFAULT(INTEGRAL_MAX);
    DEFAULT(W_FILTER_STRENGTH);
    DEFAULT(DQ_FILTER_STRENGTH);
    DEFAULT(THROTTLE_FILTER_STRENGTH);
    DEFAULT(KP_D);
    DEFAULT(KI_D);
    DEFAULT(KP_Q);
    DEFAULT(KI_Q);
    DEFAULT(SLOW_LOOP_COUNTER);
    DEFAULT(POLE_PAIRS);
    DEFAULT(POS_OFFSET);
    DEFAULT(RESOLVER_LOBES);
    DEFAULT(CPR);
    DEFAULT(Ld);
    DEFAULT(Lq);
    DEFAULT(FLUX_LINKAGE);
    DEFAULT(Rs);
    DEFAULT(KT);
    DEFAULT(W_SAFE);
    DEFAULT(W_CRAZY);
    DEFAULT(TH_LIMIT_LOW);
    DEFAULT(TH_LIMIT_HIGH);
    DEFAULT(TH_LIMIT_CRAZY);
    DEFAULT(ROWS);
    DEFAULT(COLUMNS);
    DEFAULT(W_STEP);
    pc->printf("Defaults Loaded\n");
}

void cmd_reload(Serial *pc, PreferenceWriter *pref) {
    pref->load();
    pc->printf("Flash Values Loaded\n");
}

void cmd_set(Serial *pc, char *buf, char *val) {
    float *fptr = checkf(buf);
    if (fptr != NULL) *fptr = (float) (atof(val));
    int *nptr = NULL;
    if (fptr == NULL) nptr = checkn(buf);
    if (nptr != NULL) *nptr = (int) (atoi(val));
    cmd_ls2(pc, buf);
    if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
}

void cmd_flush(Serial *pc, PreferenceWriter *pref) {
    if (!pref->ready()) pref->open();
    pref->flush();
}

void cmd_cfg(Serial *pc) {
    mode = MODE_CFG;
    pc->printf("%s\n", "Config mode ON");
}

void cmd_exit(Serial *pc) {
    mode = MODE_RUN;
    pc->printf("%s\n", "Run mode ON");
}

void processCmd(Serial *pc, PreferenceWriter *pref, char *buf) {
    char *tokens[10];
    int len = tokenize(buf, tokens, 10);
    
    if (mode == MODE_RUN) {
        if (len == 1 && strcmp(tokens[0], "cfg") == 0) {
            cmd_cfg(pc);
        } else {
            return;
        }
    }
    
    switch (len) {
    case 1:
        if (strcmp(tokens[0], "ls") == 0) cmd_ls(pc);
        if (strcmp(tokens[0], "defaults") == 0) cmd_defaults(pc);
        if (strcmp(tokens[0], "reload") == 0) cmd_reload(pc, pref);
        if (strcmp(tokens[0], "load") == 0) cmd_reload(pc, pref);
        if (strcmp(tokens[0], "flush") == 0) cmd_flush(pc, pref);
        if (strcmp(tokens[0], "exit") == 0) cmd_exit(pc);
        break;
    case 2:
        if (strcmp(tokens[0], "ls") == 0) cmd_ls2(pc, tokens[1]);
        if (strcmp(tokens[0], "get") == 0) cmd_ls2(pc, tokens[1]);
        break;
    case 3:
        if (strcmp(tokens[0], "set") == 0) cmd_set(pc, tokens[1], tokens[2]);
    default:
        break;
    }
}

int tokenize(char *buf, char **out, int max) {
    char* tok;
    int k = 0;
    
    tok = strtok(buf, " ");
    
    while(tok != NULL && k < max) {
        out[k] = tok;
        k++;
        tok = strtok(NULL, " ");
    }
    return k;
}

#define __check(x) if(strcmp(s, #x) == 0) return &_##x
float* checkf(char *s) {
    __check(MAX_TQPCT_PLUS);
    __check(MAX_TQPCT_MINUS);
    __check(TORQUE_MAX);
    __check(W_MAX);
    __check(BUS_VOLTAGE);
    __check(F_SW);
    __check(K_LOOP_D);
    __check(KI_BASE_D);
    __check(K_LOOP_Q);
    __check(KI_BASE_Q);
    __check(F_SLOW_LOOP);
    __check(INTEGRAL_MAX);
    __check(W_FILTER_STRENGTH);
    __check(DQ_FILTER_STRENGTH);
    __check(THROTTLE_FILTER_STRENGTH);
    __check(KP_D);
    __check(KI_D);
    __check(KP_Q);
    __check(KI_Q);
    __check(POLE_PAIRS);
    __check(POS_OFFSET);
    __check(RESOLVER_LOBES);
    __check(Ld);
    __check(Lq);
    __check(FLUX_LINKAGE);
    __check(Rs);
    __check(KT);
    __check(W_SAFE);
    __check(W_CRAZY);
    __check(W_STEP);
    return NULL;
}

int* checkn(char *s) {
    __check(TORQUE_MODE);
    __check(CPR);
    __check(TH_LIMIT_LOW);
    __check(TH_LIMIT_HIGH);
    __check(TH_LIMIT_CRAZY);
    __check(ROWS);
    __check(COLUMNS);
    return NULL;
}