// python function (void) add_channel_pulse(int channel, int gpio, int width_start, int width) static PyObject* py_add_channel_pulse(PyObject *self, PyObject *args) { int channel, gpio, width_start, width; if (!PyArg_ParseTuple(args, "iiii", &channel, &gpio, &width_start, &width)) return NULL; if (add_channel_pulse(channel, gpio, width_start, width) == EXIT_FAILURE) return raise_error(); Py_INCREF(Py_None); return Py_None; }
int main(int argc , char *argv[]) { int socket_desc , client_sock , c; struct sockaddr_in server , client; pthread_t control_thread_id; FILE *log=NULL; int i,j; if(IMUBInit()<0){ printf("Error with IMU appear\n"); exit(0); } IMUB_DLPF(6); IMUBAccelScale(16); if(argc>=2) { log=fopen(argv[1],"w"); if(log==NULL) { printf("Error to open user file: %s\n",argv[1]); return -1; } } else { printf("Using default name: IMU_log.txt\n"); log=fopen("IMU_log.txt","w"); } #ifdef MOTORS //Setup PWM setup(1,DELAY_VIA_PWM); init_channel(channel, SUBCYCLE_TIME_US_DEFAULT); for(i=0;i<6;i++) add_channel_pulse(channel, esc[i], 0, 1000); #endif usleep(5000000); if( pthread_create( &control_thread_id , NULL , control_handler , (void*)NULL) < 0) { perror("could not create control thread"); return 1; } //Create socket socket_desc = socket(AF_INET , SOCK_STREAM , 0); if (socket_desc == -1) { printf("Could not create socket"); } puts("Socket created"); //Prepare the sockaddr_in structure server.sin_family = AF_INET; server.sin_addr.s_addr = INADDR_ANY; server.sin_port = htons( 8888 ); //Bind if( bind(socket_desc,(struct sockaddr *)&server , sizeof(server)) < 0) { //print the error message perror("bind failed. Error"); return 1; } puts("bind done"); //Listen listen(socket_desc , 3); //Accept and incoming connection puts("Waiting for incoming connections..."); c = sizeof(struct sockaddr_in); //Accept and incoming connection puts("Waiting for incoming connections..."); c = sizeof(struct sockaddr_in); pthread_t thread_id; while( (client_sock = accept(socket_desc, (struct sockaddr *)&client, (socklen_t*)&c)) ) { puts("Connection accepted"); if( pthread_create( &thread_id , NULL , connection_handler , (void*) &client_sock) < 0) { perror("could not create thread"); return 1; } //Now join the thread , so that we dont terminate before the thread //pthread_join( thread_id , NULL); puts("Handler assigned"); } if (client_sock < 0) { perror("accept failed"); return 1; } return 0; }
void *control_handler(void *socket_desc) { int i,mid_seconds; while(1) { IMUBPollRaw(raw); if(memcmp(raw,raw_before,sizeof(float)*10)!=0) { memcpy(raw_before,raw,sizeof(float)*10); }else {continue;} /* discard_values--; if(discard_values!=0) continue; */ total_counter++; if(total_counter==250) { total_counter=0; for(i=0;i<6;i++) printf("%f ",omega[i]); fflush(log); mid_seconds++; if(mid_seconds>0 && mid_seconds<=40) { WBase = WBase_1 + 300*(mid_seconds/40.0f) ; } else if(mid_seconds>40) { WBase=1300; } printf(" Power: %f ",WBase); } //Yaw segment Pitch Roll omega[0]= errors[0]*Kyaw + errors[1]*Kpitch + WBase; omega[1]= -errors[0]*Kyaw + errors[1]*Kpitch + errors[2]*Kroll + WBase; omega[2]= errors[0]*Kyaw - errors[1]*Kpitch + errors[2]*Kroll + WBase; omega[3]= -errors[0]*Kyaw - errors[1]*Kpitch + WBase; omega[4]= errors[0]*Kyaw - errors[1]*Kpitch - errors[2]*Kroll + WBase; omega[5]= -errors[0]*Kyaw + errors[1]*Kpitch - errors[2]*Kroll + WBase; fprintf(log,"%f, %f, %f",raw[3],raw[4],raw[5]); #ifdef MOTORS for(i=0;i<6;i++) { //fprintf(log,"%i, ",omega[i]); add_channel_pulse(channel, esc[i], 0, omega[i]); //add_channel_pulse(channel, esc[i], 0, 1200); } #endif fprintf(log,"\n"); } return 0; }