UsaPack for mbed OS 6

Dependents:   mbedOS_UsaPackOS

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers UsaPackOS.cpp Source File

UsaPackOS.cpp

00001 #include "UsaPackOS.hpp"
00002 #include "mbed.h"
00003 
00004 
00005 void UsaPackOS::Receive()
00006 {
00007         while (serial.readable())
00008         {
00009             char c;
00010             serial.read(&c, 1);
00011             receive_buffer[receive_index] = c;
00012             receive_index = (receive_index + 1) % receive_size;
00013             if (c == '\0')
00014             {
00015 //                printf("DEC\r\n");
00016                 Decode();
00017                 receive_index = 0;
00018             }
00019         }
00020 }
00021 
00022 void UsaPackOS::SendByte()
00023 {
00024     while (serial.writable())
00025     {
00026         if (send_index != send_end_index)
00027         {
00028             serial.write(&send_buffer[send_index],1);
00029             send_index = (send_index + 1) % send_size;
00030         }
00031         else
00032         {
00033             serial.attach(NULL, UnbufferedSerial::TxIrq);
00034             break;
00035         }
00036     }
00037 }
00038 
00039 void UsaPackOS::Decode()
00040 {
00041     char data[receive_size];
00042 //    for (int i = 0; i < 43; i++) {
00043 //        printf("%02x ", receive_buffer[i]);
00044 //    }
00045 //    printf("\r\n");
00046 //    printf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x \r\n",receive_buffer[0],receive_buffer[1],receive_buffer[2],receive_buffer[3],receive_buffer[4],receive_buffer[5],receive_buffer[6],receive_buffer[7],receive_buffer[8],receive_buffer[9],receive_buffer[10],receive_buffer[11]);
00047     CobsDecode(receive_buffer, receive_index-2, data);
00048 //    printf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x \r\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11]);
00049 
00050 //    for (int i = 0; i < 43; i++) {
00051 //        printf("%02x ", data[i]);
00052 //    }
00053 //    printf("\r\n");
00054     union c4
00055     {
00056         char c[4];
00057         int a;
00058     };
00059     c4 _address;
00060     c4 _checksum;
00061     for (int i = 0; i < 4; i++)
00062     {
00063         _address.c[i] = data[i];
00064     }
00065 //    printf("%d\r\n",package_index);
00066     for (int i = 0; i < 4; i++)
00067     {
00068         _checksum.c[i] = data[i+4];
00069     }
00070     for (int i = 0; i < package_index; i++)
00071     {
00072         if (package_address[i] == _address.a)
00073         {
00074 //            printf("%\r\n",package_size[0]);
00075             int checksum = 0;
00076             for (int j = 0; j < package_size[i]; j++)
00077             {
00078                 checksum += data[j+8];
00079             }
00080 //            printf("check %d %d\r\n",_checksum.a,checksum);
00081             if (_checksum.a != checksum)
00082             {
00083 //                printf("not checksum\r\n");
00084                 return;
00085             }
00086             for (int j = 0; j < package_size[i]; j++)
00087             {
00088                 *((char*)(package_object[i])+j) = data[j+8];
00089             }
00090 //            printf("Dec ok\r\n");
00091             break;
00092         }
00093     }
00094 }
00095 
00096 UsaPackOS::UsaPackOS(PinName tx, PinName rx, int baud)
00097     :serial(tx, rx, baud), receive_index(0), package_index(0), send_index(0), send_end_index(0)
00098 {
00099     serial.attach(callback(this, &UsaPackOS::Receive), UnbufferedSerial::RxIrq);
00100     for (int i = 0; i < receive_size; i++)
00101     {
00102         receive_buffer[i] = '\0';
00103     }
00104     //receive_thread.start(callback(this, &UsaPackOS::ThreadReceive));
00105     
00106 }
00107 
00108 void UsaPackOS::CobsEncode(char data[], int length, char send_data[])
00109 {
00110     int last_zero = 0;
00111     for (int i = 0; i < length; i++)
00112     {
00113         if (data[i] == 0)
00114         {
00115             send_data[last_zero] = i - last_zero + 1;
00116             last_zero = i + 1;
00117         }
00118         else
00119         {
00120             send_data[i + 1] = data[i];
00121         }
00122     }
00123     send_data[last_zero] = length - last_zero + 1;
00124     send_data[length + 1] = 0;
00125 }
00126 
00127 void UsaPackOS::CobsDecode(char receive_data[], int length, char data[])
00128 {
00129     int next_zero;
00130     next_zero = receive_data[0] - 1;
00131     for (int i = 0; i < length; i++)
00132     {
00133         data[i] = receive_data[i + 1];
00134     }
00135     while (next_zero < length)
00136     {
00137         data[next_zero] = 0;
00138         next_zero += receive_data[next_zero + 1];
00139     }
00140 }
00141