Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : Send and receive JSON backend data to alow a second AP instance to ride along
17 : */
18 :
19 : #include "SIM_JSON_Master.h"
20 :
21 : #if AP_SIM_JSON_MASTER_ENABLED
22 :
23 : #include <AP_Logger/AP_Logger.h>
24 : #include <errno.h>
25 : #include <stdio.h>
26 : #include <SITL/SITL.h>
27 :
28 : using namespace SITL;
29 :
30 0 : void JSON_Master::init(const int32_t num_slaves)
31 : {
32 0 : socket_list *list = &_list;
33 0 : uint8_t i = 1;
34 0 : for (i = 1 ; i <= num_slaves; i++) {
35 : // init each socket and instance
36 0 : list->instance = i;
37 0 : uint16_t port = 9002 + 10 * i;
38 :
39 0 : if (!list->sock_in.reuseaddress()) {
40 0 : AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s", port, strerror(errno));
41 : }
42 :
43 0 : if (!list->sock_in.bind("127.0.0.1", port)) {
44 0 : AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s", port, strerror(errno));
45 : }
46 :
47 0 : if (!list->sock_in.set_blocking(false)) {
48 0 : AP_HAL::panic( "JSON master: socket set_blocking(false) failed on port: %d - %s", port, strerror(errno));
49 : }
50 :
51 0 : printf("Slave %u: listening on %u\n", list->instance, port);
52 0 : list->next = NEW_NOTHROW socket_list;
53 0 : list = list->next;
54 :
55 0 : initialized = true;
56 : }
57 0 : }
58 :
59 : // Receive PWM outs from ride along controlers
60 187275482 : void JSON_Master::receive(struct sitl_input &input)
61 : {
62 187275482 : if (!initialized) {
63 187275482 : return;
64 : }
65 :
66 0 : uint8_t master_instance = AP::sitl()->ride_along_master.get();
67 :
68 0 : for (socket_list *list = &_list; list->next; list=list->next) {
69 : // cycle through all ride along instances
70 : struct servo_packet {
71 : uint16_t magic;
72 : uint16_t frame_rate;
73 : uint32_t frame_count;
74 : uint16_t pwm[16];
75 0 : } buffer{};
76 :
77 : while (true) {
78 0 : ssize_t ret = list->sock_in.recv(&buffer, sizeof(buffer), 100);
79 0 : if (ret == 0) {
80 : // wait some more
81 0 : continue;
82 : }
83 0 : if (buffer.magic != 18458) {
84 : // magic value does not match
85 0 : continue;
86 : }
87 0 : if (!list->connected) {
88 : // connect back to the last address for send
89 : uint16_t port;
90 0 : const char *_ip = nullptr;
91 0 : list->sock_in.last_recv_address(_ip, port);
92 0 : list->connected = list->sock_out.connect(_ip, port) && (port != 0);
93 0 : if (list->connected) {
94 0 : printf("Slave %u connected to %s:%u\n", list->instance, _ip, port);
95 : }
96 : }
97 0 : if (list->connected) {
98 0 : break;
99 : }
100 0 : }
101 :
102 : #if HAL_LOGGING_ENABLED
103 0 : const bool use_servos = list->instance == master_instance;
104 :
105 : // @LoggerMessage: SLV1
106 : // @Description: Log data received from JSON simulator 1
107 : // @Field: TimeUS: Time since system startup (us)
108 : // @Field: Instance: Slave instance
109 : // @Field: magic: magic JSON protocol key
110 : // @Field: frame_rate: Slave instance's desired frame rate
111 : // @Field: frame_count: Slave instance's current frame count
112 : // @Field: active: 1 if the servo outputs are being used from this instance
113 0 : AP::logger().WriteStreaming("SLV1", "TimeUS,Instance,magic,frame_rate,frame_count,active",
114 : "s#----",
115 : "F?????",
116 : "QBHHIB",
117 : AP_HAL::micros64(),
118 0 : list->instance,
119 0 : buffer.magic,
120 0 : buffer.frame_rate,
121 : buffer.frame_count,
122 : use_servos);
123 :
124 : // @LoggerMessage: SLV2
125 : // @Description: Log data received from JSON simulator 2
126 : // @Field: TimeUS: Time since system startup
127 : // @Field: Instance: Slave instance
128 : // @Field: C1: channel 1 output
129 : // @Field: C2: channel 2 output
130 : // @Field: C3: channel 3 output
131 : // @Field: C4: channel 4 output
132 : // @Field: C5: channel 5 output
133 : // @Field: C6: channel 6 output
134 : // @Field: C7: channel 7 output
135 : // @Field: C8: channel 8 output
136 : // @Field: C9: channel 9 output
137 : // @Field: C10: channel 10 output
138 : // @Field: C11: channel 11 output
139 : // @Field: C12: channel 12 output
140 : // @Field: C13: channel 13 output
141 : // @Field: C14: channel 14 output
142 0 : AP::logger().WriteStreaming("SLV2", "TimeUS,Instance,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14",
143 : "s#YYYYYYYYYYYYYY",
144 : "F?--------------",
145 : "QBHHHHHHHHHHHHHH",
146 : AP_HAL::micros64(),
147 0 : list->instance,
148 0 : buffer.pwm[0],
149 0 : buffer.pwm[1],
150 0 : buffer.pwm[2],
151 0 : buffer.pwm[3],
152 0 : buffer.pwm[4],
153 0 : buffer.pwm[5],
154 0 : buffer.pwm[6],
155 0 : buffer.pwm[7],
156 0 : buffer.pwm[8],
157 0 : buffer.pwm[9],
158 0 : buffer.pwm[10],
159 0 : buffer.pwm[11],
160 0 : buffer.pwm[12],
161 0 : buffer.pwm[13]);
162 : #endif
163 :
164 0 : if (list->instance == master_instance) {
165 : // Use the servo outs from this instance
166 0 : memcpy(input.servos,buffer.pwm,sizeof(buffer.pwm));
167 : }
168 : }
169 : }
170 :
171 : // send vehicle state to ride along controlers
172 187275482 : void JSON_Master::send(const struct sitl_fdm &output, const Vector3d &position)
173 : {
174 187275482 : if (!initialized) {
175 187275482 : return;
176 : }
177 :
178 : // message is the same to all slaves
179 0 : int length = asprintf(&json_out,"\n{\"timestamp\":%f,\"imu\":{\"gyro\":[%f,%f,%f],\"accel_body\":[%f,%f,%f]},\"position\":[%f,%f,%f],\"quaternion\":[%f,%f,%f,%f],\"velocity\":[%f,%f,%f],\"no_time_sync\":1}\n",
180 0 : output.timestamp_us * 1e-6,
181 0 : radians(output.rollRate), radians(output.pitchRate), radians(output.yawRate),
182 0 : output.xAccel, output.yAccel, output.zAccel,
183 0 : position.x, position.y, position.z,
184 0 : output.quaternion.q1, output.quaternion.q2, output.quaternion.q3, output.quaternion.q4,
185 0 : output.speedN, output.speedE, output.speedD);
186 :
187 0 : for (socket_list *list = &_list; list->next; list=list->next) {
188 0 : list->sock_out.send(json_out,length);
189 : }
190 0 : free(json_out);
191 : }
192 :
193 :
194 : #endif // AP_SIM_JSON_MASTER_ENABLED
|
