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 |
|