File content as of revision 18:e6ed582f7022:

#include "mbed.h"
#include "yoda2.h"

extern unsigned char rx_buffer[8], tx_buffer[8];
extern unsigned char rx_length, tx_length;
extern uint8_t can_tx_data[8];
extern uint8_t can_rx_data[8];
extern int current_weight;

typedef struct can_id_d {
    int dest_id;
    int source_id;
    int message_id;
};

typedef struct data_field_d {
    uint8_t cmd;
    uint16_t value1;
    uint16_t value2;
    uint16_t value3;
    uint8_t value4;
};
can_id_d can_id_s;
data_field_d can_rxdata_frame;
data_field_d can_txdata_frame;
CANMessage tx_message;
extern MemoryPool<CANMessage, 16> can_mpool;
extern Queue<CANMessage, 16> can_queue;
extern void can_sendData(int can_id, uint8_t *tx_data, int length);
extern Device_Type_d device_type_v;
extern int device_address;  // last 8 bits of address
extern int device_type; // first 3 bits of adddress

void handleCupTrackCommand(data_field_d data_pack)
{   
}
void handleJamTrackCommand(data_field_d data_pack)
{   
}
void handleTeaTrackCommand(data_field_d data_pack)
{   
}
void handleJamCommand(data_field_d data_pack)
{   
}
void handleTeaCommand(data_field_d data_pack)
{   
    int can_id;
    switch (data_pack.cmd)
    {
            case COMMAND_INIT:
                can_txdata_frame.cmd = COMMAND_INIT;
                can_txdata_frame.value1 = current_weight;
                can_txdata_frame.value2 = 0;
                can_txdata_frame.value3 = 0;
                can_txdata_frame.value4 = 0;
                can_tx_data[0] = can_txdata_frame.cmd;
                can_tx_data[1] = (can_txdata_frame.value1 & 0x00FF);
                can_tx_data[2] = ((can_txdata_frame.value1 >> 8) & 0x00FF);
                can_tx_data[3] = (can_txdata_frame.value2 & 0x00FF);
                can_tx_data[4] = ((can_txdata_frame.value2 >> 8) & 0x00FF);   
                can_tx_data[5] = (can_txdata_frame.value3 & 0x00FF);
                can_tx_data[6] = ((can_txdata_frame.value3 >> 8) & 0x00FF);  
                can_tx_data[7] = can_txdata_frame.value4;  
                can_id = (can_id_s.source_id << 18) | (can_id_s.dest_id << 7) | can_id_s.message_id | 0x80000000;
                can_sendData(can_id, can_tx_data, 8);
                break;
            default:
                break;
    }
}
void handleShakerCommand(data_field_d command)
{   
}

void analyzePayload()
{
    int can_id;
    int loop;
    
    printf("analyzePayload thread\r\n");
    while (true) {
        osEvent evt = can_queue.get();
        if (evt.status == osEventMessage) {
            CANMessage *message = (CANMessage*)evt.value.p;
            memcpy((void *)&tx_message, (void *)message, sizeof(tx_message)); 

            printf("analyzePayload got message id=%d 0x%08x\r\n", tx_message.id, tx_message.id);

            for (loop = 0; loop < tx_message.len; loop++)
            {
                can_rx_data[loop] = tx_message.data[loop];
            }
            
            printf("analyzePayload got data: length:%d\r\n", tx_message.len);
            for (loop = 0; loop < tx_message.len; loop++)
            {
                printf("data[%d]=%d\r\n", loop, can_rx_data[loop]);
            }  
            can_id_s.dest_id = (tx_message.id & 0x1FFC0000) >> 18;
            can_id_s.source_id = (tx_message.id & 0x0003FF80) >> 7;
            can_id_s.message_id = (tx_message.id & 0x0000007F) >> 0;
//            memcpy((void*)&can_rxdata_frame, (void*)can_rx_data, sizeof(can_rx_data));            
            can_mpool.free(message);   
            printf("dest_id=%d source_id=%d message_id=%d \r\n", can_id_s.dest_id, can_id_s.source_id, can_id_s.message_id);
            can_rxdata_frame.cmd = can_rx_data[0];
            can_rxdata_frame.value1 = (can_rx_data[2] << 8 ) | can_rx_data[1];
            can_rxdata_frame.value2 = (can_rx_data[4] << 8 ) | can_rx_data[3];
            can_rxdata_frame.value3 = (can_rx_data[6] << 8 ) | can_rx_data[5];
            can_rxdata_frame.value4 = can_rx_data[7];
            printf("cmd=%d, value1=0x%04x, value2=0x%04x, value3=0x%04x, value4=0x%02x \r\n", can_rxdata_frame.cmd, can_rxdata_frame.value1, can_rxdata_frame.value2, can_rxdata_frame.value3, can_rxdata_frame.value4);                             
            device_type_v = (Device_Type_d)device_type;
            switch (device_type_v)
            {
                case CupTrack:
                    printf("CupTrack command: %d \r\n", can_rxdata_frame.cmd);
                    handleCupTrackCommand(can_rxdata_frame);
                    break;
                case JamTrack:
                    printf("JamTrack command: %d \r\n", can_rxdata_frame.cmd);
                    handleJamTrackCommand(can_rxdata_frame);
                    break;
                case TeaTrack:
                    printf("TeaTrack command: %d \r\n", can_rxdata_frame.cmd);                
                    handleTeaTrackCommand(can_rxdata_frame);
                    break;
                case Tea:
                    printf("Tea command: %d \r\n", can_rxdata_frame.cmd); 
                    handleTeaCommand(can_rxdata_frame);
                    break;
                case Jam:
                    printf("Jam command: %d \r\n", can_rxdata_frame.cmd); 
                    handleJamCommand(can_rxdata_frame);
                    break;
                case Shaker:
                    printf("Shaker command: %d \r\n", can_rxdata_frame.cmd);
                    handleShakerCommand(can_rxdata_frame);
                    break;
            }

        }
    }   
        
    
}