368 lines
10 KiB
C
368 lines
10 KiB
C
/*******************************************************************************
|
|
* *
|
|
* Copyright 2010 Rheinmetall Canada Inc. *
|
|
* *
|
|
* No part of this document may be reproduced, stored in *
|
|
* a retrieval system, or transmitted, in any form or by any means, *
|
|
* electronic, mechanical, photocopying, recording, or otherwise, *
|
|
* without the prior written permission of Rheinmetall Canada Inc. *
|
|
* *
|
|
*******************************************************************************/
|
|
/*
|
|
Description:
|
|
This is a template file for standard C code file.
|
|
|
|
*/
|
|
|
|
/* ************************************************************************** */
|
|
/* ¤Revision:
|
|
000 20100616 JFM,
|
|
Original version.
|
|
|
|
### YYYYMMDD Initial, Bug Identification
|
|
Change description.
|
|
*/
|
|
|
|
/* ************************************************************************** */
|
|
/* Includes */
|
|
#include "define.h"
|
|
#include "Util.h"
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include "PrintfServer.h"
|
|
//#include "Watchdog.h"
|
|
|
|
/* ************************************************************************** */
|
|
/* Local variables */
|
|
#ifdef ENABLE_DEBUG_LOG
|
|
char acDebugLog[DEBUG_LOG_SIZE][100];
|
|
int iLogIndex = 0;
|
|
int iWrapCntr = 0;
|
|
#endif
|
|
|
|
//void _mon_putc(char c); //override from stdio to redirect stdout
|
|
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//void _mon_putc(char c)
|
|
//{
|
|
// TelnetPutPrintf(c);
|
|
//}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
short SwapEndianShort(short p_sData)
|
|
{
|
|
return (((p_sData & 0xFF00) >> 8) | ((p_sData & 0x00FF) << 8));
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
int SwapEndianInt(int p_iData)
|
|
{
|
|
return (((p_iData & 0xFF000000) >> 24) | ((p_iData & 0x00FF0000) >> 8) | ((p_iData & 0x0000FF00) << 8) | ((p_iData & 0x000000FF) << 24));
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
#ifdef ENABLE_DEBUG_LOG
|
|
char* GetDebugLogPtr(void)
|
|
{
|
|
char *pcPtr;
|
|
|
|
if(iLogIndex == DEBUG_LOG_SIZE)
|
|
{
|
|
iLogIndex = 0;
|
|
iWrapCntr++;
|
|
}
|
|
|
|
memset(&acDebugLog[iLogIndex][0],0,sizeof(acDebugLog[iLogIndex]));
|
|
pcPtr = &acDebugLog[iLogIndex][0];
|
|
|
|
iLogIndex++;
|
|
|
|
return pcPtr;
|
|
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
void ClearDebugLog(void)
|
|
{
|
|
int i;
|
|
for(i = 0; i < DEBUG_LOG_SIZE; i++)
|
|
{
|
|
memset(&acDebugLog[i][0],0,sizeof(acDebugLog[i]));
|
|
}
|
|
|
|
iLogIndex = 0;
|
|
iWrapCntr = 0;
|
|
}
|
|
|
|
#endif
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
int ConvertIntToStrLeadingZero(char* p_pcString, unsigned int p_iChar, int digit)
|
|
{
|
|
char temp[8];
|
|
int i=0;
|
|
sprintf(temp, "%X", p_iChar);
|
|
//itoa(p_cChar,temp,radix);
|
|
if(p_iChar < 0xF)
|
|
{
|
|
for (i=0; i<digit-1; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
else if(p_iChar < 0xFF)
|
|
{
|
|
for (i=0; i<digit-2; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
else if(p_iChar < 0xFFF)
|
|
{
|
|
for (i=0; i<digit-3; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
else if(p_iChar < 0xFFFF)
|
|
{
|
|
for (i=0; i<digit-4; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
else if(p_iChar < 0xFFFFF)
|
|
{
|
|
for (i=0; i<digit-5; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
else if(p_iChar < 0xFFFFFF)
|
|
{
|
|
for (i=0; i<digit-6; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
else if(p_iChar < 0xFFFFFFF)
|
|
{
|
|
for (i=0; i<digit-7; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
int ConvertIntToStr(char* p_pcString, unsigned int p_iChar)
|
|
{
|
|
char temp[8];
|
|
int i=0;
|
|
|
|
sprintf(temp, "%X", p_iChar);
|
|
if(p_iChar < 0xF)
|
|
{
|
|
p_pcString[0] = temp[0];
|
|
return 1;
|
|
}
|
|
else if(p_iChar < 0xFF)
|
|
{
|
|
for (i=0; i<2; i++)
|
|
p_pcString[i] = temp[i];
|
|
return 2;
|
|
}
|
|
else if(p_iChar < 0xFFF)
|
|
{
|
|
for (i=0; i<3; i++)
|
|
p_pcString[i] = temp[i];
|
|
return 3;
|
|
}
|
|
else if(p_iChar < 0xFFFF)
|
|
{
|
|
for (i=0; i<4; i++)
|
|
p_pcString[i] = temp[i];
|
|
return 4;
|
|
}
|
|
else if(p_iChar < 0xFFFFF)
|
|
{
|
|
for (i=0; i<5; i++)
|
|
p_pcString[i] = temp[i];
|
|
return 5;
|
|
}
|
|
else if(p_iChar < 0xFFFFFF)
|
|
{
|
|
for (i=0; i<6; i++)
|
|
p_pcString[i] = temp[i];
|
|
return 6;
|
|
}
|
|
else if(p_iChar < 0xFFFFFFF)
|
|
{
|
|
for (i=0; i<7; i++)
|
|
p_pcString[i] = temp[i];
|
|
return 7;
|
|
}
|
|
else if(p_iChar < 0xFFFFFFFF)
|
|
{
|
|
for (i=0; i<8; i++)
|
|
p_pcString[i] = temp[i];
|
|
return 8;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
int ConvertCharToStrLeadingZero(char* p_pcString, unsigned char p_cChar, int digit)
|
|
{
|
|
char temp[2];
|
|
int i=0;
|
|
sprintf(temp, "%X", p_cChar);
|
|
//itoa(p_cChar,temp,radix);
|
|
if(p_cChar < 0xF)
|
|
{
|
|
for (i=0; i<digit-1; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
else if(p_cChar < 0xFF)
|
|
{
|
|
for (i=0; i<digit-2; i++)
|
|
p_pcString[i] = '0'; //Add a Zero in front of the Character
|
|
memcpy(&p_pcString[i], &temp[0], digit-i);
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
unsigned int ConvertStrToValue(char* p_pcString, int p_iSize, int p_iRadix)
|
|
{
|
|
int i=0;
|
|
int j=0;
|
|
unsigned int uiValue = 0;
|
|
unsigned int uiFactor = 1;
|
|
|
|
if (p_iRadix == 16)
|
|
{
|
|
for (i=p_iSize-1; i>=0; i--)
|
|
{
|
|
if (i==p_iSize-1)
|
|
{
|
|
uiValue = p_pcString[i] - 0x30;
|
|
}
|
|
else
|
|
{
|
|
for (j=i; j<=p_iSize-2; j++)
|
|
uiFactor *= 16;
|
|
uiValue += (p_pcString[i] - 0x30) * uiFactor;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
else if (p_iRadix == 10)
|
|
{
|
|
for (i=0; i<p_iSize; i++)
|
|
{
|
|
if (i==0)
|
|
uiFactor = 1;
|
|
else
|
|
{
|
|
for (j=0; j<i; j++)
|
|
uiFactor *= 10;
|
|
}
|
|
|
|
uiValue += (p_pcString[i] - 0x30) * uiFactor;
|
|
}
|
|
|
|
}
|
|
|
|
return uiValue;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
int ConvertCharToStr(char* p_pcString, unsigned char p_cChar)
|
|
{
|
|
char temp[2];
|
|
|
|
sprintf(temp, "%X", p_cChar);
|
|
if(p_cChar < 0xF)
|
|
{
|
|
p_pcString[0] = temp[0];
|
|
return 1;
|
|
}
|
|
else if(p_cChar < 0xFF)
|
|
{
|
|
p_pcString[0] = temp[0];
|
|
p_pcString[1] = temp[1];
|
|
return 2;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//-----------------------------------------------------------------------------
|
|
// CRC-8 table (256 bytes)
|
|
// Crc table for poly: $C7
|
|
const int CRC8_TABLE[256] = { 0, 199, 73, 142, 146, 85, 219, 28, 227, 36, 170, 109, 113, 182, 56, 255,
|
|
1, 198, 72, 143, 147, 84, 218, 29, 226, 37, 171, 108, 112, 183, 57, 254,
|
|
2, 197, 75, 140, 144, 87, 217, 30, 225, 38, 168, 111, 115, 180, 58, 253,
|
|
3, 196, 74, 141, 145, 86, 216, 31, 224, 39, 169, 110, 114, 181, 59, 252,
|
|
4, 195, 77, 138, 150, 81, 223, 24, 231, 32, 174, 105, 117, 178, 60, 251,
|
|
5, 194, 76, 139, 151, 80, 222, 25, 230, 33, 175, 104, 116, 179, 61, 250,
|
|
6, 193, 79, 136, 148, 83, 221, 26, 229, 34, 172, 107, 119, 176, 62, 249,
|
|
7, 192, 78, 137, 149, 82, 220, 27, 228, 35, 173, 106, 118, 177, 63, 248,
|
|
8, 207, 65, 134, 154, 93, 211, 20, 235, 44, 162, 101, 121, 190, 48, 247,
|
|
9, 206, 64, 135, 155, 92, 210, 21, 234, 45, 163, 100, 120, 191, 49, 246,
|
|
10, 205, 67, 132, 152, 95, 209, 22, 233, 46, 160, 103, 123, 188, 50, 245,
|
|
11, 204, 66, 133, 153, 94, 208, 23, 232, 47, 161, 102, 122, 189, 51, 244,
|
|
12, 203, 69, 130, 158, 89, 215, 16, 239, 40, 166, 97, 125, 186, 52, 243,
|
|
13, 202, 68, 131, 159, 88, 214, 17, 238, 41, 167, 96, 124, 187, 53, 242,
|
|
14, 201, 71, 128, 156, 91, 213, 18, 237, 42, 164, 99, 127, 184, 54, 241,
|
|
15, 200, 70, 129, 157, 90, 212, 19, 236, 43, 165, 98, 126, 185, 55, 240 };
|
|
|
|
//---------------------------------------------------------------------------
|
|
//
|
|
// Function: Crc8
|
|
//
|
|
// Parameter: char* message
|
|
// int number of characters in the command
|
|
//
|
|
// Descrip: calc the crc of a commans string
|
|
//
|
|
// Author: ADubreuil (12/05/06)
|
|
//
|
|
//---------------------------------------------------------------------------
|
|
int Crc8(char* answer, int length)
|
|
{
|
|
int i,crc,key;
|
|
|
|
crc = 0;
|
|
for(i=0;i<length;i++) {
|
|
key = CRC8_TABLE[crc];
|
|
crc = answer[i] ^ key;
|
|
}
|
|
|
|
return crc;
|
|
}
|
|
|
|
//---------------------------------------------------------------------------
|
|
void Delay(unsigned int count)
|
|
{
|
|
// KickWatchdog();
|
|
// while(--count);
|
|
// KickWatchdog();
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
//EOF
|
|
|