//-------------------------------------------------------
int Compass(int mode){
if (mode == 1){
SerialWrite(port,"k\n");
return SerialRead(port,1)[0];
}
else{
SerialWrite(port,"l\n");
char *datas = SerialRead(port,2);
return (datas[0]*256+datas[1]);
}
}
char Ult(unsigned char i){
switch(i){
case 1: SerialWrite(port,"a\n");
return SerialRead(port,1)[0]; break;
case 2: SerialWrite(port,"s\n");
return SerialRead(port,1)[0]; break;
case 3: SerialWrite(port,"d\n");
return SerialRead(port,1)[0]; break;
case 4: SerialWrite(port,"f\n");
return SerialRead(port,1)[0]; break;
case 5: SerialWrite(port,"g\n");
return SerialRead(port,1)[0]; break;
}
}
char ReflexiveIR(unsigned char i){
switch(i){
case 1: SerialWrite(port,"j\n");
return SerialRead(port,1)[0]; break;
case 2: SerialWrite(port,"k\n");
return SerialRead(port,1)[0]; break;
case 3: SerialWrite(port,"l\n");
return SerialRead(port,1)[0]; break;
case 4: SerialWrite(port,"q\n");
return SerialRead(port,1)[0]; break;
case 5: SerialWrite(port,"w\n");
return SerialRead(port,1)[0]; break;
}
}
//-------------------------------------------------------
int *BallSensors(void){
SerialWrite(port,"a\n");
char *dataChar = SerialRead(port,8);
int *i = (int*)malloc(sizeof(int)*9);
for(int x=0;x<9;x++) i[x] = (int)dataChar[x];
return i;
}
void TwitterSendToOne(char *name, char *message) {
//Return the second we locate the person
if (TwitterWireMasterEnabled) {
if (wireSlaveA.id && !strcmp(wireSlaveA.name, name)) {
WireSend(wireSlaveA.id, message, strlen(message)+1, NULL);
return;
} else if (wireSlaveB.id && !strcmp(wireSlaveB.name, name)) {
WireSend(wireSlaveB.id, message, strlen(message)+1, NULL);
return;
} else if (wireSlaveC.id && !strcmp(wireSlaveC.name, name)) {
WireSend(wireSlaveC.id, message, strlen(message)+1, NULL);
return;
}
}
if (TwitterWireSlaveEnabled) {
strcpy(twitterWireSlaveMessageToSend, message);
slaveHasRequest = 1;
}
//We can't know what's downstream here, just send a message
if (TwitterSerialEnabled) {
SerialWrite(message);
}
}
void TwitterSendToAll(char *message) {
if (TwitterWireSlaveEnabled) {
if (isARelay && lastInterface != WireSlaveIF || (!isARelay)) {
//Multicast
strcpy(twitterWireSlaveMessageToSend, message);
slaveHasRequest = 1;
}
}
if (TwitterWireMasterEnabled) {
if (isARelay && lastInterface != WireMasterIF || (!isARelay)) {
//Multicast to slaves
if (wireSlaveA.id)
WireSend(wireSlaveA.id, message, strlen(message)+1, NULL);
if (wireSlaveB.id)
WireSend(wireSlaveB.id, message, strlen(message)+1, NULL);
if (wireSlaveC.id)
WireSend(wireSlaveC.id, message, strlen(message)+1, NULL);
}
}
if (TwitterSerialEnabled) {
if (isARelay && lastInterface != SerialIF || (!isARelay))
SerialWrite(message);
}
}
void inline write_FrSky8(uint8_t Data)
{
#ifdef TELEMETRY_FRSKY_SOFTSERIAL_PIN
telemSerial.write(Data);
#else
SerialWrite(TELEMETRY_FRSKY_SERIAL, Data);
#endif
}
void SerialGpsPrint(const char PROGMEM * str) {
char b;
while(str && (b = pgm_read_byte(str++))) {
SerialWrite(GPS_SERIAL, b);
#if defined(UBLOX)
delay(5);
#endif
}
}
//-------------------------------------------------------
void SetMotors(int speed1,int speed2,int speed3,int speed4){
char *bufferx = (char*)malloc(21*sizeof(char));
sprintf(bufferx,"f%4dg%4dh%4di%4d\n",speed1,speed2,speed3,speed4);
unsigned char p=0;
do{
if (bufferx[p] == ' ') bufferx[p] = '0';
}
while(bufferx[p++] != '\0');
SerialWrite(port,bufferx);
}
//------------------------------------------------------------------------------
// print a sting to the debug log file
//------------------------------------------------------------------------------
SQRESULT SQ_debugLog_print (HSQUIRRELVM v, const SQChar *pTxt)
{
if (serial.IsOpen())
{
wchar_t *inTxt = (wchar_t *)pTxt;
//fwprintf_s(gpDebugLogFile, inTxt);
char dst[100];
sprintf_s(dst, 100, "%ls", inTxt);
SerialWrite(dst);
}
return SQ_OK;
}
//------------------------------------------------------------------------------
// print an int to the debug log file
//------------------------------------------------------------------------------
SQRESULT SQ_debugLog_printInt (HSQUIRRELVM v, SQInteger val)
{
if (serial.IsOpen())
{
int inVal = (int)val;
//fprintf(gpDebugLogFile, "%d", inVal);
char dst[32];
sprintf_s(dst, 32, "%d", inVal);
SerialWrite(dst);
}
return SQ_OK;
}
//------------------------------------------------------------------------------
// print a float to the debug log file
//------------------------------------------------------------------------------
SQRESULT SQ_debugLog_printFloat (HSQUIRRELVM v, SQFloat val)
{
if (serial.IsOpen())
{
float inVal = (float)val;
//fprintf(gpDebugLogFile, "%f", inVal);
char dst[32];
sprintf_s(dst, 32, "%f", inVal);
SerialWrite(dst);
}
return SQ_OK;
}
static tVoid ReplaceLine( int* curr , int* last , char* str )
{
int idx ;
for( idx = 0 ; idx < *curr ; idx++ )
{
SerialWrite( SERIAL_PORT_MONITOR , (tUByte*)LeftString , sizeof( LeftString ) ) ;
}
for( idx = 0 ; idx < *last ; idx++ )
{
OutString( " " ) ;
}
for( idx = 0 ; idx < *last ; idx++ )
{
SerialWrite( SERIAL_PORT_MONITOR , (tUByte*)LeftString , sizeof( LeftString ) ) ;
}
OutString( str ) ;
*curr = strlen( str ) ;
*last = *curr ;
}
void PacketFinish()
{
int i, ch, count = 0;
PacketSent = 0;
SerialWrite('$');
for (i = 0; i < DataOutAddr; i++)
{
SerialWrite(DataOutBuffer[i]);
}
SerialWrite('#');
SerialWrite(hex[(DataOutCsum >> 4) & 15]);
SerialWrite(hex[DataOutCsum & 15]);
while(!chr(serport) && ((ch = SerialRead()) != '+') && (ch != '$'));
if (ch == '$')
{
ParseState = 0;
DataInAddr = 0;
ComputedCsum = 0;
}
}
//------------------------------------------------------------------------------
// print a float vector to the debug log file
//------------------------------------------------------------------------------
SQRESULT SQ_debugLog_printVector (HSQUIRRELVM v, SQFloat valX, SQFloat valY, SQFloat valZ)
{
if (serial.IsOpen())
{
float inValX = (float)valX;
float inValY = (float)valY;
float inValZ = (float)valZ;
//fprintf(gpDebugLogFile, "%f, %f, %f", inValX, inValY, inValZ);
char dst[100];
sprintf_s(dst, 100, "%f, %f, %f", inValX, inValY, inValZ);
SerialWrite(dst);
}
return SQ_OK;
}
int *LineSensors(void){
SerialWrite(port,"e\n");
char *datas = SerialRead(port,18);
int *i = (int*)malloc(sizeof(int)*9);
i[0] = (((int)datas[0]<<8)+(int)datas[1])-MinValue;
i[1] = (((int)datas[2]<<8)+(int)datas[3])-MinValue;
i[2] = (((int)datas[4]<<8)+(int)datas[5])-MinValue;
i[3] = (((int)datas[6]<<8)+(int)datas[7])-MinValue;
i[4] = (((int)datas[8]<<8)+(int)datas[9])-MinValue;
i[5] = (((int)datas[10]<<8)+(int)datas[11])-MinValue;
i[6] = (((int)datas[12]<<8)+(int)datas[13])-MinValue;
i[7] = (((int)datas[14]<<8)+(int)datas[15])-MinValue;
i[8] = (((int)datas[16]<<8)+(int)datas[17])-MinValue;
for (unsigned char a=0;a<9;a++){
if (i[a] < 0) i[a] = 0;
}
return i;
}
tBoolean CliGet()
{
static int idx = 0 ;
static int idxMax = 0 ;
static tBoolean esc = FALSE ;
tUByte inbyte ;
tBoolean enterHit = FALSE ;
tUByte* ptr ;
int size ;
static int lineCurrent = 0 ;
static int lineBuffer = 0 ;
static int lineMax = 0 ;
static tBoolean prompt = FALSE ;
int echo = FALSE ;
ptr = &inbyte ;
size = 1 ;
if( !prompt )
{
OutString( ">" ) ;
prompt = TRUE ;
}
if( SerialRead( SERIAL_PORT_MONITOR , &inbyte , 1 ) )
{
if( inbyte == ESCAPE_KEY )
{
esc = TRUE ;
}
else if( esc )
{
if( inbyte == ESCAPE2_KEY ) /* ignore after escape */
{
inbyte = 0 ;
}
else {
switch( inbyte )
{
case UP_KEY :
inbyte = 0 ;
lineBuffer-- ;
if( lineBuffer < 0 )
{
lineBuffer = lineMax-1 ;
}
memcpy( sLineCurrent , (char*)sLineBuf[ lineBuffer ] , sizeof( sLineCurrent ) ) ;
ReplaceLine( &idx , &idxMax , (char*)sLineCurrent ) ;
break ;
case DOWN_KEY :
inbyte = 0 ;
lineBuffer++ ;
if( lineBuffer >= lineMax )
{
lineBuffer = 0 ;
}
memcpy( sLineCurrent , (char*)sLineBuf[ lineBuffer ] , sizeof( sLineCurrent ) ) ;
ReplaceLine( &idx , &idxMax , (char*)sLineCurrent ) ;
break ;
case RIGHT_KEY :
inbyte = MAP_RIGHT ;
ptr = (tUByte*)RightString ;
size = sizeof( RightString ) ;
break ;
case LEFT_KEY :
inbyte = MAP_LEFT ;
ptr = (tUByte*)LeftString ;
size = sizeof( LeftString ) ;
break ;
default :
inbyte = 0 ;
break ;
}
esc = FALSE ;
}
}
if( ( inbyte == BACKSPACE_KEY )
|| ( inbyte == ENTER_KEY )
|| ( inbyte == MAP_LEFT )
|| ( inbyte == MAP_RIGHT )
|| ( ( inbyte >= ' ' ) && ( inbyte <= '~' ) ) )
{
if( inbyte == ENTER_KEY )
{
sLineCurrent[ idxMax ] = 0 ; /* null terminate string */
idx = 0 ;
idxMax = 0 ;
enterHit = TRUE ;
memcpy( sLineBuf[ lineCurrent ] , sLineCurrent , sizeof( sLineCurrent ) ) ;
lineCurrent++ ;
if( lineCurrent >= BUFFER_LINES )
{
lineCurrent = 0 ;
}
lineBuffer = lineCurrent ;
if( lineCurrent > lineMax )
{
lineMax = lineCurrent ;
}
prompt = FALSE ;
echo = TRUE ;
}
else if( inbyte == BACKSPACE_KEY )
{
if( idx )
{
SerialWrite( SERIAL_PORT_MONITOR , (tUByte*)LeftString , sizeof( LeftString ) ) ;
OutString( " " ) ;
SerialWrite( SERIAL_PORT_MONITOR , (tUByte*)LeftString , sizeof( LeftString ) ) ;
idx-- ;
}
}
else if( inbyte == MAP_LEFT )
{
if( idx )
{
idx-- ;
echo = TRUE ;
}
}
else if( ( inbyte == MAP_RIGHT ) )
{
if( idx < idxMax )
{
idx++ ;
echo = TRUE ;
}
}
else if( idx < ( LINE_BUF_SIZE - 1 ) )
{
sLineCurrent[ idx++ ] = inbyte ;
echo = TRUE ;
}
}
}
if( echo )
{
SerialWrite( SERIAL_PORT_MONITOR , ptr , size ) ;
}
if( idx > idxMax )
{
idxMax = idx ;
}
return enterHit ;
}
void LED_IR(bool status){
if (status == 0) SerialWrite(port,"6\n");
else SerialWrite(port,"7\n");
}
void MotorR(int speed){
SerialWrite(port,int2char("u",speed,true));
}
void *threadSerialPCPrinterFunction(void *message) {
int ret;
char SerialPOSBuf[BUF_MAX];
time_t CurrentTime = 0 ;
time_t LastTime = 0;
int i;
int WriteFlag = 0;
ret = pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
if (0!=ret)
{
perror("Thread pthread_setcancelstat failed");
pthread_exit(NULL);
}
ret = pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);
if (0!=ret)
{
perror("Thread pthread_setcanceltype failed");
pthread_exit(NULL);
}
Message.data_length = 0;
while(1) {
pthread_mutex_lock(&(Message.data_lock));
pthread_mutex_lock(&(Response.data_lock));
if (Response.data_length>0)
{
int ret;
printf("Write to PC %d Datas!\n", Response.data_length);
ret = SerialWrite(SerialReadfd, &(Response.data[0]), Response.data_length);
if(ret!=Response.data_length)
{
printf("Response to PC failed!");
}
else
{
Response.data_length = 0;
}
}
pthread_mutex_unlock(&(Response.data_lock));
if(Message.data_length<=0)
{
//printf("hhhhhhhhhh\n");
//memset(&(Message.data[0]), '\0', BUF_MAX);
//Message.data_length = SerialRead(SerialReadfd, &(Message.data[0]), sizeof(Message.data));
Message.data_length = SerialRead(SerialReadfd, &(Message.data[0]), sizeof(Message.data));
//printf("Data length = %d\n", Message.data_length);
if(Message.data_length > 0)
{
//#define rdtscll(val) __asm__ __volatile__("rdtsc" : "=A" (val))
int n = 0;
printf("Received %d bytes data\n", Message.data_length);
LastTime = time(NULL);
/* for(n=0; n<Message.data_length; ++n)
{
printf("%x\t", Message.data[n]);
}*/
//fflush(stdout);
WriteARecord(&Message.data[0], Message.data_length);//Write data to a file;
// LOGD("%s(%d)-%s:",__FILE__,__LINE__,__FUNCTION__);
WriteFlag = 1;
// Message.data_length = 0;
pthread_mutex_unlock(&Message.data_lock);
msSleep(10);
}
else
{
CurrentTime = time(NULL);
if (((CurrentTime - LastTime)>5)&&(1 == WriteFlag))
{
WriteFlag = 0;
WriteARecord(&Message.data[0], 0);//close the crrent file ,and creat a queue member;
printf("Creat file!\n");
}
/* printf("no data in serialread!\n"); */
pthread_mutex_unlock(&Message.data_lock);
msSleep(10);
}
}
else
{
/* printf("Length>0\n"); */
pthread_mutex_unlock(&Message.data_lock);
msSleep(10);
}
}
pthread_exit(0);
}
char Buttons(void){
SerialWrite(port,"h\n");
return SerialRead(port,1)[0];
}
int SerialInterrupt(int signal, ppc_trap_frame_t *tf)
{
int ch;
if (!chr(serport)) return 0;
Signal = signal;
RegisterSaveArea = tf;
do
{
ch = SerialRead();
if (ch == 3) /* Break in - tehe */
{
Continue = 0;
PacketWriteSignal(3);
}
else if (ch == '+')
{
/* Nothing */
}
else if (ch == '$')
{
DataInAddr = 0;
ParseState = 0;
ComputedCsum = 0;
ActualCsum = 0;
}
else if (ch == '#' && ParseState == 0)
{
ParseState = 2;
}
else if (ParseState == 0)
{
ComputedCsum += ch;
DataInBuffer[DataInAddr++] = ch;
}
else if (ParseState == 2)
{
ActualCsum = ch;
ParseState++;
}
else if (ParseState == 3)
{
ActualCsum = hex2int(ch) | (hex2int(ActualCsum) << 4);
ComputedCsum &= 255;
ParseState = -1;
if (ComputedCsum == ActualCsum)
{
ComputedCsum = 0;
DataInBuffer[DataInAddr] = 0;
DataInAddr = 0;
Continue = 0;
SerialWrite('+');
GotPacket();
}
else
SerialWrite('-');
}
else if (ParseState == -1)
SerialWrite('-');
}
while (!Continue);
return 1;
}
void InitBlaze(const char *port_name,speed_t speed){
port = SerialOpen(port_name,speed);
SerialWrite(port,int2char("b",MaxValue,true));
}
void SetMotors(int speedA,int speedB){
char *buffer = int2char("y",speedA,false);
SerialWrite(port,int2char(buffer,speedB,true));
}
void GPS_SerialInit(void) {
SerialOpen(GPS_SERIAL,GPS_BAUD);
delay(1000);
#if defined(UBLOX)
for(uint8_t i=0;i<5;i++){
SerialOpen(GPS_SERIAL,init_speed[i]); // switch UART speed for sending SET BAUDRATE command (NMEA mode)
#if (GPS_BAUD==19200)
SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,19200,0*23\r\n")); // 19200 baud - minimal speed for 5Hz update rate
#endif
#if (GPS_BAUD==38400)
SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,38400,0*26\r\n")); // 38400 baud
#endif
#if (GPS_BAUD==57600)
SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,57600,0*2D\r\n")); // 57600 baud
#endif
#if (GPS_BAUD==115200)
SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,115200,0*1E\r\n")); // 115200 baud
#endif
while(!SerialTXfree(GPS_SERIAL)) delay(10);
}
delay(200);
SerialOpen(GPS_SERIAL,GPS_BAUD);
for(uint8_t i=0; i<sizeof(UBLOX_INIT); i++) { // send configuration data in UBX protocol
SerialWrite(GPS_SERIAL, pgm_read_byte(UBLOX_INIT+i));
delay(5); //simulating a 38400baud pace (or less), otherwise commands are not accepted by the device.
}
#elif defined(INIT_MTK_GPS) // MTK GPS setup
for(uint8_t i=0;i<5;i++){
SerialOpen(GPS_SERIAL,init_speed[i]); // switch UART speed for sending SET BAUDRATE command
#if (GPS_BAUD==19200)
SerialGpsPrint(PSTR("$PMTK251,19200*22\r\n")); // 19200 baud - minimal speed for 5Hz update rate
#endif
#if (GPS_BAUD==38400)
SerialGpsPrint(PSTR("$PMTK251,38400*27\r\n")); // 38400 baud
#endif
#if (GPS_BAUD==57600)
SerialGpsPrint(PSTR("$PMTK251,57600*2C\r\n")); // 57600 baud
#endif
#if (GPS_BAUD==115200)
SerialGpsPrint(PSTR("$PMTK251,115200*1F\r\n")); // 115200 baud
#endif
while(!SerialTXfree(GPS_SERIAL)) delay(80);
}
// at this point we have GPS working at selected (via #define GPS_BAUD) baudrate
// So now we have to set the desired mode and update rate (which depends on the NMEA or MTK_BINARYxx settings)
SerialOpen(GPS_SERIAL,GPS_BAUD);
SerialGpsPrint(MTK_NAVTHRES_OFF);
while(!SerialTXfree(GPS_SERIAL)) delay(80);
SerialGpsPrint(SBAS_ON);
while(!SerialTXfree(GPS_SERIAL)) delay(80);
SerialGpsPrint(WAAS_ON);
while(!SerialTXfree(GPS_SERIAL)) delay(80);
SerialGpsPrint(SBAS_TEST_MODE);
while(!SerialTXfree(GPS_SERIAL)) delay(80);
SerialGpsPrint(MTK_OUTPUT_5HZ); // 5 Hz update rate
#if defined(NMEA)
SerialGpsPrint(MTK_SET_NMEA_SENTENCES); // only GGA and RMC sentence
#endif
#if defined(MTK_BINARY19) || defined(MTK_BINARY16)
SerialGpsPrint(MTK_SET_BINARY);
#endif
#endif //elif init_mtk_gps
}
void LED_R(bool status){
if (status == 0) SerialWrite(port,"2\n");
else SerialWrite(port,"3\n");
}
void *threadSerialPrinterPCFunction(void *message) {
int ret;
char SerialPrinterBuf[BUF_MAX];
int i;
int fd;
//ret = open
ret = pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
if (0!=ret)
{
perror("Thread pthread_setcancelstat failed");
pthread_exit(NULL);
}
ret = pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);
if (0!=ret)
{
perror("Thread pthread_setcanceltype failed");
pthread_exit(NULL);
}
while(1) {
// memset(SerialPrinterBuf, '\0', BUF_MAX);
/* printf("LOCK"); */
pthread_mutex_lock(&Message.data_lock);
pthread_mutex_lock(&Response.data_lock);
if(Response.data_length<=0)
{
int n = 0;
Response.data_length = SerialRead(SerialWritefd, &(Response.data[0]), sizeof(Response.data));
if(Response.data_length>0)
{
printf("Receive Data length is = %d---------------------------------------------------\n", Response.data_length);
/*for(n=0; n<Response.data_length; ++n)
{
printf("%x\t", Response.data[n]);
}*/
}
/* else
{
printf("NO DATA FROM REPONSE!\n");
}*/
}
pthread_mutex_unlock(&Response.data_lock);
/* printf("Locked!"); */
//SerialWrite(SerialWritefd, "fuckfuckfuckfuckfuckfuckfuck", 28);
/* printf("Length= %d", Message.data_length); */
// ret = SerialRead(SerialWritefd, SerialPrinterBuf, sizeof(SerialPrinterBuf));
if(Message.data_length > 0)
{
int tmp = 0;
/* char tmp1[11]= "abcdeferty";//{a, b, c, d, e, f, g, g, g, g}; */
/* SerialWrite(SerialWritefd, tmp1, 10); */
/* Message.data_length = 0; */
/* pthread_mutex_unlock(&Message.data_lock); */
/* continue; */
//tmp =
//tmp = SerialWrite(SerialWritefd, Message.data, Message.data_length);
printf("in wirte!\n");
tmp = SerialWrite(SerialWritefd, Message.data, Message.data_length);
if (Message.data_length!=tmp)//write data to printer
{
printf("actual num = %d, therial num = %d\n", tmp, Message.data_length);
PRINTERR("Write wrong!\n")
//pthread_mutex_unlock(&Message.data_lock);
//pthread_exit(NULL);
}
else
{
void LED_G(bool status){
if (status == 0) SerialWrite(port,"4\n");
else SerialWrite(port,"5\n");
}
void LED_Y(bool status){
if (status == 0) SerialWrite(port,"0\n");
else SerialWrite(port,"1\n");
}
void InitMotors(char mode){
if (mode == 'a') SerialWrite(port,"8\n");
else SerialWrite(port,"9\n");
}
