DataPipeThread::DataPipeThread(QObject *parent) :
QThread(parent)
{
initTcpServer();
initCom();
start();
}
//////////////////////////////////////
// Main
/////////////////////////////////////
int main(void)
{
//int a = 0;
/////////////////////////////////////
///TODO//////////////////////////////
/////////////////////////////////////
//
//participer a la coupe
/////////////////////////////////////
/////////////////////////////////////
//Initialisations////////////////////
/////////////////////////////////////
uart_init();
time_init(128);
printf("fdevopen \n");
fdevopen(uart0_send,uart0_recv, 1);
// Affichage uart: tip -s 9600 -l /dev/ttyS0
// prog usb :
// avarice -j /dev/ttyUSB0 --erase --program -f ./main.hex
adc_init();
DDRE |= 0x00;
PORTE |= 0x10;//active le pull up
//initialisation du fpga
sbi(XMCRA,SRW11);
sbi(XMCRA,SRW00);
sbi(MCUCR,SRE);
wait_ms(1000);
//on reset le fpga
U_RESET = 0x42;
sbi(PORTB,0);
wait_ms(100);
//on relache le reset
cbi(PORTB,0);
sbi(PORTE,3);
wait_ms(100);
//printf("stack begin :%d \n", &a);
//init des interruptions
printf("init i2c \n");
scheduler_init();
position_init(&pos);
asserv_init(&asserv,&pos);
trajectory_init(&traj,&pos,&asserv);
i2cm_init();
//autorise les interruptions
sei();
//infinite loop!!!
//get_enc_value();
//init communication
#ifdef ENABLE_MECA
printf("init com \n");
initCom();
printf("end init com \n");
#endif
//init A*
initObstacle();
//init méca
#ifdef ENABLE_MECA
mecaCom(TIROIR_FERMER);
mecaCom(PEIGNE_FERMER);
#endif
team = getTeam();
while(MAXIME);//boucle anti_maxime
// while(1)
// {
// printf("gp2r = %d ,gp2l = %d ,gp2m = %d \n",adc_get_value(ADC_REF_AVCC | MUX_ADC0),adc_get_value(ADC_REF_AVCC | MUX_ADC1),adc_get_value(ADC_REF_AVCC | MUX_ADC2));
// }
//avoidance_init();
//antipatinage_init();
// homologation();
// while(1);
// test_evitement();
// while(1);
if(team == RED)
{
disableSpinning();
findPosition(team);
enableSpinning();
trajectory_goto_d(&traj, END, 20);
trajectory_goto_a(&traj, END, 55);
while(!trajectory_is_ended(&traj));
//
// trajectory_goto_a(&traj, END, 90);
//
// trajectory_goto_a(&traj, END, 0);
// while(!trajectory_is_ended(&traj));
asserv_set_vitesse_normal(&asserv);
initTaskManager(&tkm);
//addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &throwSpears, LOW_PRIORITY, RED);
addTask(&tkm, &putPaint, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, LOW_PRIORITY, R3);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R2);
addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
// addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &findPosition, HIGH_PRIORITY, REDPAINT);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y1);
//addTask(&tkm, &throwSpears, HIGH_PRIORITY, YELLOW);
addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3);
// addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y2);
while (TIRETTE);
printf("begin match \n");
avoidance_init();
while(doNextTask(&tkm));
printf("finish \n");
}
else // Team jaune
{
disableSpinning();
findPosition(team);
enableSpinning();
trajectory_goto_d(&traj, END, 20);
trajectory_goto_a(&traj, END, -55);
while(!trajectory_is_ended(&traj));
// addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &returnFire, LOW_PRIORITY, Y2);
addTask(&tkm, &throwSpears, LOW_PRIORITY, YELLOW);
addTask(&tkm, &putPaint, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3);
// addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &findPosition, HIGH_PRIORITY, YELLOWPAINT);
//addTask(&tkm, &returnFire, HIGH_PRIORITY, R1);
//addTask(&tkm, &throwSpears, HIGH_PRIORITY, RED);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R3);
// addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R2);
while (TIRETTE);
printf("begin match \n");
avoidance_init();
while(doNextTask(&tkm));
printf("finish \n");
// // attend que la tirette soit retirée
// printf("begin match \n");
// printf("start pos: x %lf pos y %lf \n",position_get_x_cm(&pos),position_get_y_cm(&pos));
}
printf("time: %ld \n",time_get_s());
while(1);//attention boucle infinie
}
void ServiceMain(int argc, char** argv)
{
ServiceStatus.dwServiceType = SERVICE_WIN32;
ServiceStatus.dwCurrentState = SERVICE_START_PENDING;
ServiceStatus.dwControlsAccepted = SERVICE_ACCEPT_STOP | SERVICE_ACCEPT_SHUTDOWN;
ServiceStatus.dwWin32ExitCode = 0;
ServiceStatus.dwServiceSpecificExitCode = 0;
ServiceStatus.dwCheckPoint = 0;
ServiceStatus.dwWaitHint = 0;
if (isService) {
hStatus = RegisterServiceCtrlHandler(HSP_SERVICE_NAME,
(LPHANDLER_FUNCTION)ControlHandler);
if (hStatus == 0)
{
return;
}
}
// Test for only one instance is running
HANDLE mutex = CreateMutex(NULL, TRUE, instanceMutexName);
DWORD err = GetLastError();
if (mutex != NULL && err == ERROR_ALREADY_EXISTS ||
mutex == NULL && err == ERROR_ACCESS_DENIED) {
// Mutex found, so another instance is running
if (hStatus != 0) {
ServiceStatus.dwCurrentState = SERVICE_STOPPED;
ServiceStatus.dwWin32ExitCode = ERROR_SINGLE_INSTANCE_APP;
SetServiceStatus(hStatus, &ServiceStatus);
} else {
myLog(LOG_ERR, "%s.ServiceMain: only one instance can run, existing instance found", HSP_SERVICE_NAME);
}
return;
} else {
ServiceStatus.dwCurrentState = SERVICE_RUNNING;
if (hStatus != 0) {
// We are the first instance, report the running status to SCM.
SetServiceStatus (hStatus, &ServiceStatus);
}
}
wchar_t programDataDir[MAX_PATH];
if (!initialiseProgramDataDir(programDataDir, MAX_PATH)) {
*programDataDir = NULL;
}
char mbcLogFilename[MAX_PATH];
if (isService && *programDataDir != NULL) {
//set the log file name to the default.
size_t dirLen = 0;
if (0 == wcstombs_s(&dirLen, mbcLogFilename, MAX_PATH, programDataDir, wcslen(programDataDir))) {
PathAppend(mbcLogFilename, HSP_DEFAULT_LOGFILE);
logFilename = mbcLogFilename;
} else {
logFilename = NULL;
}
}
if (logFilename != NULL) {
// Logging on
errno_t error = fopen_s(&logFile, logFilename, "wt");
if (error != 0) {
logFile = stderr;
myLog(LOG_ERR, "%s.ServiceMain: could not open log file %s: error %d\n", HSP_SERVICE_NAME, logFilename, error);
}
logFilename = NULL;
}
myLog(debug, "-------------Starting %s %s--------------", HSP_SERVICE_NAME, HSP_VERSION);
fflush(logFile);
HRESULT initComHr = initCom();
if (FAILED(initComHr)) {
myLog(LOG_ERR, "%s.ServiceMain: cannot initialize COM for WMI error=0x%x", HSP_SERVICE_NAME, initComHr);
if (hStatus != 0) {
ServiceStatus.dwCurrentState = SERVICE_STOPPED;
ServiceStatus.dwWin32ExitCode = ERROR_APP_INIT_FAILURE;
SetServiceStatus(hStatus, &ServiceStatus);
}
return;
}
HSP sp = { 0 };
sp.DNSSD_startDelay = HSP_DEFAULT_DNSSD_STARTDELAY;
sp.DNSSD_retryDelay = HSP_DEFAULT_DNSSD_RETRYDELAY;
// look up host-id fields at startup only (hostname
// may change dynamically so will have to revisit this $$$)
sp.host_hid.hostname.str = (char *)my_calloc(SFL_MAX_HOSTNAME_CHARS+1);
sp.host_hid.os_release.str = (char *)my_calloc(SFL_MAX_OSRELEASE_CHARS+1);
readHidCounters(&sp, &sp.host_hid);
sp.nio_polling_secs = HSP_NIO_POLLING_SECS_32BIT;
readInterfaces(&sp, TRUE);
if (!readConfig(&sp)) {
myLog(LOG_ERR, "%s.ServiceMain: invalid configuration", HSP_SERVICE_NAME);
if (hStatus != 0) {
ServiceStatus.dwCurrentState = SERVICE_STOPPED;
ServiceStatus.dwWin32ExitCode = ERROR_INVALID_PARAMETER;
SetServiceStatus(hStatus, &ServiceStatus);
}
return;
}
sp.hyperV = testForHyperv();
if (sp.hyperV) {
myLog(debug, "%s.ServiceMain Hyper-V services are running", HSP_SERVICE_NAME);
if (programDataDir == NULL || !initialiseProgramDataFiles(&sp, programDataDir)) {
myLog(LOG_ERR, "%s.ServiceMain: cannot initialise switch port and VM state files", HSP_SERVICE_NAME);
if (hStatus != 0) {
ServiceStatus.dwCurrentState = SERVICE_STOPPED;
ServiceStatus.dwWin32ExitCode = ERROR_FILE_NOT_FOUND;
SetServiceStatus(hStatus, &ServiceStatus);
}
return;
}
readGuidStore(sp.f_vmStore, sp.vmStoreFile, &sp.vmStore, &sp.maxDsIndex);
readGuidStore(sp.f_portStore, sp.portStoreFile, &sp.portStore, &sp.maxIfIndex);
}
if (sp.DNSSD) {
//start the DNS thread and loop waiting until we have the config from DNSSD
HANDLE dnsSDThread;
unsigned threadID;
dnsSDThread = (HANDLE)_beginthreadex(NULL, 0, &runDNSSD, &sp, 0, &threadID);
BOOL gotConfig = FALSE;
while (!gotConfig) {
HSPSFlowSettings *settings = sp.sFlow->sFlowSettings;
if (newerSettingsAvailable(settings)) {
HSPSFlowSettings *newSettings = newSFlowSettings();
if (readSFlowSettings(newSettings)) {
//we have the config now
gotConfig = TRUE;
if (settings != NULL) {
freeSFlowSettings(settings);
}
sp.sFlow->sFlowSettings = newSettings;
myLog(debug, "%s.ServiceMain: DNS-SD enabled. Initial sFlow settings from registry currentconfig",
HSP_SERVICE_NAME);
} else {
freeSFlowSettings(newSettings);
myLog(LOG_ERR, "%s.ServiceMain: invalid DNS-SD discovered sFlow settings", HSP_SERVICE_NAME);
if (hStatus != 0) {
ServiceStatus.dwCurrentState = SERVICE_STOPPED;
ServiceStatus.dwWin32ExitCode = ERROR_INVALID_PARAMETER;
SetServiceStatus(hStatus, &ServiceStatus);
}
return;
}
} else {
Sleep(sp.DNSSD_startDelay*1000);
}
}
} else { // read the manual config
HSPSFlowSettings *newSettings = newSFlowSettings();
if (readSFlowSettings(newSettings)) {
sp.sFlow->sFlowSettings = newSettings;
} else {
myLog(LOG_ERR, "%s.ServiceMain: invalid sFlow configuration in registry", HSP_SERVICE_NAME);
if (hStatus != 0) {
ServiceStatus.dwCurrentState = SERVICE_STOPPED;
ServiceStatus.dwWin32ExitCode = ERROR_INVALID_PARAMETER;
SetServiceStatus(hStatus, &ServiceStatus);
}
return;
}
}
openFilter(&sp); //try to initialise the sFlow filter for sampling
initAgent(&sp);
logSFlowSettings(sp.sFlow);
// initialize the clock so we can detect second boundaries
sp.clk = time(NULL);
// main loop
BOOL dataAvailable = true;
uint32_t currReadNum = 0;
while (ServiceStatus.dwCurrentState == SERVICE_RUNNING && dataAvailable)
{
// check for second boundaries and generate ticks for the sFlow library
time_t now = time(NULL);
if ((now < sp.clk) || (now - sp.clk) > HSP_MAX_TICKS) {
// avoid a busy-loop of ticks if time jumps
myLog(LOG_INFO, "%s.ServiceMain: time jump detected", HSP_SERVICE_NAME);
sp.clk = now - 1;
}
while (sp.clk < now) { //only happens on second boundary
//start critical
if (sp.sFlow->sFlowSettings) {
if (sp.clk%5 == 0 && newerSettingsAvailable(sp.sFlow->sFlowSettings)) {
//get the new config
HSPSFlowSettings *newSettings = newSFlowSettings();
if (readSFlowSettings(newSettings)) {
//we have the config now
BOOL pollingChanged = sp.sFlow->sFlowSettings->pollingInterval != newSettings->pollingInterval;
BOOL samplingChanged = sp.sFlow->sFlowSettings->samplingRate != newSettings->samplingRate;
freeSFlowSettings(sp.sFlow->sFlowSettings);
sp.sFlow->sFlowSettings = newSettings;
myLog(debug, "%s.ServiceMain: updated configuration settings", HSP_SERVICE_NAME);
logSFlowSettings(sp.sFlow);
if (pollingChanged) {
for (SFLPoller *poller = sp.sFlow->agent->pollers;
poller; poller = poller->nxt) {
sfl_poller_set_sFlowCpInterval(poller, sp.sFlow->sFlowSettings->pollingInterval);
}
}
if (samplingChanged && HSP_FILTER_ACTIVE(sp.filter)) {
setFilterSamplingParams(&sp);
}
} else {
freeSFlowSettings(newSettings);
}
}
tick(&sp);
}
//end critical
sp.clk++;
}
DWORD result;
//process a queued counter poller
processQueuedPoller(&sp);
//timeout is set so that we loop around checking for ticks and samples
//several times/s.
//calculate timeout 200 if the counter poller queue is empty, 0 otherwise
DWORD timeout = sp.pollerQHead == NULL ? HSP_TIMEOUT : 0;
if (HSP_FILTER_ACTIVE(sp.filter)) {
result = WaitForSingleObject(sp.filter.overlaps[currReadNum].hEvent,
timeout);
if (result == WAIT_OBJECT_0) {
dataAvailable = sp.filter.overlaps[currReadNum].Internal == ERROR_SUCCESS;
if (dataAvailable && sp.filter.overlaps[currReadNum].InternalHigh > 0) {
//process the sample info in sp.filter.buffers[currReadNum]
readPackets(&sp, sp.filter.buffers[currReadNum]);
}
// Re-queue this read
queueRead(sp.filter.dev,
sp.filter.buffers[currReadNum],
sizeof(sp.filter.buffers[currReadNum]),
&sp.filter.overlaps[currReadNum]);
//set the next buffer to read
currReadNum = (currReadNum+1)%numConcurrentReads;
}
} else {
Sleep(timeout);
}
}
return;
}
int main(int argc, char* argv[])
{
int res,i,tot=0;
char* s_dev="COM2";
char* s_fname=0;
char* s_add=0;
char* s_len=0;
unsigned address=0x500000,len=0,tlen;
FILE* mybin=0;
struct stat info;
char buffer[BUFFER_SIZE];
unsigned ok=0xc1a0c1a0;
unsigned long lenw;
for(i=1;i<argc;i++){
if(!strcmp(argv[i],"-d")){
s_dev = argv[++i];
continue;
}
if(!strcmp(argv[i],"-a")){
s_add = argv[++i];
continue;
}
/* if(!strcmp(argv[i],"-reset")){
operation = CMD_RESET;
continue;
}*/
if(!strcmp(argv[i],"-h")){
printf("usage is %s <bin> [-a <flash address>]\n",argv[0]);
exit(0);
}
s_fname = argv[i];
// printf("name -> %s",s_fname);
}
if(s_fname){
mybin=fopen(s_fname,"rb");
if(mybin==0){
printf("## you must specify a valid filename\n");
exit(1);
}
} else {
printf("## you must specify a valid filename\n");
exit(1);
}
if(s_add){
address=strtoul(s_add,0,0);
}
initCom(s_dev);
{
int fdf=fseek(mybin,0,SEEK_END );
fdf = ftell(mybin);
fseek(mybin,0,SEEK_SET );
len = fdf- ftell(mybin);
}
printf("* binary len %d bytes flash add 0x%x..\n",len,address);
if(ask(PRG_FLASH,address,len)!=0) {
fclose(mybin);
CloseHandle(hComm);
return 0;
}
// check flsh id
if((res=check_ack())!=0){
printf("## flash id check failed 0x%x\n",res);
CloseHandle(hComm);
exit(1);
}
printf("* flash id check ok\n");
WriteFile(hComm,&ok,4,&lenw,NULL);
if((res=check_ack())!=0){
printf("## flash id check failed 0x%x\n",res);
CloseHandle(hComm);
exit(1);
}
printf("* erasing space address 0x%x... please wait\n",address);
// verify erase
if((res=check_ack())!=0){
printf("## verify erase failed 0x%x\n",res);
CloseHandle(hComm);
exit(1);
}
printf("* flash erase check ok\n");
printf("* start programming %d bytes.\n",len);
tlen=len;
while(len>0){
unsigned long ll=(len<BUFFER_SIZE)?len:BUFFER_SIZE;
tot+=ll;
fread(buffer,1,ll,mybin);
//usleep(8000);
//write(fd,buffer,ll);
WriteFile(hComm,buffer,ll,&lenw,NULL);
if(ll!=lenw){
printf("## error writing byte write %d != byte to write %d\n",lenw,ll);
exit(1);
}
printf("* downloading %.3f (rem:%d bytes)\r",(tlen-len)*100.0/tlen,tot);
if((res=check_ack())!=0){
printf("## programming failed base add 0x%x\n",res);
// close(fd);
// exit(1);
}
len-=ll;
}
printf("\n* 100%% end programming\n");
return 0;
}
/* UART to USB com port init routine */
ErrorCode_t UCOM_init(USBD_HANDLE_T hUsb, USB_CORE_DESCS_T *pDesc, USBD_API_INIT_PARAM_T *pUsbParam)
{
ErrorCode_t ret = LPC_OK;
/* Store USB stack handle for future use. */
g_uCOM1.selected=0;
g_uCOM2.selected=LPC_UART0;
g_uCOM3.selected=LPC_UART2;
g_uCOM4.selected=0;
g_uCOM5.selected=0;
g_uCOM1.can=0;
g_uCOM2.can=0;
g_uCOM3.can=0;
g_uCOM4.can=&gCAN1;
g_uCOM5.can=&gCAN2;
g_uCOM1.inEndpoint=USB_CDC1_IN_EP;
g_uCOM1.outEndpoint=USB_CDC1_OUT_EP;
g_uCOM1.pInHdlr = &UCOM_bulk_in_hdlr;
g_uCOM1.pOutHdlr = &UCOM_bulk_hdlr;
if (ret == LPC_OK) {
ret= initCom(hUsb, pDesc, pUsbParam, &g_uCOM1, 0);
}
g_uCOM2.inEndpoint=USB_CDC2_IN_EP;
g_uCOM2.outEndpoint=USB_CDC2_OUT_EP;
g_uCOM2.pInHdlr = &UCOM_bulk_hdlrInGeneric;
g_uCOM2.pOutHdlr = &UCOM_bulk_hdlrOutGeneric;
if (ret == LPC_OK) {
ret=initCom(hUsb, pDesc, pUsbParam, &g_uCOM2, 1);
}
g_uCOM3.inEndpoint=USB_CDC3_IN_EP;
g_uCOM3.outEndpoint=USB_CDC3_OUT_EP;
g_uCOM3.pInHdlr = &UCOM_bulk_hdlrInGeneric;
g_uCOM3.pOutHdlr = &UCOM_bulk_hdlrOutGeneric;
if (ret == LPC_OK) {
ret= initCom(hUsb, pDesc, pUsbParam, &g_uCOM3, 2);
}
g_uCOM4.inEndpoint=USB_CDC4_IN_EP;
g_uCOM4.outEndpoint=USB_CDC4_OUT_EP;
g_uCOM4.pInHdlr = &UCOM_bulk_hdlrInGeneric;
g_uCOM4.pOutHdlr = &UCOM_bulk_hdlrOutGeneric;
if (ret == LPC_OK) {
ret=initCom(hUsb, pDesc, pUsbParam, &g_uCOM4, 3);
}
g_uCOM5.inEndpoint=USB_CDC5_IN_EP;
g_uCOM5.outEndpoint=USB_CDC5_OUT_EP;
g_uCOM5.pInHdlr = &UCOM_bulk_hdlrInGeneric;
g_uCOM5.pOutHdlr = &UCOM_bulk_hdlrOutGeneric;
if (ret == LPC_OK) {
ret=initCom(hUsb, pDesc, pUsbParam, &g_uCOM5, 4);
}
if (ret == LPC_OK) {
ret = USBD_API->core->RegisterClassHandler(hUsb, &UCOM_ControlHandler, 0);
}
return ret;
}
