File content as of revision 1:79b1ee0f97ef:

#include "mbed.h"
#include "can.hpp"
#include "rtos.h"

/*
 * COMMON
 */

CAN can(p9, p10);

uint8 can_missing[CAN_RX_PERIODIC_MSG];

void init_can()
{
    printf("RECOVERY: %d\r\n", CAN_MISSING_DETECTION);
    for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++)
        can_missing[i] = CAN_MISSING_DETECTION;
}

/*
 * RECEIVER
 */

can_cmd_engine_t can_cmd_engine;
can_cmd_body_t can_cmd_body;
can_cmd_time_t can_cmd_time;
can_cmd_driver_t can_cmd_driver;

void can_rx()
{
    bool ret = false;
    CANMessage message;

    do{
        ret = can.read(message);

        if (ret == 0)
            break;

        switch(message.id) {
        case CAN_CMD_ENGINE_ID:
            can_cmd_engine.payload.buf[0] = message.data[0];
            can_cmd_engine.payload.buf[1] = message.data[1];
            can_cmd_engine.payload.buf[2] = message.data[2];
            can_cmd_engine.payload.buf[3] = message.data[3];
            can_cmd_engine.flag = CAN_FLAG_RECEIVED;
            can_missing[CAN_MISSING_ENGINE_ID] = CAN_MISSING_DETECTION+1;
            break;
        case CAN_CMD_BODY_ID:
            can_cmd_body.payload.buf[0] = message.data[0];
            can_cmd_body.payload.buf[1] = message.data[1];
            can_cmd_body.payload.buf[2] = message.data[2];
            can_cmd_body.payload.buf[3] = message.data[3];
            can_cmd_body.flag = CAN_FLAG_RECEIVED;
            can_missing[CAN_MISSING_BODY_ID] = CAN_MISSING_DETECTION+1;
            break;
        case CAN_CMD_TIME_ID:
            can_cmd_time.payload.buf[0] = message.data[0];
            can_cmd_time.payload.buf[1] = message.data[1];
            can_cmd_time.payload.buf[2] = message.data[2];
            can_cmd_time.payload.buf[3] = message.data[3];
            can_cmd_time.flag = CAN_FLAG_RECEIVED;
            can_missing[CAN_MISSING_TIME_ID] = CAN_MISSING_DETECTION+1;
            break;
        case CAN_CMD_DRIVER_ID:
            can_cmd_driver.payload.buf[0] = message.data[0];
            can_cmd_driver.payload.buf[1] = message.data[1];
            can_cmd_driver.payload.buf[2] = message.data[2];
            can_cmd_driver.payload.buf[3] = message.data[3];
            can_cmd_driver.payload.buf[4] = message.data[4];
            can_cmd_driver.payload.buf[5] = message.data[5];
            can_cmd_driver.payload.buf[6] = message.data[6];
            can_cmd_driver.payload.buf[7] = message.data[7];
            can_cmd_driver.flag = CAN_FLAG_RECEIVED;
            break;
        };
    } while(1);

    for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++)
        if (can_missing[i]>0)
            can_missing[i]--;
}

/*
 * SENDER
 */
can_sts_body_t can_sts_body;
can_sts_driver_t can_sts_driver;

void can_tx ()
{
    /** periodic messages */
    if (can_sts_body.flag == 0) {
        if (!can.write(CANMessage(CAN_STS_BODY_ID, (char*)(&(can_sts_body.payload.buf)), CAN_STS_PAYLOAD_BODY)))
//            printf("SEND STS_BODY OK\r\n");
//        else
            printf("SEND STS_BODY NOT OK\r\n");
        can_sts_body.flag = CAN_STS_BODY_PERIOD;
    }
    can_sts_body.flag--;

    /** event messages */
    if (can_sts_driver.flag == CAN_FLAG_SEND) {
        if (!can.write(CANMessage(CAN_STS_DRIVER_ID, (char*)(&(can_sts_driver.payload.buf)), CAN_STS_PAYLOAD_DRIVER)))
//            printf("SEND STS_DRIVER OK\r\n");
//        else
            printf("SEND STS_DRIVER NOT OK\r\n");
        can_sts_driver.flag = CAN_FLAG_EMPTY;
    }
}

/*
 * THREAD
 */

void thread_can (void const *args)
{
    while(1) {
        can_rx();
        can_tx();
        Thread::wait(CAN_THREAD_PERIOD);
    }
}

bool can_msg_is_missing (uint8 msg_id)
{
    if (msg_id < CAN_RX_PERIODIC_MSG)
        return (can_missing[msg_id]==0);
    return true;
}