Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1406 → Rev 1407

/demos/trunk/chimera/send.c
39,6 → 39,10
 
HEXAPOD_STATE status;
 
void *start_tracer;
void *end_tracer;
void *trace_pointer;
 
extern struct leg_calibration calibration_table[];
 
void print_status(int n){
159,6 → 163,68
 
}
 
int trace_init(int buffer_size)
{
 
start_tracer = trace_pointer = (void *)malloc(buffer_size);
end_tracer = start_tracer + buffer_size;
memset(trace_pointer,0,buffer_size);
 
if (trace_pointer == NULL) return -1;
 
return 0;
}
 
int trace_consumption(int sensor, int value)
{
 
struct timespec t;
SYS_FLAGS f;
 
if (trace_pointer == NULL) return -1;
 
if (trace_pointer >= (end_tracer-16)) return -1;
 
f = kern_fsave();
 
sys_gettime(&t);
 
*(unsigned int *)trace_pointer = (sensor & 0xFF) | 0xAABBFF00;
*(unsigned int *)(trace_pointer + 4) = value;
*(unsigned int *)(trace_pointer + 8) = t.tv_sec;
*(unsigned int *)(trace_pointer + 12) = t.tv_nsec;
 
trace_pointer += 16;
 
kern_frestore(f);
 
return 0;
 
}
 
int tracer_send() {
 
static char pkg_buffer[1100];
int actual = 0;
 
trace_pointer = start_tracer;
 
while(((*(int *)(trace_pointer) >> 8) & 0xAABBFF) == 0xAABBFF) {
 
memcpy(&(pkg_buffer[actual]),trace_pointer,16);
 
actual += 16;
 
if (actual > 1000) {
pkg_buffer[actual] = 0;
udp_sendto(socket, pkg_buffer, strlen(pkg_buffer), &target);
usleep(10000);
}
 
}
 
}
 
TASK servo_send()
{
HEXAPOD_STATE old_status;
250,7 → 316,7
#ifdef SERIAL_ON
task_nopreempt();
status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg);
TRACER_LOGEVENT(FTrace_EVT_ipoint,actual_leg,status.cfg[actual_leg].adc_in);
trace_consumption(actual_leg,status.cfg[actual_leg].adc_in);
task_preempt();
#endif