1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
|
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <stdint.h>
#include <termios.h>
#include <unistd.h>
#include "sim.h"
#include "../shared/consts.h"
#include "../shared/protocol.h"
#include "sercomm.h"
#include "errcatch.h"
struct timespec reference_time; // NOLINT
bool g_w2_sim_headless = false;
static const char* const W2_CMD_NAMES[] = {
"PING",
"EXPT",
"MODE",
"SPED",
"DIRC",
"CORD",
"BOMD",
"SRES",
"MCFG",
"SENS",
"INFO",
"DISP",
"PLAY",
"CLED",
};
static const char* const W2_CMD_DIRECTIONS[] = {
"RX",
"TX",
};
void time_reset() {
simprintfunc("time_reset", "");
clock_gettime(CLOCK_MONOTONIC, &reference_time);
}
unsigned long get_ms() {
simprintfunc("get_ms", "");
struct timespec elapsed;
clock_gettime(CLOCK_MONOTONIC, &elapsed);
return ((elapsed.tv_sec * 1000) + (elapsed.tv_nsec / 1000000)) -
((reference_time.tv_sec * 1000) + (reference_time.tv_nsec / 1000000));
}
void red_led(unsigned char on) {
simprintfunc("red_led", "%i", on);
}
void green_led(unsigned char on) {
simprintfunc("green_led", "%i", on);
}
void clear() {
simprintfunc("clear", "");
}
void play(const char* melody) {
simprintfunc("play", "\"%s\"", melody);
}
void serial_set_baud_rate(unsigned int rate) {
simprintfunc("serial_set_baud_rate", "%u", rate);
}
void serial_send(char* message, unsigned int length) {
if (g_w2_sim_headless) {
for (unsigned int byte = 0; byte < length; byte++)
putc(message[byte] & 0xff, stdout);
return;
}
if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", "<see below>, %u", length);
if (!DBG_ENABLE_SERIAL) return;
w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message);
w2_sim_print_serial(bin);
free(bin);
}
void serial_receive_ring(char* buffer, unsigned char size) {
simprintfunc("serial_receive_ring", "0x%016lx, %u", (unsigned long) buffer, size);
}
unsigned char serial_get_received_bytes() {
simprintfunc("serial_get_received_bytes", "");
return g_w2_serial_buffer_head;
}
void w2_sim_setup(int argc, char **argv) {
if (argc > 1 && strcmp(argv[1], "headless") == 0)
g_w2_sim_headless = true;
// disable echo and enable raw mode
struct termios term;
tcgetattr(STDIN_FILENO, &term);
term.c_lflag &= ~(ECHO | ICANON);
term.c_cc[VTIME] = 0;
term.c_cc[VMIN] = 0;
tcsetattr(STDIN_FILENO, 0, &term);
// debug error
// w2_errcatch_throw(W2_E_WARN_BATTERY_LOW);
}
void w2_sim_cycle_begin() {
// read bytes from stdin
while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0)
g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE;
}
void w2_sim_print_serial(w2_s_bin *data) {
if (g_w2_sim_headless) return;
simprintf(COL_GRN "[%s_%s]" COL_RST, W2_CMD_NAMES[data->data[0] >> 1], W2_CMD_DIRECTIONS[data->data[0] & W2_CMD_DIRECTION_MASK]);
for (int i = 0; i < data->bytes; i++)
printf(" %02x", data->data[i]);
printf("\n");
}
void set_motors(int left, int right) {
simprintfunc("set_motors", "%i, %i", left, right);
}
|