File content as of revision 233:2ca26ab47b4f:

#include "mbed.h"
#include "math.h"

#include "PositionSensor.h"
#include "FastPWM.h"
#include "PwmIn.h"
#include "MathHelpers.h"
#include "Transforms.h"
#include "DQMapper.h"
#include "ThrottleMapper.h"
#include "Calibration.h"
#include "Filter.h"
#include "LedBlinker.h"

#include "BREMSStructs.h"
#include "BREMSConfig.h"

#include "prefs.h"
#include "hardware.h"
#include "derived.h"
#include "main.h"

#include "errors.h"

IOStruct io;
ReadDataStruct read;
FOCStruct foc;
ControlStruct control;

DQMapper *dq;
ThrottleMapper *th;

int loop_counter = 0;

void commutate() {  
    /*safety checks, do we do anything this cycle?*/
    if (checks_passed()) {
        go_enabled();
    }
    
    /*update velocity, references*/
    update_velocity();
    if (loop_counter % SLOW_LOOP_COUNTER == 0) {
        loop_counter = 0;
        slow_loop();
    }
    loop_counter++;
    io.blink->update();
        
    /*update position, sin, cos*/
    foc.p = io.pos->GetElecPosition() - _POS_OFFSET;
    float sin_p = sinf(foc.p);
    float cos_p = cosf(foc.p);
    
    /*scale and offset currents (adval1, 2 are updated in ISR)*/    
    foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ad1_supp_offset) / I_SCALE;
    foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ad2_supp_offset) / I_SCALE;
    
    /*compute d, q*/
    clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
    park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
    
    /*PI controller*/
    control.d_filtered = update_filter(control.d_filtered, foc.d, _DQ_FILTER_STRENGTH);
    control.q_filtered = update_filter(control.q_filtered, foc.q, _DQ_FILTER_STRENGTH);
        
    float d_err = control.d_ref - control.d_filtered;
    float q_err = control.q_ref - control.q_filtered;
    
    control.d_integral += d_err * KI_D;
    control.q_integral += q_err * KI_Q;
    
    constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, _INTEGRAL_MAX);
    
    foc.vd_decouple = -_Lq * _POLE_PAIRS * read.w * foc.q / _BUS_VOLTAGE / 2.0f;
    foc.vq_decouple = _Ld * _POLE_PAIRS * read.w * foc.d / _BUS_VOLTAGE / 2.0f;
    
    constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f);
    
    foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple;
    foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple;
    
    constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + _OVERMODULATION_FACTOR);
    
    float pv = foc.p + read.w / _V_PHASE_SWIZZLE;
    float sin_pv = sinf(pv);
    float cos_pv = cosf(pv);
    
    /*inverse transforms*/
    invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta);
        
    /*set outputs*/
    float va, vb, vc, voff;
    
    invclarke(foc.valpha, foc.vbeta, &va, &vb);
    vc = -va - vb;
    
    /*SVPWM*/
    voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
    va = va - voff;
    vb = vb - voff;
    vc = vc - voff;
    
    /*safety checks, reset integral*/
    if (!checks_passed() || !mode_enables_output()) {
        go_disabled();
    }
        
    /*log data*/
    if (_ENABLE_LOGGING && mode_enables_logging()) log();
    
    /*disable outputs if necessary*/
    if (!control.enabled) {
        va = 0.0f; vb = 0.0f; vc = 0.0f;
    }
    
    //output to timers (some of the calibration modes may want to override this)
    if (!mode_overrides_timers()) {
        set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
        set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
        set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
    }
}

void slow_loop() {
    switch (BREMS_src) {
    case CMD_SRC_RC:
        control.user_cmd = control.throttle_filter->update(io.throttle_in->get_throttle());
        break;
    case CMD_SRC_ANALOG:
        //handle analog throttle here
        break;
    case CMD_SRC_TERMINAL:
    case CMD_SRC_SERIAL:
    case CMD_SRC_CAN:
    case CMD_SRC_INTERNAL:
        //these sources are updated by callbacks
        break;
    default:
        //this should never happen!
        control.user_cmd = 0.0f;
        break;
    }

    control.torque_percent = th->map(control.user_cmd, read.w);
    dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
}

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

int main() {    
    dq = new InterpolatingLutMapper();
    th = new LimitingThrottleMapper(1500.0f);

    BREMSInit(&io, &read, &foc, &control, false);
    io.pc->attach(rxCallback);
        
    for (;;) {
        io.logger->flush();
    }
}