File content as of revision 0:c0bb943bc2a7:

/* this is a beta release for a can sequencer for prius gen3 on mbed
it is based on a work from Moon Light on mbed.org for the sequencer
and from Yoshi for the filtering



harware:
    mbed LPC1768
    Can transceiver mcp2551 on pin 30 & 29
    BT module BlueToothBee on pin 28 & 27

main mods:
    remove sd card (not used yet)
    change structure for table.cpp: use cod1 & cod2 instead of code
    change content in this table to follow only usefull request (known content)
    install filters

mod 2/04/2012: change detection for last frame in case of multiframe

mod 9/04/2012:: use external files on /local/ for filters & requests  * Author: teamprii

priusfan @ priusfan.info

*/


#include "mbed.h"
#include "MODSERIAL.h"      // buffered serial
#include "stdarg.h"
#include "common.h"
#include <iostream>
#include <fstream>
#include "sstream"
DigitalOut myled(LED1);

LocalFileSystem local("local");
vector<reqTbl> reqTable;
int reqNumTbl;




Ticker          ticker;
Timer           timer;
DigitalOut      led1(LED1);
DigitalOut      led2(LED2);
DigitalOut      led3(LED3);
DigitalOut      led4(LED4);
// We use can on mbed pins 29(CAN_TXD) and 30(CAN_RXD).
CAN can2(p30, p29);

MODSERIAL pc(USBTX, USBRX, 1000, 100);  // tx, rx, txBuffer, rxBuffer

MODSERIAL BT(p28, p27, 1000, 100);  // tx, rx, txBuffer, rxBuffer






int   len_max = 0;

// this request will be completely modified according external file /local/request.txt
char  msg_Request[]  = { 0x02, 0x99, 0x99, 0x00, 0x00, 0x00, 0x00, 0x00 }; // Request Data; 02 is the length of the qry

char  msg_Continue[] = { 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // Continue Receiving Additional Data
int   msg_ReqIdx     = 0;
int   msg_ReqFrameW = 0;

//
char  msg_MF_data[100]       ; // used to store Multiframe
int   msg_MF_Idx             ; // cursor for previous
unsigned int msg_MF_Id       ; // Multiframe Id

char Crc                    ;   // crc char for checking integrity



//


//============================================
// declaration functions (prototypes)
//============================================
void init();
int install_filters();
int read_reqTable(vector<reqTbl>& reqtable, int& reqnumtbl);
void CAN2_wrFilter (uint32_t id);
void timchk();
void can2rcv();
//void calcs();
//void calcm();
//=======================================
unsigned int prev_us  = 0; // work
unsigned int curr_us  = 0; // micro sec looped
unsigned int curr_uH  = 0; // micro sec upper dword
unsigned int diff_us  = 0; // erapsed us
unsigned int curr_ms  = 0; // milli sec looped
unsigned int left_ms  = 0; // work
unsigned int curr_sc  = 0; // sec looped
int          ms_1000  = 0; // 1000 looped ms
int          ms_60000 = 0; // 60000 looped ms
unsigned int max_diff = 0;
int          send_per_sec =  0;
int          rec_per_sec =  0;
int          HiNi=0;
int          NbB=0;
//============================================

int main() {
    init();
    //
    CANMessage can_MsgRx;

    while (1) {
        if (can2.read(can_MsgRx)) {
            led4=!led4;
//              check if new can message
//                  if long response, ask for next part
//                  check if frame is complete (case of multiframe)

            if (can_MsgRx.id < 0x700) {   // passive  frame ,
                msg_ReqFrameW--;

                pc.printf("p\t%03X\t",can_MsgRx.id);
                for (int i=0; i<can_MsgRx.len; i++) {
                    pc.printf(" ");
                    pc.printf("%02X",can_MsgRx.data[i]);
                } // for
                pc.printf("\r\n");

                BT.printf("%03X",can_MsgRx.id);
                Crc=can_MsgRx.id;
                for (int i=0; i<can_MsgRx.len; i++) {
                    BT.printf("%02X",can_MsgRx.data[i]);
                    Crc = Crc + can_MsgRx.data[i];
                } // for
                BT.printf("%02X",Crc);
                BT.printf("\r\n");

                // calcs();

                // send next req
                if (msg_ReqFrameW <= 0) {
                    // it is ok to send another request
                    // because  we received at least X passive frames since completion of previous request
                    // get the full message to send
                    for (int i=0; i<8; i++) {
                        msg_Request[i] = reqTable[msg_ReqIdx].code[i];
                    }
                    // log request
                    pc.printf("T\t%03X\t",reqTable[msg_ReqIdx].id);
                    for (int i=0; i<8; i++) {
                        pc.printf(" ");
                        pc.printf("%02X", msg_Request[i]);
                    } // for
                    pc.printf("\r\n");
                    // end  log request
                    if (can2.write(CANMessage(reqTable[msg_ReqIdx].id, msg_Request,8))) {
                        send_per_sec++;
                        msg_ReqFrameW = 100;

                        // next req
                        do {
                            reqTable[msg_ReqIdx].fcnt++;
                            if (reqTable[msg_ReqIdx].fcnt >= reqTable[msg_ReqIdx].freq) {
                                reqTable[msg_ReqIdx].fcnt = 0;
                            } // endif
                            msg_ReqIdx++;
                            if (msg_ReqIdx > reqNumTbl) {
                                msg_ReqIdx = 0;
                            } // endif
                        } while (reqTable[msg_ReqIdx].fcnt != 0);
                    } // end if can2.write
                } // end  if (msg_ReqFrameW <= 0)

            } else if (can_MsgRx.id >= 0x7E8 ) {        // from here, we are dealing with answers
                led2 =!led2;
                HiNi = can_MsgRx.data[0]>>4;    // test HighNibble to detect continuation frame
                switch (HiNi) {
                    case 0:                         // short response  if (can_MsgRx.data[0] < 0x10) {

                        BT.printf("%03X",can_MsgRx.id);
                        Crc=can_MsgRx.id;
                        for (int i=0; i<can_MsgRx.len; i++) {
                            BT.printf("%02X",can_MsgRx.data[i]);
                            Crc = Crc + can_MsgRx.data[i];
                        } // for
                        BT.printf("%02X",Crc);
                        BT.printf("\r\n");

                        pc.printf("r\t%03X\t",can_MsgRx.id);

                        for (int i=0; i<8; i++) {
                            pc.printf(" ");
                            pc.printf("%02X",can_MsgRx.data[i]);
                        } // for
                        pc.printf("\r\n");

                        msg_ReqFrameW = 2;
                        break;

                    case 1 :    // long response   if (can_MsgRx.data[0] == 0x10) {


                        NbB =int(can_MsgRx.data[1]); // the Number of Bytes in the multiframe
                        msg_ReqFrameW = 50;                  // continue receive
                        // store msg
                        msg_MF_Idx = 0;
                        // store id
                        msg_MF_Id = can_MsgRx.id;
                        // store from 1 to 7

                        for (int i = 1;  i < can_MsgRx.len ; i++) {
                            msg_MF_data[msg_MF_Idx] = can_MsgRx.data[i];
                            msg_MF_Idx ++;
                        }

                        // send contination request
                        if (can2.write(CANMessage(can_MsgRx.id -8, msg_Continue, 8))) {
                            led3 =!led3;
                        }
                        break;

                    case 2 : //               if (can_MsgRx.data[0] > 0x20 && can_MsgRx.data[0] <= 0x2F)
                        //
                        // store from 1 to 7
                        for (int i = 1; i < can_MsgRx.len ; i++) {
                            msg_MF_data[msg_MF_Idx] = can_MsgRx.data[i];
                            msg_MF_Idx ++;
                        }
                        //
                        if ( msg_MF_Idx > NbB) {   // we received the full multiframe
                            msg_ReqFrameW = 2;
                            // multiframe is finished, we send the complete thing to pc

                            BT.printf("%03X",msg_MF_Id);
                            Crc=msg_MF_Id;
                            for (int i=0; i <=  NbB; i++) {
                                BT.printf("%02X", msg_MF_data[i]);
                                Crc = Crc + msg_MF_data[i];
                            } // for
                            BT.printf("%02X",Crc);
                            BT.printf("\r\n");

                            pc.printf("r\t%03X\t",msg_MF_Id);
                            for (int i=0; i <=  NbB; i++) {
                                pc.printf(" ");
                                pc.printf("%02X", msg_MF_data[i]);
                            } // for
                            pc.printf("\r\n");
                            // calcm();
                        } else {
                            msg_ReqFrameW = 50;
                            break;
                        default: //                 } else if (can_MsgRx.data[0] == 0x30) {
                            // ignore
                            break;
                        } // end  if ( msg_MF_Idx >= NbB)
                } // end  switch (HiNi)


                // Error check
                int rerr = can2.rderror();
                int werr = can2.tderror();
                if (rerr > 0 || werr > 0) {
                    can2.reset();
                    led4 = !led4;
                } // end if error
            } // end  if (can_MsgRx.id
        } // end if (can2.read(can_MsgRx)
    } //  while (1)
}

//============================================
// timchk()
// description: this routine is called every 100microsec
//              a) manage time_counters

//============================================
void timchk() {

    // count time
    curr_us = timer.read_us(); // 0x00000000-0xFFFFFFFF looped
    diff_us = curr_us - prev_us;
    if (curr_us < prev_us) {
        curr_uH++;
    }
    left_ms += diff_us;
    while (left_ms >= 1000) { // timer.read_ms() not looped. it calculated from read_us() / 1000. so buggy.
        curr_ms++;
        ms_60000++;
        ms_1000++;
        if (ms_60000 >= 60000) {
            ms_60000 = 0;
        }
        if (ms_1000 >= 1000) {
            ms_1000 = 0;
            curr_sc++;
            send_per_sec =0;
            rec_per_sec=0;

        }
        led1 = (ms_60000 / 250) & 1;
        left_ms -= 1000;
        //

    }
    if (diff_us > max_diff) {
        max_diff = diff_us;
    }
    prev_us = curr_us;


}



//*----------------------------------------------------------------------------
//  init()
//  description: no comment
//*----------------------------------------------------------------------------
void init() {


    // Init
    can2.frequency(500000);
    //can2->Attach(*can2rcv);   // not ready now for queuing
    pc.baud(921600);

    BT.baud(38400);                 // when the BT module is new, the bps is 38400
    BT.printf(" \r\n+STBD=460800\r\n");            // Set baudrate 460800. Save and Rest
    wait(0.5);
    BT.baud(115200);                // in case we played before with 115200
    BT.printf(" \r\n+STBD=460800\r\n");            // Set baudrate 460800. Save and Rest
    wait(0.5);
    BT.baud(460800);
    wait(0.5);
    BT.printf("\r\n+INQ=1\r\n");               // start broadcasting (make device visible)

    // install filters ( Filter)
    install_filters();

    int reqtblresult = read_reqTable(reqTable, reqNumTbl);

    timer.start();

    led1=0;
    led2=0;
    led3=0;
    led4=0;

    msg_ReqFrameW = 2;
    // CAN
    prev_us = timer.read_us();
    ticker.attach_us(&timchk, 100);
}
// When a CAN message is received, it is placed in queue
void can2rcv() {
    CANMessage msg;
    if (can2.read(msg)) {
//       inQueue.Enqueue(msg);
        led3=!led3;
    }
}

/*============================================
// calcs()
// description: this routine is called at the recept of each single frame answer
// we will use msg.id & msg.data[]
//
//
//
//============================================
void calcs() {
    if (msg.id== 0x00B4) { }
    if (msg.id== 0x01C4) { }
    if (msg.id== 0x0245) { }
    if (msg.id== 0x03D3) { }
    if (msg.id== 0x0498) { }
    if (msg.id== 0x04A6) { }
    if (msg.id== 0x07E8 &&  msg.data[1]==0x61 && msg.data[2]==0x3C) {}
    if (msg.id== 0x07E8 &&  msg.data[1]==0x61 && msg.data[2]==0x49) {}
    if (msg.id== 0x07E8 &&  msg.data[1]==0x41 && msg.data[2]==0x3C) {}
    if (msg.id== 0x07E8 &&  msg.data[1]==0x41 && msg.data[2]==0x3E) {}

    if (msg.id== 0x07EA &&  msg.data[1]==0x61) {
        if (msg.data[2] == 0x61) {}
        if (msg.data[2] == 0x62) {}
        if (msg.data[2] == 0x67) {}
        if (msg.data[2] == 0x68) {}
        if (msg.data[2] == 0x70) {}
        if (msg.data[2] == 0x71) {}
        if (msg.data[2] == 0x87) {}
        if (msg.data[2] == 0x8A) {}
        if (msg.data[2] == 0x98) {}
    }

}


//============================================
// calcm()
// description: this routine is called at the end of multiframe answer
// we will use  msg_MF_Id  & msg_MF_data[]
//
//
//
//============================================
void calcm() {
    if ( msg_MF_Id== 0x07E8 &&   msg_MF_data[1]==0x61 &&  msg_MF_data[2]==0x01) {}
    if ( msg_MF_Id== 0x07E8 &&   msg_MF_data[1]==0x61 &&  msg_MF_data[2]==0x49) {}
    if ( msg_MF_Id== 0x07EA &&   msg_MF_data[1]==0x61 &&  msg_MF_data[2]==0x01) {}
    if ( msg_MF_Id== 0x07E8 &&   msg_MF_data[1]==0x61 &&  msg_MF_data[2]==0x01) {}

}
*/

/*----------------------------------------------------------------------------
  CAN_wrFilter()
 description: setup acceptance filter.  CAN controller (1..2)

  setup acceptance filter for CAN controller 2
  original http://www.dragonwake.com/download/LPC1768/Example/CAN/CAN.c
  simplified for CAN2 interface and std id (11 bit) only by YM1784
 *----------------------------------------------------------------------------*/
void CAN2_wrFilter (uint32_t id)  {
    static int CAN_std_cnt = 0;
    uint32_t buf0, buf1;
    int cnt1, cnt2, bound1;

    /* Acceptance Filter Memory full */
    if (((CAN_std_cnt + 1) >> 1) >= 512)
        return;                                       /* error: objects full */

    /* Setup Acceptance Filter Configuration
      Acceptance Filter Mode Register = Off  */
    LPC_CANAF->AFMR = 0x00000001;

    id |= 1 << 13;                        /* Add controller number(2) */
    id &= 0x0000F7FF;                            /* Mask out 16-bits of ID */

    if (CAN_std_cnt == 0)  {                     /* For entering first  ID */
        LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
    }  else if (CAN_std_cnt == 1)  {             /* For entering second ID */
        if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
            LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
        else
            LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
    }  else  {
        /* Find where to insert new ID */
        cnt1 = 0;
        cnt2 = CAN_std_cnt;
        bound1 = (CAN_std_cnt - 1) >> 1;
        while (cnt1 <= bound1)  {                  /* Loop through standard existing IDs */
            if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id)  {
                cnt2 = cnt1 * 2;
                break;
            }
            if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id)  {
                cnt2 = cnt1 * 2 + 1;
                break;
            }
            cnt1++;                                  /* cnt1 = U32 where to insert new ID */
        }                                          /* cnt2 = U16 where to insert new ID */

        if (cnt1 > bound1)  {                      /* Adding ID as last entry */
            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1]  = 0x0000FFFF | (id << 16);
            else                                     /* Odd  number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1]  = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
        }  else  {
            buf0 = LPC_CANAF_RAM->mask[cnt1];        /* Remember current entry */
            if ((cnt2 & 0x0001) == 0)                /* Insert new mask to even address */
                buf1 = (id << 16) | (buf0 >> 16);
            else                                     /* Insert new mask to odd  address */
                buf1 = (buf0 & 0xFFFF0000) | id;

            LPC_CANAF_RAM->mask[cnt1] = buf1;        /* Insert mask */

            bound1 = CAN_std_cnt >> 1;
            /* Move all remaining standard mask entries one place up */
            while (cnt1 < bound1)  {
                cnt1++;
                buf1  = LPC_CANAF_RAM->mask[cnt1];
                LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
                buf0  = buf1;
            }

            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
        }
    }
    CAN_std_cnt++;

    /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
    buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
    buf1 = buf0;

    /* Setup acceptance filter pointers */
    LPC_CANAF->SFF_sa     = 0;
    LPC_CANAF->SFF_GRP_sa = buf0;
    LPC_CANAF->EFF_sa     = buf0;
    LPC_CANAF->EFF_GRP_sa = buf1;
    LPC_CANAF->ENDofTable = buf1;

    LPC_CANAF->AFMR = 0x00000000;                  /* Use acceptance filter */
} // CAN2_wrFilter


/* =================================================
read /local/filters.txt & install them
the file contains something like:
// max line length 128
025 // comments can be added to note what this filter (025) is for
030
038
039
// 03A // comment out this line so this filter (03A) will not be installed
230
244

 Author: teamprii
=====================================================*/
int install_filters() {
    const int maxlinelen=128; // max line length in filters.txt file; 128 should be plenty for things like "0B4 // comment for 0B4 ....... " etc.
    char str[maxlinelen];
    int filter;
    ifstream infile("/local/filters.txt"); // hard coded pathname for filter specification file
    if (infile.is_open()) {
        while (infile.good()) {
            infile.getline(str,maxlinelen);
            if (str[0]=='/' && str[1]=='/') {
                // pc.printf("skip commented line %s",str);
                // pc.printf("\r\n");
                //    cout<<str<<", skip commented line."<<endl; // skip commented line
            } else {
                sscanf(str,"%x",&filter);
                //    cout<<str<<", install filter "<<hex<<filter<<endl; // to be replaced with CAN2_wrFilter(hex);
                pc.printf("filter:\t%03X\r\n",filter);
                CAN2_wrFilter (filter);
            }
        }
        infile.close();
        return 0;
    } else {
        // pc.printf("Unable to open filter spec file");
        //     cout<<"Unable to open filter spec file";
        return -1;
    }
}
int read_reqTable(vector<reqTbl>& reqtable, int& reqnumtbl) {
    const int maxlinelen=256; // max line length in request.txt file; 256 should be plenty.
    char str[maxlinelen];
    int id; // PID
    int freq; // request frequency
    int codej; // to store read code temporarily
    reqTbl tmp;
    ifstream infile("/local/request.txt"); // hard coded pathname for request specification file
    if (infile.is_open()) {
        while (infile.good()) {
            infile.getline(str,maxlinelen);
            if (str[0]=='/' && str[1]=='/') {
                //     pc.printf("skip commented line %s",str);
                //     pc.printf("\r\n");
                //    cout<<str<<", skip commented line."<<endl; // skip commented line
            } else {
                //  cout<<str<<", add this request to reqTable."<<endl; // add request to reqTable
                stringstream ss;
                ss<<str;
                if (ss.str().length() > 10) {        // use only real lines
                    ss>>hex>>id>>freq;
                    pc.printf("\r\n add this request to reqTable.\r\n");
                    tmp.id=id;
                    tmp.freq=freq;
                    vector<int> code; // list of code
                    do {
                        ss>>hex>>codej;
                        if (ss.good()) {
                            code.push_back(codej);
                        }
                    } while (ss.good());
                    tmp.code=code;
                    //cout<<"PID="<<tmp.id<<", freq="<<tmp.freq<<", code=";
                    pc.printf("PID=%03X",tmp.id);
                    pc.printf("\tfreq=%03d",tmp.freq);
                    pc.printf("\tCode=");

                    for (int j=0; j<(int)tmp.code.size(); j++) {
                        pc.printf(" %02x",tmp.code[j]);
                        //    cout<<" "<<tmp.code[j]<<", ";
                    }
                    // cout<<endl;
                    pc.printf("\r\n");
                    reqtable.push_back(tmp);
                }
            }
        }
        reqnumtbl=reqtable.size();
        infile.close();
        return 0;
    } else {
        // pc.printf("Unable to open request spec file");
        //   cout<<"Unable to open request spec file";
        return -2;
    }
}