X-Git-Url: http://pileus.org/git/?a=blobdiff_plain;f=hw2%2Fmain.cpp;h=87bca115891bdbb3132dcbe0c42211b4a25714ae;hb=70a817bdd055b39c1990778594ed97a532748d31;hp=aac88f2c0dccf3962b60f5656b42089b4507b082;hpb=d5a9226714d158f2ac458425eba2550fd02c5cbe;p=~andy%2Fcsm213a-hw diff --git a/hw2/main.cpp b/hw2/main.cpp index aac88f2..87bca11 100644 --- a/hw2/main.cpp +++ b/hw2/main.cpp @@ -1,165 +1,557 @@ -#include "mbed.h" - -/** - * Mode of operation: - * Devices 1 and 2 synchronize clocks using serial messages. - * - * 1. Each serial message timestamped using the hardware timer capture - * registers in both the sender and receiver. - * 2. The sender transmits the send timestamp during the next time-sync - * message. - * 3. The receiver then compares the senders timestamp with it's own - * timestamp for the corresponding messages and calculates an offset. - * 4. The offset is used to compensate the receivers local clock. - * - * Time synchronization is performed in both directions. - */ - -/*********************** - * Message Definitions * - ***********************/ - -#define MSG_HEADER 0x1234 - -typedef enum { - MSG_ID_SYNC, // Time synchronization - MSG_ID_EVENT, // Event occurred -} msgid_t; - -typedef struct { - uint32_t seconds; // Seconds since 1970 (without leap seconds) - uint32_t nanosec; // Nanoseconds since 'seconds' -} ntime_t; - -typedef struct { - uint16_t header; // Message Header - uint16_t mesgid; // Message ID - uint16_t length; // Body length - uint16_t cksum; // Body checksum -} header_t; - -typedef struct { - uint16_t seq; // Current sequence counter - uint16_t prev; // Sequence of previous message - ntime_t time; // Time of previous message -} sync_msg_t; - -typedef struct { - uint16_t device; // Device ID - uint16_t event; // Event ID - ntime_t time; // Timestamp -} event_msg_t; - -/******************* - * Timer functions * - *******************/ - -/** - * Generate time stamp for an async event: - * time: drift compensated wall-clock time - * stamp: event timestamp from Timer/PWM Module - */ -void time_stamp(ntime_t *time, uint32_t stamp) -{ - // todo -} - -/** - * Compensate the Real-Time-Clock oscillator for - * temperature and drift errors. Called at 1Hz and - * synchronous to the RTC 1Hz output. - */ -void time_rtc_comp(void) -{ - // todo -} - -/** - * Synchronize the timer internal state with updates - * from an external time sync message. - * ours: our internal timestamp for the event - * ref: reference timestamp from the other device - */ -void time_ext_sync(ntime_t *ours, ntime_t *ref) -{ - // todo -} - -/************************ - * Serial I/O functions * - ************************/ - -/** - * Output time sync message - */ -void serial_send_sync(void) -{ -} - -/** - * Output external event received message - * event: id of the received event - * time: compensated timestamp of the event - */ -void serial_send_event(uint16_t event, ntime_t *time) -{ -} - -/** - * Process serial receive messages - */ -void serial_receive(void) -{ -} - -/******************** - * Data definitions * - ********************/ - -// LEDs -DigitalOut led1(LED1); -DigitalOut led2(LED2); - -// UARTs tx rx -Serial uart0(USBTX, USBRX); -Serial uart1(PTE0, PTE1); -Serial uart2(PTE16, PTE17); - -/******** - * Main * - ********/ - -void test_uart(void) -{ - char xmt[32] = "hello, world"; - char rcv[32] = {}; - - printf("start\r\n"); - for (int i = 0; xmt[i]; i++) { - uart1.putc(xmt[i]); - rcv[i] = uart2.getc(); - } - printf("xmt: %s\r\n", xmt); - printf("rcv: %s\r\n", rcv); -} - -void test_leds(void) -{ - led1 = 1; led2 = 0; wait(0.1); - led1 = 0; led2 = 1; wait(0.1); -} - -int main(int argc, char **argv) -{ - uart0.baud(115200); - uart1.baud(115200); - uart2.baud(115200); - - test_uart(); - test_leds(); - - while (1) { - printf("tick\r\n"); - test_leds(); - } -} +#include "messages.h" + +#include "mbed.h" +#include "serial_irq.h" +#include "serial_dma.h" +#include "timer_dma.h" + +/** + * Mode of operation: + * Devices 1 and 2 synchronize clocks using serial messages. + * + * 1. Each serial message timestamped using the hardware timer capture + * registers in both the sender and receiver. + * 2. The sender transmits the send timestamp during the next time-sync + * message. + * 3. The receiver then compares the senders timestamp with it's own + * timestamp for the corresponding messages and calculates an offset. + * 4. The offset is used to compensate the receivers local clock. + * + * Time synchronization is performed in both directions. + */ + +/**************** + * Testing vars * + ****************/ + +uint32_t test_xmt_enab = 0; +uint64_t test_xmt_time0 = 0; +uint64_t test_xmt_time1 = 0; + +uint32_t test_rcv_enab = 0; +uint64_t test_rcv_time = 0; + +/******************* + * Timer functions * + *******************/ + +#define NSEC_PER_SEC 1000000000ULL + +uint64_t time_last_local; // timestamp at last time sync +uint64_t time_last_world; // offset at last time sync + +/** + * Generate time stamp for an async event: + * local: drift compensated wall-clock time + * world: nanoseconds in world time world + * valid: local timestamp at valid valid + */ +//uint64_t time_to_local(uint64_t world, uint64_t valid) +//{ +// uint64_t now = +// local = + (stamp); +//} + +/** + * Generate time stamp for an async event: + * time: drift compensated wall-clock time + * stamp: event timestamp from PIT Module + */ +uint64_t time_to_world(uint64_t local) +{ + uint64_t elapsed = local - time_last_local; + return time_last_world + elapsed; +} + +/** + * Compensate the Real-Time-Clock oscillator for + * temperature and drift errors. Called at 1Hz and + * synchronous to the RTC 1Hz output. + */ +void time_rtc_comp(void) +{ + // todo +} + +/** + * Synchronize the timer internal state with updates + * from an external time sync message. + * local: our internal timestamp for the event + * world: reference timestamp from the other device + */ +void time_ext_init(uint64_t local, uint64_t world) +{ + sirq_printf("initialize clocks: %d -> %d\r\n", + (int)(local/NSEC_PER_SEC), + (int)(world/NSEC_PER_SEC)); + + time_last_local = local; + time_last_world = world; +} + +/** + * Synchronize the timer internal state with updates + * from an external time sync message. + * local: our internal timestamp for the event + * world: reference timestamp from the other device + */ +void time_ext_sync(uint64_t local, uint64_t world) +{ + uint64_t guess = time_to_world(local); + uint64_t error = world > guess ? world - guess : + guess > world ? guess - world : 0; + int ahead = guess > world; + + time_last_local = local; + time_last_world = (guess/2) + (world/2); + //time_last_world = (guess * 3 / 4) + (world * 1 / 4); + //time_last_world = + // (guess - ( guess / 2)) + + // (world - (world - world / 2)); + //time_last_world = + // (guess - (guess - guess / 4)) + + // (world - ( world / 4)); + + world = time_last_world; + +//#ifdef VERBOSE +#if 0 + sirq_printf("syncing clocks: %6d=%d.%04u -> %d.%04u (err: %s%ld.%09lu)\r\n", + (int)((local / NSEC_PER_SEC)), + (int)((guess / NSEC_PER_SEC)), + (int)((guess % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)), + (int)((world / NSEC_PER_SEC)), + (int)((world % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)), + ahead ? "-" : " ", + (int32_t )(error / (int64_t)NSEC_PER_SEC), + (uint32_t)(error % (int64_t)NSEC_PER_SEC)); +#endif +//#endif + + // .000000284 + // .000000253 + // .000000264 + // .000000451 + // .000000284 + // .000000267 + // .000000223 + // .000000326 + +} + +void time_printf(const char *label, uint64_t local) +{ + uint64_t world = time_to_world(local); + sirq_printf("%s -- %d.%09u -> %d.%09u\r\n", + label, + (int)(local / NSEC_PER_SEC), + (int)(local % NSEC_PER_SEC), + (int)(world / NSEC_PER_SEC), + (int)(world % NSEC_PER_SEC)); +} + +/************************ + * Serial I/O functions * + ************************/ + +typedef struct { + int index; + int state; + uint8_t buffer[256]; +} parser_t; + +const uint64_t serial_sync_delay = NSEC_PER_SEC / 100; // 1hz +static uint64_t serial_sync_due = 0; + +static tdma_t *serial_tdma_rcv = NULL; +static tdma_t *serial_tdma_xmt = NULL; + +static uint64_t serial_prev_local = 0; +static uint64_t serial_prev_seq = 0; + +static uint64_t serial_xmt_local = 0; +static uint64_t serial_xmt_seq = 0; + +/** + * Output time sync message + */ +void serial_send_sync(sirq_t *port, uint64_t now) +{ + if (serial_sync_due == 0 || now < serial_sync_due) + return; // not ready + + //sirq_printf("sending sync\r\n"); + + // Calculate world time + uint64_t local = 0; + uint64_t world = time_to_world(serial_xmt_local); + + // Message data + header_t head; + sync_msg_t body; + + // Transmit sync message + head.header = MSG_HEADER; + head.msgid = MSG_ID_SYNC; + head.length = sizeof(body); + head.cksum = 0; // todo + + body.seq = serial_xmt_seq; + body.time.seconds = world / NSEC_PER_SEC; + body.time.nanosec = world % NSEC_PER_SEC; + + tdma_stop(serial_tdma_rcv); + + test_xmt_enab = 1; + test_xmt_time0 = 0; + test_xmt_time1 = 0; + + tdma_start(serial_tdma_xmt); + sirq_write(port, &head, sizeof(head)); + sirq_write(port, &body, sizeof(body)); + tdma_stop(serial_tdma_xmt); + + // save transmit time + //local = test_xmt_time1; + int valid = tdma_stamp(serial_tdma_xmt, &local); + if (!valid) { + sirq_printf("sync transmit time -- missed\r\n"); + } else { + //time_printf("sync transmit time ", local); + //time_printf("sync transmit test0", test_xmt_time0); + //time_printf("sync transmit test1", test_xmt_time1); + } + + tdma_start(serial_tdma_rcv); + + serial_xmt_seq += 1; + serial_sync_due = 0; + serial_xmt_local = local; +} + +/** + * Output external event received message + * event: id of the received event + * time: compensated timestamp of the event + */ +void serial_send_event(uint16_t event, uint64_t local) +{ + uint64_t world = time_to_world(local); + + ntime_t time = {}; + time.seconds = (uint32_t)(world / NSEC_PER_SEC); + time.nanosec = (uint32_t)(world % NSEC_PER_SEC); + + sirq_printf("event received - %08x:%08x - %u.%09u\r\n", + (uint32_t)(local >> 32), (uint32_t)local, + time.seconds, time.nanosec); + // todo +} + +/** + * Handle sync message + */ +void serial_handle_sync(sync_msg_t *msg) +{ + // Read receive timestamp for next time sync message + uint64_t current = 0; + int valid = tdma_stamp(serial_tdma_rcv, ¤t); + if (!valid) + sirq_printf("sync receive time -- missing\r\n"); + //else + // time_printf("sync receive time ", current); + tdma_stop(serial_tdma_rcv); + + // Lookup times + uint64_t world = ((uint64_t)msg->time.seconds) * NSEC_PER_SEC + + ((uint64_t)msg->time.nanosec); + + // Initialize + if (msg->seq == 0) { + uint64_t local = tdma_time(); + time_ext_init(local, world); + } + + // Valid times timestamp + if (serial_prev_seq == (msg->seq-1)) { + uint64_t local = serial_prev_local; + time_ext_sync(local, world); + } + + // Queue transmit to other board + serial_sync_due = tdma_time() + serial_sync_delay; + + // Update states + serial_prev_local = current; + serial_prev_seq = msg->seq; +} + +/** + * Handle event message + */ +void serial_handle_event(event_msg_t *msg) +{ +} + +/** + * Deliver message + */ +void serial_deliver(int msgid, void *body) +{ + switch (msgid) { + case MSG_ID_SYNC: + //sirq_printf("received sync msg\r\n"); + serial_handle_sync((sync_msg_t*)body); + break; + case MSG_ID_EVENT: + //sirq_printf("received event msg\r\n"); + serial_handle_event((event_msg_t*)body); + break; + } +} + +/** + * Process serial receive messages + */ +void serial_receive(parser_t *parser, int byte) +{ + //sirq_printf("serial_receive - %02x\r\n", byte); + + // Lookup pointers + header_t *head = (header_t*)parser->buffer; + void *body = (void*)(head+1); + const int max_length = sizeof(parser->buffer)-sizeof(header_t); + + // Process uart messages + parser->buffer[parser->index++] = byte; + switch (parser->state) { + case 0: // Search + if (parser->index == sizeof(uint16_t)) { + if (head->header == MSG_HEADER) { + parser->state = 1; + } else { + parser->buffer[0] = parser->buffer[1]; + parser->index = 1; + } + } + break; + case 1: // Header + if (parser->index == sizeof(header_t)) { + if (head->length <= max_length && + head->msgid <= MSG_MAXID) { + parser->state = 2; + } else { + parser->index = 0; + parser->state = 0; + } + } + break; + case 2: // Data + if (parser->index == (int)sizeof(header_t)+head->length) { + serial_deliver(head->msgid, body); + parser->index = 0; + parser->state = 0; + } + break; + } +} + +/******************** + * Data definitions * + ********************/ + +// LEDs +DigitalOut led1(LED1); +DigitalOut led2(LED2); + +// Message Parsers +parser_t parser_dbg; +parser_t parser_bbb; +parser_t parser_mbed; + +// Serial IRQ +sirq_t *sirq_dbg; +sirq_t *sirq_bbb; +sirq_t *sirq_mbed; + +// Timer DMA +tdma_t *tdma_evt; +tdma_t *tdma_rcv; +tdma_t *tdma_xmt; + +/********* + * Tasks * + *********/ + +void task_serial(uint64_t local, uint64_t world) +{ + while (sirq_ready(sirq_dbg)) { + //sirq_printf("serial recv - dbg\r\n"); + serial_receive(&parser_dbg, sirq_getc(sirq_dbg)); + } + + while (sirq_ready(sirq_bbb)) { + //sirq_printf("serial recv - bbb\r\n"); + serial_receive(&parser_bbb, sirq_getc(sirq_bbb)); + } + + while (sirq_ready(sirq_mbed)) { + //sirq_printf("serial recv - mbed\r\n"); + serial_receive(&parser_mbed, sirq_getc(sirq_mbed)); + } +} + +void task_events(uint64_t local, uint64_t world) +{ + uint64_t event = 0; + +#ifdef VERBOSE + if (tdma_stamp(tdma_evt, &event)) { + sirq_printf("event received - evt\r\n"); + if (tdma_stamp(tdma_rcv, &event)) + sirq_printf("event received - rcv\r\n"); + if (tdma_stamp(tdma_xmt, &event)) + sirq_printf("event received - xmt\r\n"); +#endif + + if (tdma_stamp(tdma_evt, &event)) + serial_send_event(0, event); + tdma_stop(tdma_evt); + tdma_start(tdma_evt); +} + +void task_sync(uint64_t local, uint64_t world) +{ + serial_send_sync(sirq_mbed, local); +} + +void task_leds(uint64_t local, uint64_t world) +{ + static uint32_t which = 0; + led1 = (which == 0); + led2 = (which == 1); + which ^= 1; +} + +void task_debug(uint64_t local, uint64_t world) +{ + //tdma_debug(tdma_rcv); + //tdma_debug(tdma_xmt); + + //sirq_debug(sirq_mbed); + +#ifdef VERBOSE + sirq_printf("background - %6u.%02u -> %u.%02u\r\n", + (uint32_t)(local / NSEC_PER_SEC), + (uint32_t)(local % NSEC_PER_SEC / 10000000), + (uint32_t)(world / NSEC_PER_SEC), + (uint32_t)(world % NSEC_PER_SEC / 10000000)); +#endif +} + +/******** + * Main * + ********/ + +#define N_ELEM(x) (sizeof(x) / sizeof((x)[0])) + +extern void test_main(void); +extern serial_t stdio_uart; + +static struct { + void (*task)(uint64_t, uint64_t); + uint64_t period; + uint64_t due; +} tasks[] = { + { task_serial, 0 }, // always + { task_events, 1000000000 }, // always + { task_sync, 0 }, // always + { task_leds, 100000000 }, // 10hz + { task_debug, 1000000000 }, // 1hz +}; + +void background(void) +{ + // Debugging + uint64_t local = tdma_time(); + uint64_t world = time_to_world(local); + + // Run the scheduler + for (unsigned i = 0; i < N_ELEM(tasks); i++) { + if (local >= tasks[i].due) { + tasks[i].task(local, world); + tasks[i].due += tasks[i].period; + } + } +} + +int main(int argc, char **argv) +{ + tdma_init(); + + // Open serial ports + sirq_dbg = sirq_open(SIRQ_UART0, USBTX, USBRX, 115200); // to pc + sirq_bbb = sirq_open(SIRQ_UART1, PTE0, PTE1, 115200); // to bbb + sirq_mbed = sirq_open(SIRQ_UART2, PTD3, PTD2, 115200); // to mbed + + // Setup timers + tdma_evt = tdma_open(TDMA_CHAN0, 3, PTC9, PullUp); // async event + + // mbed time sync + tdma_rcv = tdma_open(TDMA_CHAN2, 3, PTD2, PullUp); // time sync rcv + tdma_xmt = tdma_open(TDMA_CHAN3, 3, PTD3, PullUp); // time sync xmt + + // host time sync + //tdma_rcv = tdma_open(TDMA_CHAN2, 2, USBRX, PullUp); // time sync rcv + //tdma_xmt = tdma_open(TDMA_CHAN3, 2, USBTX, PullUp); // time sync xmt + + // start timers + tdma_start(tdma_evt); + tdma_start(tdma_rcv); + tdma_start(tdma_xmt); + + // Serial timestamping + serial_tdma_rcv = tdma_rcv; + serial_tdma_xmt = tdma_xmt; + + // Test clocks + //MCG->C1 = 0x05; // was 0x1A + //MCG->C2 = 0x2C; // was 0x24 + //MCG->C3 = 0x91; // was 0x91 + //MCG->C4 = 0x10; // was 0x10 + //MCG->C5 = 0x01; // was 0x01 + //MCG->C6 = 0x40; // was 0x40 + //MCG->S = 0x6E; // was 0x6E + //MCG->SC = 0x02; // was 0x02 + //MCG->ATCVH = 0x00; // was 0x00 + //MCG->ATCVL = 0x00; // was 0x00 + //MCG->C7 = 0x00; // was 0x00 + //MCG->C8 = 0x80; // was 0x80 + //MCG->C9 = 0x00; // was 0x00 + //MCG->C10 = 0x00; // was 0x00 + + //sirq_printf("MGC - C1 %02hx\r\n", MCG->C1); // 1A + //sirq_printf("MGC - C2 %02hx\r\n", MCG->C2); // 24 + //sirq_printf("MGC - C3 %02hx\r\n", MCG->C3); // 91 + //sirq_printf("MGC - C4 %02hx\r\n", MCG->C4); // 10 + //sirq_printf("MGC - C5 %02hx\r\n", MCG->C5); // 01 + //sirq_printf("MGC - C6 %02hx\r\n", MCG->C6); // 40 + //sirq_printf("MGC - S %02hx\r\n", MCG->S); // 6E + //sirq_printf("MGC - SC %02hx\r\n", MCG->SC); // 02 + //sirq_printf("MGC - ATCVH %02hx\r\n", MCG->ATCVH); // 00 + //sirq_printf("MGC - ATCVL %02hx\r\n", MCG->ATCVL); // 00 + //sirq_printf("MGC - C7 %02hx\r\n", MCG->C7); // 00 + //sirq_printf("MGC - C8 %02hx\r\n", MCG->C8); // 80 + //sirq_printf("MGC - C9 %02hx\r\n", MCG->C9); // 00 + //sirq_printf("MGC - C10 %02hx\r\n", MCG->C10); // 00 + + // Run background loop + printf("hello"); + while (true) + background(); + + // Run tests + //test_main(); + + return 0; +}