File content as of revision 252:38644631ed97:

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

#include "globals.h"
#include "errors.h"
#include "prefs.h"

#include "tables.h"
#include "defaults.h"
#include "derived.h"

void cmd_ls(Serial *pc) {
    if (control.enabled) {
        pc->printf("%s\n", "Turn motor off first");
        return;
    }
    
    DPRINT(PREFS_VALID);
    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);
    FPRINT2(KP_D);
    FPRINT2(KI_D);
    FPRINT2(KP_Q);
    FPRINT2(KI_Q);
    FPRINT(F_SLOW_LOOP);
    FPRINT(INTEGRAL_MAX);
    FPRINT(OVERMODULATION_FACTOR);
    FPRINT(V_PHASE_SWIZZLE);
    DPRINT(W_FILTER_WINDOW);
    FPRINT(DQ_FILTER_STRENGTH);
    DPRINT(THROTTLE_FILTER_WINDOW);
    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);
    DPRINT2(ROWS);
    DPRINT2(COLUMNS);
    FPRINT(W_STEP);
    DPRINT(ENABLE_LOGGING);
    DPRINT(LOG_PAGE_SIZE);
    DPRINT(LOG_HEADER_SIZE);
    DPRINT(LOG_PACKET_SIZE);
    DPRINT(LOG_BAUD_RATE);
    FPRINT(K_LOOP_W);
    FPRINT(KI_BASE_W);
    FPRINT(W_SETPOINT_MAX);
    FPRINT(W_LOOP_MAX_TQ);
}

#define ls_specialf(a) if (strcmp(buf, #a) == 0) {pc->printf("%s: %f\n", #a, a); return;}
#define ls_speciald(a) if (strcmp(buf, #a) == 0) {pc->printf("%s: %d\n", #a, a); return;}
void cmd_ls2(Serial *pc, char *buf) {    
    if (strcmp(buf, "mode") == 0) {
        pc->printf("%s\n", mode_to_str(BREMS_mode));
        return;
    }
    if (strcmp(buf, "src") == 0) {
        pc->printf("%s\n", src_to_str(BREMS_src));
        return;
    }
    if (strcmp(buf, "op") == 0) {
        pc->printf("%s\n", op_to_str(BREMS_op));
        return;
    }
    
    ls_specialf(KP_D);
    ls_specialf(KP_Q);
    ls_specialf(KP_W);
    ls_specialf(KI_D);
    ls_specialf(KI_Q);
    ls_specialf(KI_W);
    
    ls_speciald(ROWS);
    ls_speciald(COLUMNS);
    
    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) {
    if (BREMS_mode == MODE_RUN || BREMS_mode == MODE_ZERO || BREMS_mode == MODE_CHR) {
        pc->printf("%s\n", "Can only do that in CFG mode");
        return;
    }
    
    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(OVERMODULATION_FACTOR);
    DEFAULT(V_PHASE_SWIZZLE);
    DEFAULT(W_FILTER_WINDOW);
    DEFAULT(DQ_FILTER_STRENGTH);
    DEFAULT(THROTTLE_FILTER_WINDOW);
    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(W_STEP);
    DEFAULT(ENABLE_LOGGING);
    DEFAULT(LOG_PAGE_SIZE);
    DEFAULT(LOG_HEADER_SIZE);
    DEFAULT(LOG_PACKET_SIZE);
    DEFAULT(LOG_BAUD_RATE);
    DEFAULT(K_LOOP_W);
    DEFAULT(KI_BASE_W);
    DEFAULT(W_SETPOINT_MAX);
    DEFAULT(W_LOOP_MAX_TQ);
    pc->printf("Defaults Loaded\n");
}

void cmd_reload(Serial *pc, PreferenceWriter *pref) {
    if (BREMS_mode == MODE_RUN || BREMS_mode == MODE_ZERO || BREMS_mode == MODE_CHR) {
        pc->printf("%s\n", "Can only do that in CFG mode");
        return;
    }
    
    pref->load();
    if (_PREFS_VALID) 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));
    if (nptr != NULL || fptr != NULL) cmd_ls2(pc, buf);
    if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
}

void cmd_flush(Serial *pc, PreferenceWriter *pref) {
    if (control.user_cmd != 0.0f || control.torque_percent != 0.0f) {
        pc->printf("%s\n", "Turn off motor first");
        return;
    }
    io.logger->disable();
    if (!pref->ready()) pref->open();
    _PREFS_VALID = 1;
    pref->flush();
    io.logger->enable();
}

void cmd_query_err(Serial *pc) {
    if ((moded_errors & (1 << ERR_THROTTLE_DISABLED)) != 0) pc->printf("%s\n", "Throttle disabled");
    if ((moded_errors & (1 << ERR_POS_INVALID)) != 0) pc->printf("%s\n", "No encoder index");
    if ((moded_errors & (1 << ERR_NOT_DRIVING)) != 0) pc->printf("%s\n", "Not driving");
    if (moded_errors == 0) pc->printf("%s\n", "No errors");
}

void cmd_query(Serial *pc, char *buf) {
    if (strcmp(buf, "pos") == 0) pc->printf("%f\n", foc.p);
    else if (strcmp(buf, "w") == 0) pc->printf("%f\n", read.w);
    else if (strcmp(buf, "cmd") == 0) pc->printf("%f\n", control.user_cmd);
    else if (strcmp(buf, "d") == 0) pc->printf("%f\n", foc.d);
    else if (strcmp(buf, "q") == 0) pc->printf("%f\n", foc.q);
    else if (strcmp(buf, "d_ref") == 0) pc->printf("%f\n", control.d_ref);
    else if (strcmp(buf, "q_ref") == 0) pc->printf("%f\n", control.q_ref);
    else if (strcmp(buf, "vd") == 0) pc->printf("%f\n", foc.vd);
    else if (strcmp(buf, "vq") == 0) pc->printf("%f\n", foc.vq);
    else if (strcmp(buf, "tq") == 0) pc->printf("%f\n", control.torque_percent);
    else if (strcmp(buf, "ia") == 0) pc->printf("%f\n", foc.ia);
    else if (strcmp(buf, "ib") == 0) pc->printf("%f\n", foc.ib);
    else if (strcmp(buf, "ic") == 0) pc->printf("%f\n", -foc.ia - foc.ib);
    else if (strcmp(buf, "errors") == 0) cmd_query_err(pc);
    else pc->printf("%s\n", "No such parameter");
}

void cmdf_w(Serial *pc) {
    union {int16_t s; uint16_t u;};
    s = (int)(read.w);
    uint8_t h, l;
    h = (u & 0xff00) >> 8;
    l = (u & 0x00ff);
    pc->putc(h);
    pc->putc(l);
}

void cmdf_data(Serial *pc) {
    char packet[8];
    
    packet[0] = (char)(read.w / 8.0f);
    packet[1] = (char)(control.d_ref / 2.0f + 128.0f);
    packet[2] = (char)(control.d_filtered / 2.0f + 128.0f);
    packet[3] = (char)(control.q_ref / 2.0f + 128.0f);
    packet[4] = (char)(control.q_filtered / 2.0f + 128.0f);
    packet[5] = (char)(255.0f * control.torque_percent);
    packet[6] = (char)(255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f);
    packet[7] = (char)(255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f);
    
    pc->putc(packet[0]); pc->putc(packet[1]); pc->putc(packet[2]); pc->putc(packet[3]);
    pc->putc(packet[4]); pc->putc(packet[5]); pc->putc(packet[6]); pc->putc(packet[7]);
}