File content as of revision 6:e977526d114f:

/*
* INNOVATE MTS SERIAL STREAM TO MEGASQUIRT MSCAN ANALOG 
*
* Written by Arran Short <sordfish2050@gmail.com>
*
* This was tested on the olimex olimexino-stm32. 
* code comes with no warrenty, always confirm AFR readings at the device.
*
* code was based off http://developer.mbed.org/users/hudakz/code/CAN_Nucleo_Hello/
* mts datasheet http://www.innovatemotorsports.com/support/downloads/Seriallog-2.pdf
* MSCAN http://www.msextra.com/doc/pdf/Megasquirt_CAN_Broadcast.pdf
*
*
*
* ************ PLEASE NOTE ************
* MSCAN has table and data offsets to choose what data it requires,
* this code hasn't implimented any of that so regardless of them
* it just sends lambda for the first analog value and the mts function value in the second
*
*
* MTS Serial is RS232 not ttl so you will need an RS-232 Transceiver
*
*/ 

#include "mbed.h"
#include "CAN.h"

Serial device(PA_9, PA_10);
Serial pc(SERIAL_TX, SERIAL_RX);

DigitalOut      led1(PA_5); // GREEN
DigitalOut      led2(PA_1); // YELLOW

CAN             can(PB_8, PB_9);  // CAN Rx pin name, CAN Tx pin name, Automatic recovery from bus-off state enabled by default
CANMessage      rxMsg;
CANMessage      txMsg;

uint16_t        lambda;
uint8_t         mtsfunction;

volatile bool   CANmsgAvailable = false;
volatile bool   LM1 = false;
volatile bool   gotLambda = false;
volatile bool   gotMscanreq = false;

uint8_t mycanid = 0x09; // this is the id set in tunerstudio

uint8_t offset = 0;
uint8_t msgtype = 0;
uint8_t fromid = 0;
uint8_t toid = 0;
uint8_t table = 0;

uint8_t myvarblk;   // these three bytes are the location of where the MS cpu wants the data to be sent,
uint16_t myvaroff;  // MS doesnt have any error checking so make sure these values are correct.
uint8_t varbyt;     //

char rxSerial[8];
char header[2];
char txData[8];


// macros ripped from arduino
#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
#define bitSet(value, bit) ((value) |= (1UL << (bit)))
#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit))  

/**
 * @brief   'CAN receive-complete' interrup handler.
 * @note    Called on arrival of new CAN message.
 *          Keep it as short as possible.
 * @param   
 * @retval  
 */
void onCanMsgReceived() {
    CANmsgAvailable = true;
}

/**
 * @brief   'process sub packet'
 * @note    Called when main loop detects a matching Innovate MTS header 
 *          it then detects if its legacy or new protocol, then strips afr data from stream, TODO: Need to add mts inputs and 2nd o2 sensor to mscan messages
 * @param   
 * @retval  
 */
void processsubpacket() {
           
    led2 = !led2;
           
    if ( bitRead(rxSerial[2], 6) == 1 && bitRead(rxSerial[2], 1) == 1 && bitRead(rxSerial[3], 7) == 0) {
        LM1 = false;
        //pc.printf("not LM1\n");
    }
    if ( bitRead(rxSerial[2], 7) == 1 && bitRead(rxSerial[2], 1) == 0 && bitRead(rxSerial[3], 7) == 0) {
        LM1 = true;
        //pc.printf("is LM1\n");
    }

    mtsfunction = 0;

    bitWrite(mtsfunction, 2, bitRead(rxSerial[2], 4));
    bitWrite(mtsfunction, 1, bitRead(rxSerial[2], 3));
    bitWrite(mtsfunction, 0, bitRead(rxSerial[2], 2));
    
    pc.printf("function is "+mtsfunction+"\r\n");
            
    if (mtsfunction == 0) { //function 0 mean good data.
   
        lambda = rxSerial[4] << 6;
        lambda = lambda + rxSerial[5];

        gotLambda = true;
    } else {
        //turn off serial led
        led2 = 0;
    } 
 
}//processsubpacket() 

/**
 * @brief   'ms can id'
 * @note    Called when can message has been received 
 *          it then converts data into human readable names
 * @param   
 * @retval  
 */
void mscanid() {

    offset = rxMsg.id >> 18;
    msgtype = (rxMsg.id << 14) >> 29;
    fromid = (rxMsg.id << 17) >> 28;
    toid = (rxMsg.id << 21) >> 28;
    table = (((rxMsg.id << 29) >> 31) << 5) + ((rxMsg.id << 25) >> 28);
    
}

/**
 * @brief   'mscan tx header'
 * @note    Called when mscan message has been processed 
 *           and a responce needs to be generated from serial data
 * @param   
 * @retval  
 */
void mscantxheader(uint8_t messagetype, uint8_t sendtoid) { // makes an mscan header, byte is for message type
    
    switch (messagetype) {
        case 0:
            // command
            break;
        case 1:
            // request
            break;
        case 2:
            // response
            txMsg.clear();
            txMsg.type   = CANData;
            txMsg.format = CANExtended;
                        
            bitWrite(txMsg.len, 3, bitRead(varbyt, 3));
            bitWrite(txMsg.len, 2, bitRead(varbyt, 2));
            bitWrite(txMsg.len, 1, bitRead(varbyt, 1));
            bitWrite(txMsg.len, 0, bitRead(varbyt, 0)); 
            
            //offset
            bitWrite(txMsg.id, 28, bitRead(myvaroff, 10));
            bitWrite(txMsg.id, 27, bitRead(myvaroff, 9));
            bitWrite(txMsg.id, 26, bitRead(myvaroff, 8));
            bitWrite(txMsg.id, 25, bitRead(myvaroff, 7));
            bitWrite(txMsg.id, 24, bitRead(myvaroff, 6));
            bitWrite(txMsg.id, 23, bitRead(myvaroff, 5));
            bitWrite(txMsg.id, 22, bitRead(myvaroff, 4));
            bitWrite(txMsg.id, 21, bitRead(myvaroff, 3));
            bitWrite(txMsg.id, 20, bitRead(myvaroff, 2));
            bitWrite(txMsg.id, 19, bitRead(myvaroff, 1));
            bitWrite(txMsg.id, 18, bitRead(myvaroff, 0));
            //message type = responce
            bitWrite(txMsg.id, 17, 0);
            bitWrite(txMsg.id, 16, 1);
            bitWrite(txMsg.id, 15, 0);  
            //from id 
            bitWrite(txMsg.id, 14, bitRead(mycanid, 3));
            bitWrite(txMsg.id, 13, bitRead(mycanid, 2));
            bitWrite(txMsg.id, 12, bitRead(mycanid, 1));
            bitWrite(txMsg.id, 11, bitRead(mycanid, 0));
            //to id
            bitWrite(txMsg.id, 10, bitRead(sendtoid, 3));
            bitWrite(txMsg.id, 9, bitRead(sendtoid, 2));
            bitWrite(txMsg.id, 8, bitRead(sendtoid, 1));
            bitWrite(txMsg.id, 7, bitRead(sendtoid, 0));
            //var block
            bitWrite(txMsg.id, 6, bitRead(myvarblk, 3));
            bitWrite(txMsg.id, 5, bitRead(myvarblk, 2));
            bitWrite(txMsg.id, 4, bitRead(myvarblk, 1));
            bitWrite(txMsg.id, 3, bitRead(myvarblk, 0));
            //spares
            bitWrite(txMsg.id, 2, bitRead(myvarblk, 4));
            bitWrite(txMsg.id, 1, 0);
            bitWrite(txMsg.id, 0, 0);

            txMsg.data[0] = lambda >> 8;
            txMsg.data[1] = lambda;
            txMsg.data[2] = 0x00;
            txMsg.data[3] = mtsfunction;
            txMsg.data[4] = 0x00;
            txMsg.data[5] = 0x00; // 2 Spare channels, could add values from shield or other sensors.
            txMsg.data[6] = 0x00; //
            txMsg.data[7] = 0x00;
            
            break;
        case 3:
            // xsub
            break;
        case 4:
            // burn
            break;      
            }
    pc.printf("Tx Message id is: %d \r\n", txMsg.id);
    gotMscanreq = true;

}


void mscanrxdata() {

    if (msgtype == 1 ) { //message is a request
       
        myvarblk = rxMsg.data[0]; 
        pc.printf("message is a request\r\n");

        bitWrite(myvaroff, 10, bitRead(rxMsg.data[1], 7));
        bitWrite(myvaroff, 9, bitRead(rxMsg.data[1], 6));
        bitWrite(myvaroff, 8, bitRead(rxMsg.data[1], 5));
        bitWrite(myvaroff, 7, bitRead(rxMsg.data[1], 4));
        bitWrite(myvaroff, 6, bitRead(rxMsg.data[1], 3));
        bitWrite(myvaroff, 5, bitRead(rxMsg.data[1], 2));
        bitWrite(myvaroff, 4, bitRead(rxMsg.data[1], 1));
        bitWrite(myvaroff, 3, bitRead(rxMsg.data[1], 0));
        bitWrite(myvaroff, 2, bitRead(rxMsg.data[2], 7));
        bitWrite(myvaroff, 1, bitRead(rxMsg.data[2], 6));
        bitWrite(myvaroff, 0, bitRead(rxMsg.data[2], 5));

        varbyt = rxMsg.data[2] << 3 >> 3;
        
        //megasquirt ecu has requested some data, details of what data it wants is provided in the header. TODO - add a function say what data it wants.
        // we need to generate some data to send back, this is done with mscantxheader
        mscantxheader(0x02, fromid);
        }
}//mscanrxdata()
 

void flushSerialBuffer(void) { 
    //pc.printf("flushing..\r\n");
    char char1 = 0; 
    while (device.readable()) {
        char1 = device.getc();
        }
    return;
    } 
 
void testcantx(void) { 
    txMsg.clear();                      // clear Tx message storage
    txMsg.type   = CANData;
    txMsg.format = CANExtended;
    txMsg.len = 0x03;
    txMsg.id = 0x1FFD487C;
    txMsg.data[0] = 0x01;
    txMsg.data[1] = 0x02;
    txMsg.data[2] = 0x03;
    can.write(txMsg);
    pc.printf("test CAN message sent\r\n");

    } 

 
/**
 * @brief   Main
 * @note
 * @param 
 * @retval
 */ 
int main() {
    device.baud(19200);                             // set bit rate to 19200, this is the baudrate of innovate mts
    device.format(8,SerialBase::None,1);
    can.frequency(500000);                          // set bit rate to 500k
    can.attach(&onCanMsgReceived, CAN::RxIrq);      // attach 'CAN receive-complete' interrupt handler
    can.filter(mycanid << 7, 0x780, CANExtended);
    //testcantx();
    
    while (1) { //loop forever
        led1 = 1;
        
        if ( gotLambda && gotMscanreq ) { //send reply to megasquirt when we get data from mts serial and we have mscan frame ready.
            pc.printf("ready to send\r\n");  
            led2 = !led2;
            pc.printf("can tx id is: %d \r\n", txMsg.id);
            pc.printf("can tx type is: %d \r\n", txMsg.type); 
            pc.printf("can tx format is: %d \r\n", txMsg.format); 
            pc.printf("can tx len is: %d \r\n", txMsg.len); 
            pc.printf("can tx data 0 is: %d \r\n", txMsg.data[0]);
            pc.printf("can tx data 1 is: %d \r\n", txMsg.data[1]);
            pc.printf("can tx data 2 is: %d \r\n", txMsg.data[2]);
            pc.printf("can tx data 3 is: %d \r\n", txMsg.data[3]);
            pc.printf("can tx data 4 is: %d \r\n", txMsg.data[4]);
            pc.printf("can tx data 5 is: %d \r\n", txMsg.data[5]);
            pc.printf("can tx data 6 is: %d \r\n", txMsg.data[6]);
            pc.printf("can tx data 7 is: %d \r\n", txMsg.data[7]);   
            can.write(txMsg);
            pc.printf("CAN message sent\r\n");            
            //gotLambda = false;
            gotMscanreq = false;
            
        }
        
        if(CANmsgAvailable) {  //check if interupt has set the flag for avaliable messages
            //led2 = !led2;
            CANmsgAvailable = false;               // reset flag for next use
            can.read(rxMsg);                       // read message into Rx message storage 
            mscanid();
            mscanrxdata();                            
            //led2 = 0;
            }
        
        while (device.readable()) { // checks if there has been and messages from serial port
            for (int x=0; x < 8; x++){
                rxSerial[x] = device.getc();
                if (  x==0 ) {
                    if ( (rxSerial[0] & 0xA2) == 0xA2 ){
                        //led1 = 1; 
                    } else {
                        x=0;
                        pc.printf("serial data reset\r\n");
                        }
                }
            }    
            
            header[0] = (rxSerial[0] & 0xA2);
            header[1] = (rxSerial[1] & 0x80);
        
            if ( header[0] == 0xA2 && header[1] == 0x80) {
                processsubpacket();
            }
            flushSerialBuffer();
        }
        led1 = 0;
    }

}//main