File content as of revision 1:90487a39d54e:

#include "mbed.h"
#include "radioEmulator.h"

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

Timer timer;
int begin, end;
Serial pc(USBTX, USBRX); // tx, rx

#undef CHECK_HW_SHUTDOWN

//LocalFileSystem local("local");
#include "SDFileSystem.h"

SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board

char RadioEmulator::unlock[6] = {0x03,0x02,0x00,0x40,0x87,0xa5};
char RadioEmulator::lock[6] = {0x01, 0x02, 0x00, 0x40, 0x87, 0xa5};
char RadioEmulator::trunk[6] = {0x05, 0x02, 0x00, 0x40, 0x87, 0xa5};

RadioEmulator::RadioEmulator()
{
    ethBusy = false;
    eth = new EthernetNetIf(IpAddr(10,10,10,2), IpAddr(255,255,255,0), IpAddr(10,10,10,1), IpAddr(10,10,10,1));
    EthernetErr ethErr = eth->setup();
    if(ethErr)
    {
        printf("Error %d in setup.\n", ethErr);
        return;
    }
    printf("Setup OK\r\n");
    printf("Size of radiostate = %d\r\n", sizeof(RadioState));
    
    multicast = new Host(IpAddr(239, 192, 1, 100), 50000, NULL); //Join multicast group on port 50000
    
    udp.setOnEvent(this, &RadioEmulator::onUDPSocketEvent);
    
    udp.bind(*multicast);
        
    can2 = new CAN(p30,p29);
    can_RS = new DigitalOut(p28);
    canIRQ = new InterruptIn(p27);

    serialCounter = 0;
    prevSWC = 0;
    radioOn = false;
    for (int i = 0; i < 8; i++)
    {
        for (int j = 0; j < 8; j++)
        {
            memset(siriusText[i][j], 0, 8);
        }
    }
    
    printf("starting\r\n");
    
    memset(&status, 0, sizeof(status));  
    status.marker1 = 0x42;  
    status.marker2 = 0x42;  
    status.marker3 = 0x42;  
    status.marker4 = 0x42;  
    status._radioMode = SAT;
//    readInitFile();
    status._volume = 10;
    status._bass = 15;
    status._mid = 13;
    status._treble = 14;
    status._balance = 10;
    status._fade = 10;

    for (int i = 0; i < 8; i++)
    {
        sprintf(status._siriusTextLine[i], "Fun line text # %d", i);
    }

    PowerUp();
    
    ReceivedHostMsg = false;
    statusTicker.attach(this, &RadioEmulator::SendStatusToHost, 0.1);

    opMode = standalone;
    HostTimeout.attach(this, &RadioEmulator::CheckHostTimeout, 1);
    
    printf("Initialized\n\r");
}

void RadioEmulator::readInitFile()
{
    FILE *fp = fopen("/sd/stereo.txt", "r");  // Open "out.txt" on the local file system for writing
    char temp[100];
        
    while ( fscanf(fp, "%s", temp) > 0)
    {
        if (strcmp(temp, "volume") == 0)
        {
            fscanf(fp, "%d", &status._volume);
        }
        if (strcmp(temp, "bass") == 0)
        {
            fscanf(fp, "%d", &status._bass);
        }
        if (strcmp(temp, "mid") == 0)
        {
            fscanf(fp, "%d", &status._mid);
        }
        if (strcmp(temp, "treble") == 0)
        {
            fscanf(fp, "%d", &status._treble);
        }
        if (strcmp(temp, "balance") == 0)
        {
            fscanf(fp, "%d", &status._balance);
        }
        if (strcmp(temp, "fade") == 0)
        {
            fscanf(fp, "%d", &status._fade);
        }
        if (strcmp(temp, "MAC") == 0)
        {
            char temp2[64];
            fscanf(fp, "%s", temp2);
            char *pEnd;
            hostMACAddress[0] = strtoul(temp2, &pEnd, 16);
            hostMACAddress[1] = strtoul(pEnd, &pEnd, 16);
            hostMACAddress[2] = strtoul(pEnd, &pEnd, 16);
            hostMACAddress[3] = strtoul(pEnd, &pEnd, 16);
            hostMACAddress[4] = strtoul(pEnd, &pEnd, 16);
            hostMACAddress[5] = strtoul(pEnd, &pEnd, 16);
        }
    }
    
    fclose(fp);
}

void RadioEmulator::writeInitFile()
{
    FILE *fp = fopen("/sd/stereo.txt", "w");  // Open "out.txt" on the local file system for writing

    fprintf(fp,"volume %d\r\n", status._volume);
    fprintf(fp,"bass %d\r\n", status._bass);
    fprintf(fp,"mid %d\r\n", status._mid);
    fprintf(fp,"treble %d\r\n", status._treble);
    fprintf(fp,"balance %d\r\n", status._balance);
    fprintf(fp,"fade %d\r\n", status._fade);
    fclose(fp);
}

void RadioEmulator::WriteCANMessages()
{
    if (radioOn)
    {
        led2 = !led2;
        SendRadioModeMsg();
        SendEVICMsg();
        SendStereoSettingsMsg();
        SendHostMessages();
    }
}

void RadioEmulator::SendOnMsg()
{
    CANMessage msg;
    msg.id = 0x416;
    msg.len = 8;
    char temp[8] = {0xfe, 0x1b, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff};
    memcpy(msg.data, temp, 8);
    can2->write(msg);
}

void RadioEmulator::SendRadioModeMsg()
{
    CANMessage msg;
    msg.id = 0x09F;
    msg.len = 8;

    msg.data[7] = 0x0f;
    msg.data[6] = 0xff;
    msg.data[5] = 0xff;
    msg.data[4] = 0xff;
    msg.data[3] = 0x07;
    msg.data[2] = 0x00;
    msg.data[1] = 0x00;
    msg.data[0] = 0x00;

    if (status._radioMode == AM)
    {
        if (status._amPreset != 0)
        {
            msg.data[0] = (status._amPreset + 1) << 4;
        }
        msg.data[1] = (status._amFreq & 0xFF00) >> 8;
        msg.data[2] = (status._amFreq & 0x00FF);
    }
    else if (status._radioMode == FM)
    {
        if (status._fmPreset != 0)
        {
            msg.data[0] = (status._fmPreset + 1) << 4;
        }
        msg.data[0] |= 0x01;
        msg.data[1] = (status._fmFreq & 0xFF00) >> 8;
        msg.data[2] = (status._fmFreq & 0x00FF);
    }
    else if (status._radioMode == CD)
    {
        msg.data[0] = status._cdNum << 4;
        msg.data[1] = 0x20;
        msg.data[0] |= 0x03;
        msg.data[2] = status._cdTrackNum;
        msg.data[4] = status._cdHours;
        msg.data[5] = status._cdMinutes;
        msg.data[6] = status._cdSeconds;
    }
    else if (status._radioMode == SAT)
    {
        if (status._siriusPreset != 0)
        {
            msg.data[0] = (status._siriusPreset + 1) << 4;
        }
        msg.data[0] |= 0x04;
        msg.data[1] = 0;
        msg.data[2] = status._siriusChan;
    }
    else if (status._radioMode == VES)
    {
        msg.data[0] = 0x16;
        msg.data[1] = 0x10;
        msg.data[2] = 0x01;
    }

    msg.data[1] |= 0x10;

    can2->write(msg);
}

void RadioEmulator::SendEVICMsg()
{
    CANMessage msg;
    msg.id = 0x394;
    msg.len = 6;
    
    memset(msg.data, 0, 8);

    if (status._radioMode == AM)
    {
        if (status._amPreset != 0)
        {
            msg.data[0] = (status._amPreset + 1) << 4;
        }
        msg.data[1] = (status._amFreq & 0xFF00) >> 8;
        msg.data[2] = (status._amFreq & 0x00FF);
    }
    else
    {
        if (status._fmPreset != 0)
        {
            msg.data[0] = (status._fmPreset + 1) << 4;
        }
        msg.data[0] |= 0x01;
        msg.data[1] = (status._fmFreq & 0xFF00) >> 8;
        msg.data[2] = (status._fmFreq & 0x00FF);
    }

    can2->write(msg);
}

void RadioEmulator::SendStereoSettingsMsg()
{
    CANMessage msg;
    msg.id = 0x3D0;
    msg.len = 7;

    msg.data[0] = status._volume;
    msg.data[1] = status._balance;
    msg.data[2] = status._fade;
    msg.data[3] = status._bass;
    msg.data[4] = status._mid;
    msg.data[5] = status._treble;

    can2->write(msg);
}

void RadioEmulator::SendHostMessages()
{
    if (hostMessages.size() > 0)
    {
        can2->write(hostMessages.front());
        
        hostMessages.pop_front();
    }
}

void RadioEmulator::ChangeSiriusStation(int station, bool turn_on)
{
    CANMessage msg;
    msg.id = 0x3B0;
    msg.len = 6;

    if (turn_on)
    {
        msg.data[0] = 21;
    }
    else
    {
        msg.data[0] = 23;
    }
    msg.data[1] = station;

    can2->write(msg);

    memset(msg.data, 0, 8);
    msg.data[1] = station;

    can2->write(msg);
    
    status._siriusChan = station;
}

void RadioEmulator::ParseCANMessage(CANMessage can_MsgRx)
{
    if (can_MsgRx.id == 0x000)
    {
        if (can_MsgRx.data[0] > 1)
        {
            radioOn = true;
        }
        else
        {
            radioOn = false;
        }
    }

    // this message seems to be a message requesting all other devices
    // to start announcing their presence
    if ((can_MsgRx.id >= 0x400) && (can_MsgRx.data[0] == 0xfd))
    {
//      SendOnMsg();
    }
    
    if (can_MsgRx.id == 0x012)
    {
        if (memcmp(can_MsgRx.data, unlock, 6) == 0)
        {
        }
        else if (memcmp(can_MsgRx.data, lock, 6) == 0)
        {
        }
        else if (memcmp(can_MsgRx.data, trunk, 6) == 0)
        {
        }
    }

    if (can_MsgRx.id == 0x1bd)
    {
        // SDAR status
        
        if (status._siriusChan == 0)
        {
            status._siriusChan = can_MsgRx.data[1];
        }

        if (can_MsgRx.data[0] == 0x85)
        {
            ChangeSiriusStation(status._siriusChan, true);
        }
        
        if (status._siriusChan != can_MsgRx.data[1])
        {
            ChangeSiriusStation(status._siriusChan, true);
        }
    }
    
    if (can_MsgRx.id == 0x3bd)
    {
        ReadSiriusText((char *)can_MsgRx.data);
    }

    if (can_MsgRx.id == 0x3a0)
    {        
        // note = 0x01
        // volume up = 0x02
        // volume down = 0x04
        // up arrow = 0x08
        // down arrow = 0x10
        // right arrow = 0x20
               
        status.SWCButtons = can_MsgRx.data[0];
    }

    if (can_MsgRx.id == 0x000)
    {
        status._keyPosition = can_MsgRx.data[0];
    }
    else if (can_MsgRx.id == 0x002)
    {
        status._rpm = (can_MsgRx.data[0] << 8) + can_MsgRx.data[1];
        status._speed = ((can_MsgRx.data[2] << 8) + can_MsgRx.data[3]) >> 7;
    }
    else if (can_MsgRx.id == 0x003)
    {
        status._brake = can_MsgRx.data[3] & 0x01;
        status._gear = can_MsgRx.data[4];
    }
    else if (can_MsgRx.id == 0x15)
    {
        status._batteryVoltage = (float)(can_MsgRx.data[1]) / 10;
    }
    else if (can_MsgRx.id == 0x01b)
    {
        // vin number
        int part = can_MsgRx.data[0];
        if (part < 3)
        {
            for (int i = 1; i < 8; i++)
            {
                status._vin[(part*8) + i-1] = can_MsgRx.data[i];
            }
        }
    }
    else if (can_MsgRx.id == 0x0d0)
    {
        if (can_MsgRx.data[0] == 0x80)
        {
            status._parkingBrake = true;
        }
        else
        {
            status._parkingBrake = false;
        }                    
    }
    else if (can_MsgRx.id == 0x0EC)
    {
        if ((can_MsgRx.data[0] & 0x40) == 0x40)
        {
            status._fanRequested = true;
        }
        else
        {
            status._fanRequested = false;
        }

        if ((can_MsgRx.data[0] & 0x01) == 0x01)
        {
            status._fanOn = true;
        }
        else
        {
            status._fanOn = false;
        }
    }
    else if (can_MsgRx.id == 0x159)
    {
        status._fuel = can_MsgRx.data[5];
    }
    else if (can_MsgRx.id == 0x1a2)
    {
        if ((can_MsgRx.data[0] & 0x80) == 0x80)
        {
            status._rearDefrost = true;
        }
        else
        {
            status._rearDefrost = false;
        }

        if ((can_MsgRx.data[0] & 0x40) == 0x40)
        {
            status._fanRequested = true;
        }
        else
        {
            status._fanRequested = false;
        }

        if ((can_MsgRx.data[0] & 0x01) == 0x01)
        {
            status._fanOn = true;
        }
        else
        {
            status._fanOn = false;
        }
    }
    else if (can_MsgRx.id == 0x1c8)
    {
        status._headlights = can_MsgRx.data[0];
    }
    else if (can_MsgRx.id == 0x210)
    {
        status._dimmerMode = can_MsgRx.data[0];
        if (can_MsgRx.data[0] == 0x03)
        {
            status._dimmer = -1;
        }
        else if (can_MsgRx.data[0] == 0x02)
        {
            status._dimmer = can_MsgRx.data[1];
        }
    }

}

void RadioEmulator::ReadSiriusText(char *data)
{
    int num = (data[0] & 0xF0) >> 4;
    if (num > 7)
    {
        return;
    }
    
    if ((data[0] & 0x01) == 1)
    {
        for (int i = 0; i < 8; i++)
        {
            memset(siriusText[num][i], 0, 8);
        }
    }
    int part = (data[0] & 0x0E) >> 1;
    if (part > 7)
    {
        return;
    }
    
    memset(siriusText[num][part], 0, 8);
    char tempString[8] = "";
    memset(tempString,0,8);
    
    for (int i = 1; i < 8; i++)
    {
        tempString[i-1] = data[i];
    }
    
    strncpy(siriusText[num][part],tempString,7);

    int cls = (data[0] & 0x0F) >> 1;
    if (cls - 1 == 0)
    {
        for (int i = 0; i < 8; i++)
        {
            memset(status._siriusTextLine[i], 0, 64);
            for (int j = 0; j < 8; j++)
            {
                strcat(status._siriusTextLine[i], siriusText[i][j]);
            }
            
            printf("%d: %s\r\n", i, status._siriusTextLine[i]);
        }
    }
}

void RadioEmulator::readCANbus(void)
{
    if (can2->read(can_MsgRx))
    {
        led3 = !led3;
        needToParseCANMessage = true;
        ReceivedCANMsg = true;
        
        
        char buffer[11];
        buffer[0] = (can_MsgRx.id & 0xFF00) >> 8;
        buffer[1] = can_MsgRx.id & 0x00FF;
        buffer[2] = can_MsgRx.len;
        memcpy(&buffer[3], can_MsgRx.data, 8);
        
        static Host multicast3(IpAddr(239, 192, 1, 100), 41000, NULL); //Join multicast group on port 50000
        
        while (ethBusy == true) {};
        ethBusy = true;
        udp.sendto(buffer, 11, &multicast3);
        ethBusy = false;
    }
    
    if (needToParseCANMessage)
    {           
        needToParseCANMessage = false;   
        
        ParseCANMessage(can_MsgRx);
    }
}

void RadioEmulator::HostComm(void)
{
    Net::poll();              
}

void RadioEmulator::onUDPSocketEvent(UDPSocketEvent e)
{
    switch(e)
    {
        case UDPSOCKET_READABLE: //The only event for now
            char buf[64] = {0};
            Host host;
            while( int len = udp.recvfrom( buf, 63, &host ) )
            {
                if( len <= 0 )
                    break;
                    
                HandleHostComm(buf, len);
            }
        break;
    }
}

void RadioEmulator::HandleHostComm(char *msg, int len)
{
    if ((msg[0] == 0x42) && (msg[1] == 0x42) && (msg[2] == 0x42) && (msg[3] == 0x42))
    {
        ReceivedHostMsg = true;
        
        switch (msg[4])
        {
            case 0x00:
                opMode = slave;
            break;
            
            case 0x01:
                status._volume = msg[5];
                status._balance = msg[6];
                status._fade = msg[7];
                status._bass = msg[8];
                status._mid = msg[9];
                status._treble = msg[10];
                
//              writeInitFile();
            break;
            
            case 0x02:
                status._siriusChan = msg[5];
                ChangeSiriusStation(msg[5], false);
            break;
            
            case 0x03:
                status._radioMode = (radioMode)msg[5];
                
                switch (status._radioMode)
                {
                    case AM:
                        status._amPreset = msg[6];
                        status._amFreq = msg[7] + (msg[8] << 8);
                    break;
                    
                    case FM:
                        status._fmPreset = msg[6];
                        status._fmFreq = msg[7] + (msg[8] << 8);
                    break;
                    
                    case SAT:
                        status._siriusPreset = msg[6];
                        status._siriusChan = msg[7];
                    break;
                    
                    case CD:
                        status._cdNum = msg[6];
                        status._cdTrackNum = msg[7];
                        status._cdHours = msg[8];
                        status._cdMinutes = msg[9];
                        status._cdSeconds =  msg[10];
                    break;
                    
                    case VES:
                    break;
                }
            break;
            
            case 0x04:
                CANMessage canMsg;
                canMsg.id = msg[5] + (msg[6] << 8);
                canMsg.len = msg[7];
                memcpy(canMsg.data, msg + 8, canMsg.len);
                
                hostMessages.push_back(canMsg);
            break;
                
        }

    }
}

void RadioEmulator::SendStatusToHost(void)
{
    int size = sizeof(status);
    
    if (opMode == standalone)
    {
        if (status.SWCButtons == 0)
        {
            if ((prevSWC & 0x00000004) != 0)
            {
                if (status._volume > 0)
                    status._volume --;
            }
            else if ((prevSWC & 0x00000002) != 0)
            {
                if (status._volume < 40)
                    status._volume ++;
            }
            else if ((prevSWC & 0x00000010) != 0)
            {
                if (status._siriusChan > 0)
                    ChangeSiriusStation(status._siriusChan-1, false);
            }
            else if ((prevSWC & 0x00000008) != 0)
            {
                if (status._siriusChan < 256)
                    ChangeSiriusStation(status._siriusChan+1, false);
            }
            else if ((prevSWC & 0x00000001) != 0)
            {
                for (int i = 0; i < 5; i++)
                {
//                    SendWOLPacket();
                }
            }
        }
    }

    prevSWC = status.SWCButtons;

    status.count++;    
    RadioState tempStatus;
    memcpy(&tempStatus, &status, size);
    char *data = (char *)&tempStatus;
 
    static Host multicast2(IpAddr(239, 192, 1, 100), 51000, NULL); //Join multicast group on port 50000
    while (ethBusy == true) {};
    ethBusy = true;
    udp.sendto(data, size, &multicast2);
    ethBusy = false;
}

void RadioEmulator::PowerUp(void)
{
    led1 = 1;
    poweredDown = 0;
    needToParseCANMessage = false;   
    LPC_CAN2->BTR = 0x52001C;
    *can_RS = 0;
    ReceivedCANMsg = false;

/*
#ifdef CHECK_HW_SHUTDOWN
    canIRQ->rise(this, &RadioEmulator::StartEmulation);
#else
*/
    StartEmulation();
//#endif
}

void RadioEmulator::StartEmulation(void)
{
//    canIRQ->rise(0);
    
    CANBusTicker.attach(this, &RadioEmulator::WriteCANMessages, 0.5);

    ChangeSiriusStation(status._siriusChan, true);
    
//    CANTimeout.attach(this, &RadioEmulator::CheckCANTimeout, 1);

/*    
    for (int i = 0; i < 5; i++)
    {
        SendWOLPacket();
    }
*/
}

void RadioEmulator::CheckCANTimeout(void)
{
    if (!ReceivedCANMsg)
    {
        led1 = 0;
        // Need to Power Down
       
        CANBusTicker.detach();
        CANTimeout.detach(); 
                
        *can_RS = 1;
        
        canIRQ->rise(this, &RadioEmulator::RestartCAN);
    }
    
    ReceivedCANMsg = false;
}

void RadioEmulator::CheckHostTimeout(void)
{
    if (!ReceivedHostMsg)
    {
        led4 = 1;
        opMode = standalone;
    }
    else
    {
        led4 = 0;
    }

    ReceivedHostMsg = false;
}

void RadioEmulator::RestartCAN(void)
{
    if (poweredDown == 5)
    {
        canIRQ->rise(NULL);
        PowerUp();
    }
    poweredDown++;
}

void RadioEmulator::SendWOLPacket(void)
{
    unsigned char data[102];
    memset(data, 0xff, 6);
    
    for (int i = 0; i < 16; i++)
    {
        memcpy(data + 6 + (16 * i), hostMACAddress, 6);
    }
    static Host wol(IpAddr(10, 10, 10, 255), 7, NULL);

    while (ethBusy == true) {};
    ethBusy = true;    
    udp.sendto((char *)data, 102, &wol);
    ethBusy = false;
}