// 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;
}
