/**************************************Task to read Vicon Data*********************************/
void Vicon_read(void *){
int fdvicon;
SerialClose(8);
fdvicon = OpenSerial( 8 , 115200, 1, 8, eParityNone );
uint8_t i=0;
char vicon_input[100]={0};
while(1){
read(fdvicon,&vicon_input[0],1);
if((uint8_t)vicon_input[0]==header1){
read(fdvicon,&vicon_input[1],1);
read(fdvicon,&vicon_input[2],1);
if((uint8_t)vicon_input[1]==header2 && (uint8_t)vicon_input[2]==header3){
for(i=3;i<16;i++)
read(fdvicon,&vicon_input[i],1);
vicon_control.roll=(double)((int16_t)((uint16_t)vicon_input[3]*256)+(uint8_t)vicon_input[4])/10;
vicon_control.pitch=(double)((int16_t)((uint16_t)vicon_input[5]*256)+(uint8_t)vicon_input[6])/10;
vicon_control.yaw=(double)((int16_t)((uint16_t)vicon_input[7]*256)+(uint8_t)vicon_input[8])/2;
vicon_control.x=(double)((int16_t)((uint16_t)vicon_input[9]*256)+(uint8_t)vicon_input[10]/54);
vicon_control.y=(double)((int16_t)((uint16_t)vicon_input[11]*256)+(uint8_t)vicon_input[12])/54;
vicon_control.z=(double)((int16_t)((uint16_t)vicon_input[13]*256)+(uint8_t)vicon_input[14])/10;
}//second receive if
}//first receive if
}//Vicon Task While Loop
}//Vicon_Task Bracket
void CloseSocketModbusMaster( void )
{
int ScanClientSock;
ClientSocketRunning = 0;
#ifdef __WIN32__
if ( ThreadHandleClient )
TerminateThread( ThreadHandleClient, 0);
#endif
// close sockets
for( ScanClientSock=0; ScanClientSock<NBR_CLIENTS_MAX; ScanClientSock++ )
{
if ( ClientSocketOpened[ ScanClientSock ]!=-1 )
{
#ifdef __WIN32__
closesocket( ClientSockDescrip[ ScanClientSock ] );
#else
close( ClientSockDescrip[ ScanClientSock ] );
#endif
}
ClientSocketOpened[ ScanClientSock ] = -1;
ClientSockDescrip[ ScanClientSock ] = SOCK_INVALID;
}
if ( ModbusSerialPortNameUsed[ 0 ]!='\0' )
SerialClose( );
printf(_("INFO CLASSICLADDER---I/O modbus master closed!\n"));
}
//------------------------------------------------------------------------------
void SerialReInit(HANDLE * hCom, int PortNum, int BaudRate, int dataBit,
int parity, int cts, int rts, int dsr, int dtr)
{
SerialClose(*hCom);
*hCom =
SerialInit(PortNum, BaudRate, dataBit, parity, cts, rts, dsr, dtr);
}
int ReadChar(unsigned char *pChar)
{
if (g_hSerialDevFd <= 0)
{
TRACEERR("dev not open\n");
return ReturnError;
}
//TRACE("ReadChar before\n");
//pthread_mutex_lock(&g_hRdWrMutex);
//tcflush(g_hSerialDevFd, TCIFLUSH);
//gettimeofday(&tvs2, NULL);
/*read 只有一个线程执行，不需要锁*/
if (0 >= read(g_hSerialDevFd, pChar, 1))
{
g_nReadErr++;
TRACEERR("read return err\n");
if (READ_ERR_NUM <= g_nReadErr)
{
TRACEERR("us unlink\n");
SerialClose();
ClearInitSourceMac(); /*清除网关mac和远程服务器信息,换网关需要重新注册才可用*/
//ClearNodeList(); /*不清除节点信息*/
}
return ReturnError;
}
g_nReadErr = 0;
//gettimeofday(&tve2, NULL);
//TRACE("Read char : %u\n", (tve2.tv_sec - tvs2.tv_sec)*1000000 + tve2.tv_usec - tvs2.tv_usec);
//pthread_mutex_unlock(&g_hRdWrMutex);
//TRACE("ReadChar:%02x\n", *pChar);
return ReturnSuccess;
}
serialCommunicator::~serialCommunicator()
{
SerialClose();
//delete serial;
delete newSerial;
delete pTimer;
//delete ptr_io_service;
}
int USBRelayOutput::Close(void)
{
LogDebug(VB_CHANNELOUT, "USBRelayOutput::Close()\n");
SerialClose(m_fd);
m_fd = -1;
return ChannelOutputBase::Close();
}
int USBRenard_Close(void *data) {
LogDebug(VB_CHANNELOUT, "USBRenard_Close(%p)\n", data);
USBRenardPrivData *privData = (USBRenardPrivData*)data;
USBRenard_Dump(privData);
SerialClose(privData->fd);
privData->fd = -1;
}
void initPINS()
{
int fdDebug=0;
J2[31].function(3);//UART2 RX
//J2[22].function(3);//UART1 TX
SerialClose(0);
fdDebug=OpenSerial(0,115200,1,8,eParityNone);
}
int Pro_Hw_Setup(const char *device,int baudrate)
{
if(SerialStart(device,baudrate) < 0)
{
SerialClose();
return -1;
}
if(SerialStartThread() < 0)
{
return -1;
}
return 0;
}
int main(int argc,char**argv)
{
int rtn;
int Count = 0;
unsigned char *send = "Hello Uart";
unsigned char buf[1024] ={0};
if(2 > argc)
{
printf("Input error\n");
printf("Usage: %s <Device>\n",argv[0]);
printf("Eample: %s /dev/ttyS0\n",argv[0]);
return -1;
}
rtn = SerialOpen(argv[1]);
if(0 > rtn)
return -1;
rtn = SerialConfig(115200,8,'n',1);
if(0 > rtn)
return -1;
while(1)
{
rtn = SerialSend(send,strlen(send));
if(0 < rtn)
{
printf("[Send]: %s\n",send);
}
sleep(1);
rtn = SerialReceive(buf,sizeof(buf));
if(0 < rtn)
{
printf("[Recv]: %s\n",buf);
}
}
SerialClose();
}
int main(int argc, char* argv[])
{
unsigned int ser = SerialOpen(115200); //Open serial port with a 115200 baud rate
int a = 0;
SerialFlush(ser); //Clear TX and RX buffer, can be called periodically
SerialPuts(ser,"Serial communication using the Sensorian Shield.\r\n");
SerialPrintf(ser,"You are using device number %d.\r\n",ser);
SerialFlush(ser);
while(1)
{ if(SerialDataAvail(ser))
{
a = SerialGetchar(ser);
SerialPrintf(ser,"You sent %c.\r\n",(char)(a));
if(a == 'b') break;
}
}
SerialPrintf(ser,"Closing serial port ...");
SerialClose(ser); //Close serial port
return 0;
}
void ClearExternalIO(void) {
int i;
// GS I2C Start
i2c_disable();
i2c_slave_disable();
// GS I2C End
if (SerialConsole != 1) SerialClose(1);
if (SerialConsole != 2) SerialClose(2);
if (SerialConsole != 3) SerialClose(3);
if (SerialConsole != 4) SerialClose(4);
// stop the sound
SoundPlay = 0;
CloseTimer2();
#ifdef OLIMEX
CloseOC1();
#else
CloseOC2();
#endif
inttbl[NBRPINS + 2].intp = NULL; // disable the tick interrupt
for (i = 1; i < NBRPINS + 1; i++) {
inttbl[i].intp = NULL; // disable all interrupts
#ifdef OLIMEX
#ifdef OLIMEX_DUINOMITE_EMEGA
if (ExtCurrentConfig[i] != EXT_CONSOLE_RESERVED && i != 35 ) { // don't reset the serial console
#else
if (ExtCurrentConfig[i] != EXT_CONSOLE_RESERVED && i != 21 ) { // don't reset the serial console
#endif
#else
if (ExtCurrentConfig[i] != EXT_CONSOLE_RESERVED) { // don't reset the serial console
#endif
ExtCfg(i, EXT_NOT_CONFIG); // all set to unconfigured
ExtSet(i, 0); // all outputs (when set) default to low
}
}
InterruptReturn = NULL;
}
/****************************************************************************************************************************
Initialise the I/O pins
*****************************************************************************************************************************/
void initExtIO(void) {
int i;
for (i = 1; i < NBRPINS + 1; i++) {
ExtCfg(i, EXT_NOT_CONFIG); // all set to unconfigured
ExtSet(i, 0); // all outputs (when set) default to low
}
#ifndef OLIMEX
CNCONbits.ON = 1; // turn on Change Notification module
CNPUEbits.CNPUE1 = 1; // turn on the pullup for pin C13 also called CN1
#endif
ExtCurrentConfig[0] = EXT_DIG_IN; // and show that we can read from it
P_LED_TRIS = P_OUTPUT; // make the LED pin an output
ExtSet(0, 0); // and turn it off
#ifdef OLIMEX
#ifdef OLIMEX_DUINOMITE_EMEGA
ExtCurrentConfig[35] = EXT_ANA_IN;
#else
ExtCurrentConfig[21] = EXT_ANA_IN;
#endif
#endif
// setup the analog (ADC) function
AD1CON1 = 0x00E0; // automatic conversion after sampling
AD1CSSL = 0; // no scanning required
AD1CON2 = 0; // use MUXA, use AVdd & AVss as Vref+/-
AD1CON3 = 0x203; //0x1F3F; // Tsamp = 32 x Tad;
AD1CON1bits.ADON = 1; // turn on the ADC
}
/****************************************************************************************************************************
Configure an I/O pin
Returns true if all is OK
*****************************************************************************************************************************/
int ExtCfg(int pin, int cfg) {
int tris, ana, oc;
if (pin < 0 || pin > NBRPINS) return false; // initial sanity check
// make sure that interrupts are disabled in case we are changing from an interrupt input
#ifdef OLIMEX
// if (pin == 5) ConfigINT2(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_DISABLE);
// if (pin == 6) ConfigINT3(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_DISABLE);
#ifdef OLIMEX_DUINOMITE_EMEGA
#else
if (pin == 7) ConfigINT4(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_DISABLE);
#endif
#else
if (pin == 11) ConfigINT1(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_DISABLE);
if (pin == 12) ConfigINT2(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_DISABLE);
if (pin == 13) ConfigINT3(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_DISABLE);
if (pin == 14) ConfigINT4(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_DISABLE);
#endif
inttbl[pin].intp = NULL; // also disable a software interrupt on this pin
switch (cfg) {
case EXT_NOT_CONFIG:
// #ifdef OLIMEX
// if (pin == 5) {P_E5_TRIS = 1; P_E5_OC = 1;}
// if (pin == 6) {P_E6_TRIS = 1; P_E6_OC = 1;}
// if (pin == 11) {P_E11_TRIS = 1; P_E11_OC = 1;}
// if (pin == 12) {P_E12_TRIS = 1; P_E12_OC = 1;}
// #endif
tris = 1; ana = 1; oc = 1;
break;
case EXT_ANA_IN:
// #ifdef OLIMEX
if (pin > 6 && pin < 19) return false;
// if (pin == 5) {P_E5_TRIS = 1; P_E5_OC = 1;}
// if (pin == 6) {P_E6_TRIS = 1; P_E6_OC = 1;}
// #else
// if (pin > 10) return false;
// #endif
tris = 1; ana = 0; oc = 1;
break;
case EXT_FREQ_IN: // same as counting, so fall through
case EXT_PER_IN: // same as counting, so fall through
case EXT_CNT_IN:
// #ifdef OLIMEX
// #else
// if (pin == 11) {
// INT1Count = INT1Value = 0;
// ConfigINT1(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_ENABLE);
// tris = 1; ana = 1; oc = 1;
// break;
// }
// #endif
// #ifdef OLIMEX
// if (pin == 5) {
// P_E5_TRIS = 1;
// P_E5_OC = 1;
// #else
// if (pin == 12) {
// #endif
// INT2Count = INT2Value = 0;
// ConfigINT2(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_ENABLE);
// tris = 1; ana = 1; oc = 1;
// break;
// }
// #ifdef OLIMEX
// if (pin == 6) {
// P_E6_TRIS = 1;
// P_E6_OC = 1;
// #else
// if (pin == 13) {
// #endif
// INT3Count = INT3Value = 0;
// ConfigINT3(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_ENABLE);
// tris = 1; ana = 1; oc = 1;
// break;
// }
#ifdef OLIMEX_DUINOMITE_EMEGA
break;
#else
#ifdef OLIMEX
if (pin == 7) {
#else
if (pin == 14) {
#endif
INT4Count = INT4Value = 0;
ConfigINT4(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_ENABLE);
tris = 1; ana = 1; oc = 1;
break;
}
return false; // not an interrupt enabled pin
#endif
case EXT_INT_LO: // same as digital input, so fall through
case EXT_INT_HI: // same as digital input, so fall through
case EXT_DIG_IN:
#ifdef OLIMEX
// if (pin == 5) {P_E5_TRIS = 1; P_E5_OC = 1;}
// if (pin == 6) {P_E6_TRIS = 1; P_E6_OC = 1;}
// if (pin == 11) {P_E11_TRIS = 1; P_E11_OC = 1;}
// if (pin == 12) {P_E12_TRIS = 1; P_E12_OC = 1;}
#endif
tris = 1; ana = 1; oc = 1;
break;
case EXT_DIG_OUT:
#ifdef OLIMEX
// if (pin == 11) {P_E11_TRIS = 1; P_E11_OC = 1;} //return false;
// if (pin == 5) {P_E5_TRIS = 1; P_E5_OC = 1;}
// if (pin == 6) {P_E6_TRIS = 1; P_E6_OC = 1;}
// if (pin == 12) {P_E12_TRIS = 1; P_E12_OC = 1;}
#endif
tris = 0; ana = 1; oc = 0;
break;
case EXT_OC_OUT:
#ifdef OLIMEX
// if (pin == 11) {P_E11_TRIS = 1; P_E11_OC = 1;} //return false;
// if (pin == 5) {P_E5_TRIS = 1; P_E5_OC = 1;}
// if (pin == 6) {P_E6_TRIS = 1; P_E6_OC = 1;}
// if (pin == 12) {P_E12_TRIS = 1; P_E12_OC = 1;}
#else
if (pin < 11) return false;
#endif
tris = 0; ana = 1; oc = 1;
break;
case EXT_COM_RESERVED:
case EXT_CONSOLE_RESERVED:
ExtCurrentConfig[pin] = cfg; // don't do anything except set the config type
#ifdef OLIMEX
// if (pin == 5) {P_E5_TRIS = 1; P_E5_OC = 1; P_E5_ANALOG = 1;}
// if (pin == 6) {P_E6_TRIS = 1; P_E6_OC = 1; P_E6_ANALOG = 1;}
// if (pin == 11) {P_E11_TRIS = 1; P_E11_OC = 1;}
// if (pin == 12) {P_E12_TRIS = 1; P_E12_OC = 1;}
#endif
return true;
default:
return false;
}
ExtCurrentConfig[pin] = cfg;
// set the TRIS and analog characteristics
switch (pin) {
case 1: P_E1_TRIS = tris; P_E1_OC = oc; P_E1_ANALOG = ana; break;
case 2: P_E2_TRIS = tris; P_E2_OC = oc; P_E2_ANALOG = ana; break;
case 3: P_E3_TRIS = tris; P_E3_OC = oc; P_E3_ANALOG = ana; break;
case 4: P_E4_TRIS = tris; P_E4_OC = oc; P_E4_ANALOG = ana; break;
case 5: P_E5_TRIS = tris; P_E5_OC = oc; P_E5_ANALOG = ana; break;
case 6: P_E6_TRIS = tris; P_E6_OC = oc; P_E6_ANALOG = ana; break;
#ifdef OLIMEX
#ifdef OLIMEX_DUINOMITE_EMEGA
case 7: P_E7_TRIS = tris; P_E7_OC = oc; P_E7_ANALOG = ana; break;
case 8: P_E8_TRIS = tris; P_E8_OC = oc; break;
case 9: P_E9_TRIS = tris; P_E9_OC = oc; break;
case 10: P_E10_TRIS = tris; P_E10_OC = oc; break;
#else
case 7: P_E7_TRIS = tris; P_E7_OC = oc; break;
case 8: if (!S.SDEnable) { P_E8_TRIS = tris; P_E8_OC = oc;} break;
case 9: if (!S.SDEnable) { P_E9_TRIS = tris; P_E9_OC = oc;} break;
case 10: if (!S.SDEnable) { P_E10_TRIS = tris; P_E10_OC = oc;} break;
#endif
#else
case 7: P_E7_TRIS = tris; P_E7_OC = oc; P_E7_ANALOG = ana; break;
case 8: P_E8_TRIS = tris; P_E8_OC = oc; P_E8_ANALOG = ana; break;
case 9: P_E9_TRIS = tris; P_E9_OC = oc; P_E9_ANALOG = ana; break;
case 10: P_E10_TRIS = tris; P_E10_OC = oc; P_E10_ANALOG = ana; break;
#endif
case 11: P_E11_TRIS = tris; P_E11_OC = oc; break;
case 12: P_E12_TRIS = tris; P_E12_OC = oc; break;
case 13: P_E13_TRIS = tris; P_E13_OC = oc; break;
case 14: P_E14_TRIS = tris; P_E14_OC = oc; break;
case 15: P_E15_TRIS = tris; P_E15_OC = oc; break;
case 16: P_E16_TRIS = tris; P_E16_OC = oc; break;
case 17: P_E17_TRIS = tris; P_E17_OC = oc; break;
case 18: P_E18_TRIS = tris; P_E18_OC = oc; break;
#ifdef OLIMEX
// SPP +
#ifdef OLIMEX_DUINOMITE_EMEGA // edit for DuinoMite eMega
case 19: P_E19_TRIS = tris; P_E19_OC = oc; break;
case 20: P_E20_TRIS = tris; P_E20_OC = oc; break;
case 21: P_E21_TRIS = tris; P_E21_OC = oc; break;
case 22: P_E22_TRIS = tris; P_E22_OC = oc; break;
case 23: P_E23_TRIS = tris; P_E23_OC = oc; break;
case 24: P_E24_TRIS = tris; P_E24_OC = oc; break;
case 25: P_E25_TRIS = tris; P_E25_OC = oc; break;
case 26: P_E26_TRIS = tris; P_E26_OC = oc; break;
case 27: P_E27_TRIS = tris; P_E27_OC = oc; break;
case 28: P_E28_TRIS = tris; P_E28_OC = oc; break;
case 29: P_E29_TRIS = tris; P_E29_OC = oc; break;
case 30: P_E30_TRIS = tris; P_E30_OC = oc; break;
case 31: P_E31_TRIS = tris; P_E31_OC = oc; P_E31_ANALOG = ana; break;
case 32: P_E32_TRIS = tris; P_E32_OC = oc; P_E31_ANALOG = ana; break;
case 33: P_E33_TRIS = tris; P_E33_OC = oc; break;
case 34: P_E34_TRIS = tris; P_E34_OC = oc; break;
case 35: P_E35_TRIS = tris; P_E35_OC = oc; P_E31_ANALOG = ana; break;
case 36: P_E36_TRIS = tris; P_E36_OC = oc; break;
case 37: P_E37_TRIS = tris; P_E37_OC = oc; break;
case 38: P_E38_TRIS = tris; P_E38_OC = oc; break;
case 39: P_E39_TRIS = tris; P_E39_OC = oc; break;
#else // original by Geoff Graham for DuinoMite Mega
case 19: //if (!S.VideoMode) {
P_E19_TRIS = tris; P_E19_OC = oc; P_E19_ANALOG = ana;//}
break;
case 20: if (!S.VideoMode || P_VGA_COMP) {P_E20_TRIS = tris; P_E20_OC = oc; P_E20_ANALOG = ana;} break;
case 21: P_E21_TRIS = tris; P_E21_OC = oc; P_E21_ANALOG = ana; break;
case 22: P_E22_TRIS = tris; P_E22_OC = oc; break;
case 23: P_E23_TRIS = tris; P_E23_OC = oc; break;
#endif
// SPP -
#else
case 19: P_E19_TRIS = tris; P_E19_OC = oc; break;
case 20: P_E20_TRIS = tris; P_E20_OC = oc; break;
#endif
#ifdef UBW32
case 21: P_E21_TRIS = tris; P_E21_OC = oc; break;
case 22: P_E22_TRIS = tris; P_E22_OC = oc; break;
case 23: P_E23_TRIS = tris; P_E23_OC = oc; break;
case 24: P_E24_TRIS = tris; P_E24_OC = oc; break;
case 25: P_E25_TRIS = tris; P_E25_OC = oc; break;
case 26: P_E26_TRIS = tris; P_E26_OC = oc; break;
case 27: P_E27_TRIS = tris; P_E27_OC = oc; break;
case 28: P_E28_TRIS = tris; P_E28_OC = oc; break;
case 29: P_E29_TRIS = tris; P_E29_OC = oc; break;
case 30: P_E30_TRIS = tris; P_E30_OC = oc; break;
case 31: P_E31_TRIS = tris; P_E31_OC = oc; break;
case 32: P_E32_TRIS = tris; P_E32_OC = oc; break;
case 33: P_E33_TRIS = tris; P_E33_OC = oc; break;
case 34: P_E34_TRIS = tris; P_E34_OC = oc; break;
case 35: P_E35_TRIS = tris; P_E35_OC = oc; break;
case 36: P_E36_TRIS = tris; P_E36_OC = oc; break;
case 37: P_E37_TRIS = tris; P_E37_OC = oc; break;
case 38: P_E38_TRIS = tris; P_E38_OC = oc; break;
case 39: P_E39_TRIS = tris; P_E39_OC = oc; break;
case 40: P_E40_TRIS = tris; P_E40_OC = oc; break;
case 41: P_E41_TRIS = tris; P_E41_OC = oc; break;
case 42: P_E42_TRIS = tris; P_E42_OC = oc; break;
case 43: P_E43_TRIS = tris; P_E43_OC = oc; break;
case 44: P_E44_TRIS = tris; P_E44_OC = oc; break;
case 45: P_E45_TRIS = tris; P_E45_OC = oc; break;
case 46: P_E46_TRIS = tris; P_E46_OC = oc; break;
case 47: P_E47_TRIS = tris; P_E47_OC = oc; break;
case 48: P_E48_TRIS = tris; P_E48_OC = oc; break;
case 49: P_E49_TRIS = tris; P_E49_OC = oc; break;
case 50: P_E50_TRIS = tris; P_E50_OC = oc; break;
#endif
}
if (cfg == EXT_NOT_CONFIG) ExtSet(pin, 0); // set the default output to low
return true;
}
/****************************************************************************************************************************
Set the output of a digital I/O pin
Returns true if all is OK
*****************************************************************************************************************************/
int ExtSet(int pin, int val){
val = (val != 0); // non zero is on
INTDisableInterrupts(); // setting an output bit is NOT atomic and a bit set operation
// in an interrupt could result in this set corrupting the output
switch (pin) {
#ifdef UBW32
case 0: P_LED_OUT = !val; break; // this is the LED - the UBW32 wired them upside down !!
#else
case 0: P_LED_OUT = val; break; // this is the LED
#endif
case 1: P_E1_OUT = val; break;
case 2: P_E2_OUT = val; break;
case 3: P_E3_OUT = val; break;
case 4: P_E4_OUT = val; break;
case 5: P_E5_OUT = val; break;
case 6: P_E6_OUT = val; break;
case 7: P_E7_OUT = val; break;
#ifdef OLIMEX
#ifdef OLIMEX_DUINOMITE_EMEGA
case 8: P_E8_OUT = val; break;
case 9: P_E9_OUT = val; break;
case 10: P_E10_OUT = val; break;
#else
case 8: if (!S.SDEnable) P_E8_OUT = val; break;
case 9: if (!S.SDEnable) P_E9_OUT = val; break;
case 10: if (!S.SDEnable) P_E10_OUT = val; break;
#endif
#else
case 8: P_E8_OUT = val; break;
case 9: P_E9_OUT = val; break;
case 10: P_E10_OUT = val; break;
#endif
case 11: P_E11_OUT = val; break;
case 12: P_E12_OUT = val; break;
case 13: P_E13_OUT = val; break;
case 14: P_E14_OUT = val; break;
case 15: P_E15_OUT = val; break;
case 16: P_E16_OUT = val; break;
case 17: P_E17_OUT = val; break;
case 18: P_E18_OUT = val; break;
#ifdef OLIMEX
#ifdef OLIMEX_DUINOMITE_EMEGA
case 19: P_E19_OUT = val; break;
case 20: P_E20_OUT = val; break;
#else
case 19: P_E19_OUT = val; break;
case 20: if (!S.VideoMode || P_VGA_COMP) P_E20_OUT = val; break;
case 22: P_E22_OUT = val ; break;
case 23: P_E23_OUT = val ; break;
#endif
#else
case 19: P_E19_OUT = val; break;
case 20: P_E20_OUT = val; break;
#endif
#if defined UBW32 || defined OLIMEX_DUINOMITE_EMEGA
case 21: P_E21_OUT = val; break;
case 22: P_E22_OUT = val; break;
case 23: P_E23_OUT = val; break;
case 24: P_E24_OUT = val; break;
case 25: P_E25_OUT = val; break;
case 26: P_E26_OUT = val; break;
case 27: P_E27_OUT = val; break;
case 28: P_E28_OUT = val; break;
case 29: P_E29_OUT = val; break;
case 30: P_E30_OUT = val; break;
case 31: P_E31_OUT = val; break;
case 32: P_E32_OUT = val; break;
case 33: P_E33_OUT = val; break;
case 34: P_E34_OUT = val; break;
case 35: P_E35_OUT = val; break;
case 36: P_E36_OUT = val; break;
case 37: P_E37_OUT = val; break;
case 38: P_E38_OUT = val; break;
case 39: P_E39_OUT = val; break;
#endif
#ifdef UBW32
case 40: P_E40_OUT = val; break;
case 41: P_E41_OUT = val; break;
case 42: P_E42_OUT = val; break;
case 43: P_E43_OUT = val; break;
case 44: P_E44_OUT = val; break;
case 45: P_E45_OUT = val; break;
case 46: P_E46_OUT = val; break;
case 47: P_E47_OUT = val; break;
case 48: P_E48_OUT = val; break;
case 49: P_E49_OUT = val; break;
case 50: P_E50_OUT = val; break;
#endif
default:
INTEnableInterrupts();
return false;
}
INTEnableInterrupts();
return true;
}
void UserMain( void* pd ){
/////Usual Routine
InitializeStack();
OSChangePrio( MAIN_PRIO );
EnableAutoUpdate();
EnableTaskMonitor();
EnableSmartTraps();
initPINS();
initDSPI();
//Local Variables
/***********Defining Interrupt Timers*****************/
HiResTimer* timer2=0;//50 Hz Interrupt Timer
int32_t IMU_data[6]={0};
char Navcomp_send_buff[64]={0},time_ms[2]={0},G[20]={0};
uint16_t NB_counter=0,sum=0;
double TotalTime=0;
int fdDebug=0;
uint8_t i=0;
BYTE IMU_command[24]={xahigh,0,xalow,0,yahigh,0,yalow,0,zahigh,0,zalow,0,xghigh,0,xglow,0,yghigh,0,yglow,0,zghigh,0,zglow,0};
BYTE IMU3_raw[24]={0};//IMU 3
BYTE IMU4_raw[24]={0};//IMU 4
Navcomp_send_buff[0] = 0x41;
Navcomp_send_buff[1] = 0x7A;
Navcomp_send_buff[2] = 0x04;
SerialClose(0);
SerialClose(2);
SerialClose(7);
SerialClose(9);
fdNavcomp=OpenSerial(7,15200,1,8,eParityNone);
fdcrank=OpenSerial(9,15200,1,8,eParityNone);
fdgrabber=OpenSerial(2,15200,1,8,eParityNone);
fdDebug=OpenSerial(0,15200,1,8,eParityNone);
OSSimpleTaskCreate(NAVcompData,MAIN_PRIO-1);
sprintf(G,"~ECHOF 1\r");
i=0;
for (i=0;i<sizeof(G);i++){
write(fdcrank,&G[i],1);
write(fdgrabber,&G[i],1);
}
initTIMERS(timer2);
while(1){
if (FiftyHzTaskFlag==1){
DSPIStart(1,IMU_command,IMU3_raw,24,NULL);//IMU3
while(!DSPIdone(1)){/*iprintf("DSPI1done state=%s\n",(DSPIdone(1))?"true":"false");*/};
//iprintf("....................................................................\n");
DSPIStart(3,IMU_command,IMU4_raw,24,NULL);//IMU3
while(!DSPIdone(3)){/*iprintf("DSPI3done state=%s\n",(DSPIdone(3))?"true":"false");*/};
//packaging NB counter
Navcomp_send_buff[4]=(uint8_t)((NB_counter & 0xFF00)>>8);
Navcomp_send_buff[3]=(uint8_t)(NB_counter & 0x00FF);
NB_counter++;
//getting time in ms
TotalTime=timer2->readTime();
sprintf(time_ms,"%lf",TotalTime);
Navcomp_send_buff[6]=time_ms[0];
Navcomp_send_buff[5]=time_ms[1];
IMU_data[0]=(((int32_t)IMU3_raw[2]<<24|(int32_t)IMU3_raw[3]<<16|(int32_t)IMU3_raw[4]<<8|(int32_t)IMU3_raw[5])-((int32_t)IMU4_raw[2]<<24|(int32_t)IMU4_raw[3]<<16|(int32_t)IMU4_raw[4]<<8|(int32_t)IMU4_raw[5]))/2;//X-Accel
IMU_data[1]=(((int32_t)IMU3_raw[6]<<24|(int32_t)IMU3_raw[7]<<16|(int32_t)IMU3_raw[8]<<8|(int32_t)IMU3_raw[9])-((int32_t)IMU4_raw[6]<<24|(int32_t)IMU4_raw[7]<<16|(int32_t)IMU4_raw[8]<<8|(int32_t)IMU4_raw[9]))/2;//Y-Accel
IMU_data[2]=(((int32_t)IMU3_raw[10]<<24|(int32_t)IMU3_raw[11]<<16|(int32_t)IMU3_raw[12]<<8|(int32_t)IMU3_raw[13])+((int32_t)IMU4_raw[10]<<24|(int32_t)IMU4_raw[11]<<16|(int32_t)IMU4_raw[12]<<8|(int32_t)IMU4_raw[13]))/2;//Z-Accel
IMU_data[3]=(((int32_t)IMU3_raw[14]<<24|(int32_t)IMU3_raw[15]<<16|(int32_t)IMU3_raw[16]<<8|(int32_t)IMU3_raw[17])-((int32_t)IMU4_raw[14]<<24|(int32_t)IMU4_raw[15]<<16|(int32_t)IMU4_raw[16]<<8|(int32_t)IMU4_raw[17]))/2;//X-Gyro
IMU_data[4]=(((int32_t)IMU3_raw[18]<<24|(int32_t)IMU3_raw[19]<<16|(int32_t)IMU3_raw[20]<<8|(int32_t)IMU3_raw[21])-((int32_t)IMU4_raw[18]<<24|(int32_t)IMU4_raw[19]<<16|(int32_t)IMU4_raw[20]<<8|(int32_t)IMU4_raw[21]))/2;//Y-Gyro
IMU_data[5]=(((int32_t)IMU3_raw[22]<<24|(int32_t)IMU3_raw[23]<<16|(int32_t)IMU3_raw[0]<<8|(int32_t)IMU3_raw[1])+((int32_t)IMU4_raw[22]<<24|(int32_t)IMU4_raw[23]<<16|(int32_t)IMU4_raw[0]<<8|(int32_t)IMU4_raw[1]))/2;//Z-Gyro
i=0;
for(i=0;i<6;i++){
Navcomp_send_buff[4*i+10]=(BYTE)((uint32_t)(IMU_data[i] & 0xFF000000)>>24);
Navcomp_send_buff[4*i+9]=(BYTE)((uint32_t)(IMU_data[i] & 0x00FF0000)>>16);
Navcomp_send_buff[4*i+8]=(BYTE)((uint32_t)(IMU_data[i] & 0x0000FF00)>>8);
Navcomp_send_buff[4*i+7]=(BYTE)((uint32_t)(IMU_data[i] & 0x000000FF));
}
Navcomp_send_buff[47]=statestatus;
//Calculating checksum
sum=0;
i=0;
for(i=3;i<63;i++){
sum +=Navcomp_send_buff[i];
}
Navcomp_send_buff[63]=(uint8_t)(sum % 256);
for(uint8_t j=0;j<sizeof(Navcomp_send_buff);j++){
write(fdNavcomp,&Navcomp_send_buff[j],1);
}
sprintf(G,"!VAR 1 %d\r",commandstatus);
i=0;
for (i=0;i<sizeof(G);i++){
write(fdcrank,&G[i],1);
write(fdgrabber,&G[i],1);
}
//printf("Value:%f MSB:%d LSB:%d\n",0.00025*((int16_t)((uint16_t)send_buff[18]*256 + (uint16_t)send_buff[17])),(uint8_t)send_buff[18],(uint8_t) send_buff[17]);
FiftyHzTaskFlag=0;
}//FiftyHz if
}//Main While loop
///////////////////////////////////MAIN FUNCTION//////////////////////////////////////////////
void UserMain( void* pd ){
/////Usual Routine
InitializeStack();
OSChangePrio( MAIN_PRIO );//Change Main Task number to MAIN_PRIO
EnableAutoUpdate();
EnableTaskMonitor();
EnableSmartTraps();
//serviceWatchDog();
//Starting the Code
iprintf("\n\n\n..................Starting Sensor Interface Board.....................\n\n\n");
//Local Variables
//////Scan Flag
/***********Defining Interrupt Timers*****************/
HiResTimer* timer2=0;//50 Hz Interrupt Timer
char m=0;
/***********File Descriptor Variables****************/
int startup_timeout=0;
unsigned char F_range_buff[4]={0};//Radio filtered range
/**********Radio Debug Variable*******************/
double TotalTime=0; char time_ms[2]={0};
/***********Radio control Radiocount and loop counter i********************/
uint8_t Radiocount3=0,Ant_config=0;
uint16_t CRME=0;
/*********Laser Rangefinder Variables***************/
float laser_range=0;
/**********ADC channel Array***************************/
uint16_t ADC_channel[8] = {0};
/**********Navcomp send buffer and other vriables**********************/
char Navcomp_send_buff[48]={0};
Navcomp_send_buff[0]=0x41;
Navcomp_send_buff[1]=0x7A;
Navcomp_send_buff[2]=0x05;
uint16_t netburner_counter=0;//netburner 16 bit counter
//Initialize pins
initPINS();
//Initialize Analog to Digital
InitSingleEndAD();
//Initializing Serial Ports
SerialClose(5);
SerialClose(7);
SerialClose(9);
SerialClose(8);
fdRadio= OpenSerial( 8, 115200, 1, 8, eParityNone );
fdDebug = OpenSerial( 5, 115200, 1, 8, eParityNone );
fdLaser = OpenSerial( 7, 115200, 1, 8, eParityNone );
fdNAVcomp = OpenSerial( 9, 115200, 1, 8, eParityNone );
/*
ReplaceStdio(0,fdDebug);
ReplaceStdio(1,fdDebug);
ReplaceStdio(2,fdDebug);
*/
//Start the Timers and init the DSPI
//DSPIInit(1,2000000,16,0x01,0x01,1,1,0,0,0);//initializing SPI
//printf("Going to wait 3 sec\n");
//OSTimeDly(3*TICKS_PER_SECOND);
initTIMERS(timer2);
J1[7]=0;
/*startup_timeout=ReadWithTimeout(fdDebug,&m,1,2);
if(startup_timeout==-1 || startup_timeout==0){
Start_PAN=StartUpLaserScan(fdLaser);
if(Start_PAN!=11110)
Scan_Status=1;
else
Scan_Status=0;
}
else{
Scan_Status=0;
Start_PAN=11110;
}*/
//printf("Hi\n");
Scan_Complete=1;
/***********packing startup PAN angle*************************/
Navcomp_send_buff[38] = (uint8_t)((Start_PAN & 0xFF00)>>8);
Navcomp_send_buff[37] = (uint8_t)(Start_PAN & 0x00FF);
Navcomp_send_buff[36] = Scan_Status;
OSSimpleTaskCreate(NAVcompData,MAIN_PRIO-1);
OSSimpleTaskCreate(RadioData,MAIN_PRIO-2);
//OSSimpleTaskCreate(MIScompData,MAIN_PRIO+2);
//enableWatchDog( 1, 0x001F );//0x001C
//Creating Data Receiving task from the computer
while(1){
//printf("Hi\n");
TotalTime=timer1->readTime();
//First if statement to command host radio to get ranging data from 101 guest with antenna A
if(FiveHzflag==1 && Radiocount3==0){
//printf("In WHile Loop\n");
CRME=(uint16_t)radio_in_buff1[28]*256+(uint16_t)radio_in_buff1[29];
if((unsigned char)radio_in_buff1[12]==0 && CRME<60){
F_range_buff[0]=radio_in_buff1[24];
F_range_buff[1]=radio_in_buff1[25];
F_range_buff[2]=radio_in_buff1[26];
F_range_buff[3]=radio_in_buff1[27];
F_range_buff[4]=radio_in_buff1[32];
F_range_buff[5]=radio_in_buff1[33];
F_range_buff[6]=radio_in_buff1[12];
Ant_config=radio_in_buff1[11];
}
Radiocount3=1;
FiveHzflag=0;
}//first if bracket
//second if statement to command host radio to get ranging data from 102 guest with antenna A
if(FiveHzflag==1 && Radiocount3==1){
CRME=(uint16_t)radio_in_buff2[28]*256+(uint16_t)radio_in_buff2[29];
if((unsigned char)radio_in_buff2[12]==0 && CRME<60){
F_range_buff[0]=radio_in_buff2[24];
F_range_buff[1]=radio_in_buff2[25];
F_range_buff[2]=radio_in_buff2[26];
F_range_buff[3]=radio_in_buff2[27];
F_range_buff[4]=radio_in_buff2[32];
F_range_buff[5]=radio_in_buff2[33];
Ant_config=radio_in_buff2[11];
}
Radiocount3=0;
FiveHzflag=0;
}//second if bracket
if(FiftyHzflag==1){
if(Scan_Complete==1){
//printf("Start Pan=%d,Start Pan=%d\n",Start_PAN,(int16_t)Navcomp_send_buff[38]*256+(int16_t)Navcomp_send_buff[37]);
laser_range=ReadLaser(fdLaser);
Navcomp_send_buff[36] = Scan_Status;
Navcomp_send_buff[38] = (uint8_t)((Start_PAN & 0xFF00)>>8);
Navcomp_send_buff[37] = (uint8_t)(Start_PAN & 0x00FF);
//printf("laser range=%g\n",laser_range);
//printf("laser range=%g\n",laser_range);
//uint32_t Range=(uint32_t)F_range_buff[0]*16777216+(uint32_t)F_range_buff[1]*65536+(uint32_t)F_range_buff[2]*256+(uint32_t)F_range_buff[3];
//printf("%zu,%u,%u,%u\n",Range,Ant_config,(unsigned char)radio_in_buff[12],(uint16_t)radio_in_buff[32]*256+(uint16_t)radio_in_buff[33]);
StartAD();
while (!ADDone()){}
asm("nop");
for (int i = 0; i < 8; i++)
ADC_channel[i] = (unsigned short int)(1000 * (((double)GetADResult(i)) * 3.3 / (32768.0)));
//printf("%d \n", ADC_channel[1]);
sprintf(time_ms,"%lf",TotalTime);
//send data to the computer
SendtoNAVCOMP(Navcomp_send_buff,ADC_channel,time_ms,netburner_counter,laser_range,F_range_buff,PanAngle,fdNAVcomp,fdDebug,sizeof(Navcomp_send_buff),Ant_config);
netburner_counter ++;
//printf("%g\n",dYaw);
//dYaw=0;
StartAD();
while (!ADDone()){}
asm("nop");
//dYaw=93;
uint16_t ServoPot = GetADResult(0);
////Servo PAN 1 numbers
Pulse=12287-dYaw*20.51;
//printf("%d\n",ServoPot);
if(Pulse<8594 || Pulse==8594)
sim1.mcpwm.sm[1].val[5]=8594;
if(Pulse>15980 || Pulse==15980)
sim1.mcpwm.sm[1].val[5]=15980;
else
sim1.mcpwm.sm[1].val[5]=Pulse;//PAN control
/*////Servo PAN 2 numbers
Pulse=12287-dYaw*20.14;
if(Pulse<8594 || Pulse==8310)
sim1.mcpwm.sm[1].val[5]=8310;
if(Pulse>15980 || Pulse==15560)
sim1.mcpwm.sm[1].val[5]=15560;
else
sim1.mcpwm.sm[1].val[5]=Pulse;//PAN control*/
double cYaw=(ServoPot-12885)/63;//PAN 1
//double cYaw=(ServoPot-14667)/63.05;//PAN 2
//Calibration PAN servo 1
//0-8594
//90-10440
//180-12287 Position in which PAN faces front
//270-14844
//360-15980
//Calibration pot PAN servo 1
//360=1564
//180=12885 //position in which PAN faces front
//270=7270
//90=18560
//0=24290
//Calibration PAN servo 2
//0-8310
//90-10440
//180-12287 Position in which PAN faces front
//270-13900
//360-15560
//Calibration pot PAN servo 2
//360=3264
//180=14667 //position in which PAN faces front
//270=8999
//90=20085
//0=25971
pwmr_comp=sim1.mcpwm.mcr;
sim1.mcpwm.mcr |=LDOK;
PanAngle = cYaw * 10;
//printf("%g\n",cYaw);
}
FiftyHzflag=0;
//serviceWatchDog();//
}//FiftyHzflag bracket
}//Main While loop Bracket
char SerialOpen( char * SerialPortName, int Speed )
{
/* if port already opened => close it before */
if ( PortIsOpened )
SerialClose( );
/* open the device to be non-blocking (read will return immediatly) */
fd = open( SerialPortName, O_RDWR | O_NOCTTY | O_NDELAY/*don't wait DTR*/ );
//fd = open( SerialPortName, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd >=0)
{
int BaudRate = -1;
int ScanBaudRate = 0;
fcntl(fd, F_SETFL, O_RDWR | O_NOCTTY ); /* perform blocking reads */
for(ScanBaudRate = 0; SerialSpeed[ScanBaudRate]; ScanBaudRate++)
{
if ( SerialSpeed[ ScanBaudRate ]==Speed )
{
BaudRate = SerialCorres[ ScanBaudRate ];
break;
}
}
switch (ModbusSerialDataBits)
{
case 8:
default:
DATABITS = CS8;
break;
case 7:
DATABITS = CS7;
break;
case 6:
DATABITS = CS6;
break;
case 5:
DATABITS = CS5;
break;
} //end of switch data_bits
switch (ModbusSerialStopBits)
{
case 1:
default:
STOPBITS = 0;
break;
case 2:
STOPBITS = CSTOPB;
break;
} //end of switch stop bits
switch (ModbusSerialParity)
{
case 0:
default: //none
PARITYON = PARENB;
PARITY = 0;
break;
case 1: //odd
PARITYON = PARENB;
PARITY = PARODD;
break;
case 2: //even
PARITYON = 0;
PARITY = 0;
break;
} //end of switch parity
if ( BaudRate!=-1 )
{
tcgetattr(fd,&oldtio); /* save current port settings */
/* set new port settings */
bzero(&newtio, sizeof(newtio));
//newtio.c_cflag = BaudRate | /*CRTSCTS |*/ CS8 | CLOCAL | CREAD;
newtio.c_cflag = BaudRate | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD;
//newtio.c_cflag |= PARENB
newtio.c_iflag = IGNPAR | IGNBRK; // | ICRNL;
newtio.c_oflag = 0;
newtio.c_lflag = 0;
newtio.c_cc[VMIN]=0; //1;
newtio.c_cc[VTIME]=0;
tcsetattr(fd,TCSANOW,&newtio);
PortIsOpened = 1;
}
else
{
printf("Speed value not found for serial\n");
}
}
else
{
printf( "Failed to open serial port %s...\n", SerialPortName );
}
return PortIsOpened;
}
// ==================================================
// 函数名：~Comm32Port
// 参数：无
// 返回值：无
// 功能：Comm32Port类的析构函数
// 创建日期：2011-6-30
// 修改日期：
// 备注：
// =================================================
Comm32Port::~Comm32Port()
{
SerialClose(); // 关闭串口
} // ~Comm32Port
Comm32Port::~Comm32Port()
{
SerialClose();
}
//-------------------------------------------------------
void CloseSoccer(void){
SerialClose(port);
gpio_close(17);
gpio_close(18);
}
void CloseWiFi() // power down wifi module (hard power down ie power goes off and ESP8266 looses everything)
{
SerialClose(WIFI_UART);
WIFI_PWR(0);
ESP_PGM(0); // Turn off I/O for power saving
}
int SerialRoutine(int port_num){
SerialClose(port_num);
return(OpenSerial(port_num, 115200, 1, 8, eParityNone));
}
int main()
{
unsigned char buf[255];
int i, len,ret, fd=PORT3, direction;
float speed, temperature, avgspeed;
printf("Opening serial port...");
ret = SerialOpen(fd);
if(ret < 0) {
printf("Error: SerialOpen returned: %d\n", ret);
SerialClose(fd);
return -1;
}
printf("Done\n");
printf("Setting port speed...");
ret = SerialSetSpeed(fd, 2400);
if(ret < 0) {
printf("Error: SerialSetSpeed returned: %d\n", ret);
SerialClose(fd);
return -1;
}
printf("Done\n");
printf("Setting port parameters...");
ret = SerialSetParam(fd, 0, 8, 1);
if(ret < 0) {
printf("Error: SerialSetParam returned: %d\n", ret);
SerialClose(fd);
return -1;
}
printf("Done\n");
printf("Setting port flow control...");
ret = SerialFlowControl(fd, NO_FLOW_CONTROL);
if(ret < 0) {
printf("Error: SerialFlowControl returned: %d\n", ret);
SerialClose(fd);
return -1;
}
printf("Done\n");
printf("Setting data logger mode...");
ret = SerialWrite(fd, ">I\r", 3); // data logger mode
if(ret < 0) {
printf("Error: SerialWrite returned: %d\n", ret);
SerialClose(fd);
return -1;
}
printf("Done\n");
printf("Waiting for header...");
get_data_header(fd);
get_data_header(fd);
printf("Found\n");
printf("Waiting for data...");
ret = SerialBlockRead(fd, buf, 50); // read data
if(ret < 0) {
printf("Error: SerialBlockRead returned: %d\n", ret);
SerialClose(fd);
return -1;
}
printf("Found\n");
for(i=0;i<48;i++) {
printf("%c\n", buf[i]);
}
if((buf[48] != 13) && (buf[49] != 10)) {
printf("Error: End of data incorrect!\n");
SerialClose(fd);
return -1;
}
//printf("end= %d %d\n", buf[48], buf[49]);
ret = SerialWrite(fd, ">\r", 2);
SerialClose(fd);
speed = (float)get_value(buf, SPEED_OFFSET);
speed = speed / 10;
speed = speed / 3.6; //(speed * 1.60934) / 3.6;
direction = get_value(buf, DIRECTION_OFFSET) & 0x00FFFFFF; // highest byte may be FF sometimes
direction = (int)((360.0/255.0) * direction);
temperature = (float)get_value(buf, TEMPERATURE_OFFSET);
temperature = temperature / 10;
temperature = (temperature - 32) * 5 / 9;
avgspeed = (float)get_value(buf, AVGSPEED_OFFSET);
avgspeed = avgspeed / 10;
avgspeed = avgspeed / 3.6;
printf("speed=%.1f, direction=%d, temperature=%.1f, avg.speed=%.1f\n", speed, direction, temperature, avgspeed);
sprintf(buf, "wget --spider \"%s?speed=%.1f&dir=%d&temp=%.1f&avgspeed=%.1f\"\n", POST_URL, speed, direction, temperature, avgspeed);
system(buf);
return 0;
}
