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