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 : Simulator for Topotek gimbal
17 : */
18 :
19 : #include "SIM_config.h"
20 : #include <AP_Common/AP_Common.h>
21 :
22 : #if AP_SIM_TOPOTEK_ENABLED
23 :
24 : #include "SIM_Topotek.h"
25 : #include "SIM_Aircraft.h"
26 : #include <AP_Math/AP_Math.h>
27 : #include <errno.h>
28 :
29 : using namespace SITL;
30 :
31 255787 : void Topotek::update(const Aircraft &aircraft)
32 : {
33 : // Drive GimbalSim toward the angles commanded via GIP/GIY packets.
34 : // Wire encoding: pitch_cd = -pitch_deg * 100 (driver negates on read),
35 : // yaw_cd = yaw_deg * 100.
36 : {
37 255787 : const float target_pitch_rad = -radians(_commanded_pitch_cd * 0.01f);
38 255787 : const float target_yaw_rad = radians(_commanded_yaw_cd * 0.01f);
39 :
40 255787 : Vector3f ja;
41 255787 : gimbal.get_joint_angles(ja);
42 :
43 255787 : Matrix3f gimbal_dcm;
44 255787 : gimbal.get_dcm(gimbal_dcm);
45 255787 : const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * aircraft.get_dcm() * aircraft.get_gyro();
46 :
47 : static constexpr float GAIN = 10.0f;
48 255787 : gimbal.set_demanded_rates(Vector3f(
49 255787 : vehicle_rate_gimbal.x,
50 255787 : vehicle_rate_gimbal.y + (target_pitch_rad - ja.y) * GAIN,
51 255787 : vehicle_rate_gimbal.z + (target_yaw_rad - ja.z) * GAIN));
52 : }
53 :
54 255787 : gimbal.update(aircraft);
55 255787 : update_input();
56 :
57 : // send attitude at 10 Hz
58 255787 : const uint32_t now_ms = AP_HAL::millis();
59 255787 : if (now_ms - _last_attitude_ms >= 100) {
60 2130 : _last_attitude_ms = now_ms;
61 2130 : send_attitude();
62 : }
63 255787 : }
64 :
65 2339 : void Topotek::send_attitude()
66 : {
67 : // Report actual GimbalSim joint angles.
68 : // joint_angles.y = pitch (negative = down), .z = azimuth.
69 : // Wire encoding: pitch_cd = -pitch_deg * 100 (driver negates on read).
70 : // yaw_cd = yaw_deg * 100.
71 2339 : Vector3f ja;
72 2339 : gimbal.get_joint_angles(ja);
73 2339 : const int16_t yaw_cd = (int16_t)(degrees(ja.z) * 100.0f);
74 2339 : const int16_t pitch_cd = (int16_t)(-degrees(ja.y) * 100.0f);
75 2339 : const int16_t roll_cd = (int16_t)(degrees(ja.x) * 100.0f);
76 :
77 : uint8_t data[12];
78 2339 : uint16_to_hex4((uint16_t)yaw_cd, &data[0]);
79 2339 : uint16_to_hex4((uint16_t)pitch_cd, &data[4]);
80 2339 : uint16_to_hex4((uint16_t)roll_cd, &data[8]);
81 :
82 2339 : send_packet('G', "GIA", false, data, sizeof(data));
83 2339 : }
84 :
85 : /*
86 : read bytes from autopilot into _buf, then scan for complete packets.
87 : Packet format:
88 : [0] '#'
89 : [1] 'T' or 't'
90 : [2] 'P' or 'p'
91 : [3] 'U' (addr1)
92 : [4] addr2 ('G','D','M','E','P')
93 : [5] data_len as a single ASCII hex char
94 : [6] 'r' or 'w'
95 : [7..9] 3-char command ID
96 : [10..10+data_len-1] data bytes
97 : [10+data_len..11+data_len] 2-byte CRC
98 : Total packet length = 12 + data_len
99 : */
100 14764 : void Topotek::move_preamble_in_buffer(uint8_t search_start_pos)
101 : {
102 : uint8_t i;
103 14764 : for (i = search_start_pos; i < _buflen; i++) {
104 11296 : if (_buf[i] == '#') {
105 11296 : break;
106 : }
107 : }
108 14764 : if (i == 0) {
109 0 : return;
110 : }
111 14764 : memmove(_buf, &_buf[i], _buflen - i);
112 14764 : _buflen -= i;
113 : }
114 :
115 255787 : void Topotek::update_input()
116 : {
117 255787 : const ssize_t n = read_from_autopilot((char*)&_buf[_buflen], ARRAY_SIZE(_buf) - _buflen - 1);
118 255787 : if (n < 0) {
119 0 : if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
120 0 : AP_HAL::panic("Failed to read from autopilot");
121 : }
122 0 : return;
123 : }
124 255787 : _buflen += n;
125 :
126 270551 : while (_buflen >= 3) {
127 : // search for '#' at the start
128 24050 : if (_buf[0] != '#') {
129 0 : move_preamble_in_buffer(1);
130 0 : continue;
131 : }
132 24050 : if (_buf[1] != 'T' && _buf[1] != 't') {
133 0 : move_preamble_in_buffer(1);
134 0 : continue;
135 : }
136 24050 : if (_buf[2] != 'P' && _buf[2] != 'p') {
137 0 : move_preamble_in_buffer(1);
138 0 : continue;
139 : }
140 :
141 : // need at least 6 bytes to read the data_len field
142 24050 : if (_buflen < 6) {
143 9286 : break;
144 : }
145 :
146 : // parse data length from ASCII hex char at [5]
147 : uint8_t data_len;
148 23419 : if (!hex_char_to_nibble(_buf[5], data_len)) {
149 : // invalid data length — discard '#'
150 0 : move_preamble_in_buffer(1);
151 0 : continue;
152 : }
153 :
154 23419 : const uint8_t pkt_len = 12 + data_len;
155 23419 : if (pkt_len > PACKETLEN_MAX) {
156 0 : move_preamble_in_buffer(1);
157 0 : continue;
158 : }
159 :
160 : // wait for the full packet
161 23419 : if (_buflen < pkt_len) {
162 8655 : break;
163 : }
164 :
165 : // verify and dispatch the packet
166 14764 : handle_packet(data_len);
167 14764 : move_preamble_in_buffer(pkt_len);
168 : }
169 : }
170 :
171 14764 : void Topotek::handle_packet(uint8_t data_len)
172 : {
173 : // verify CRC
174 14764 : const uint8_t pkt_len = 12 + data_len;
175 14764 : const uint8_t crc = crc_sum_of_bytes(_buf, pkt_len - 2);
176 14764 : const uint8_t expected_hi = hex2char((crc >> 4) & 0x0f);
177 14764 : const uint8_t expected_lo = hex2char(crc & 0x0f);
178 14764 : if (_buf[pkt_len - 2] != expected_hi || _buf[pkt_len - 1] != expected_lo) {
179 0 : return;
180 : }
181 :
182 : // byte [3] is the source address of the sender; replies (including
183 : // the attitude stream) are sent to that address from now on
184 14764 : _reply_address = _buf[3];
185 :
186 : // ID is at bytes [7..9]
187 14764 : const char *id = (const char*)&_buf[7];
188 :
189 14764 : if (strncmp(id, "GIA", 3) == 0) {
190 : // attitude request
191 209 : send_attitude();
192 :
193 14555 : } else if (strncmp(id, "VSN", 3) == 0) {
194 2 : const uint8_t data[] { '1', '.', '0', '.', '0' };
195 2 : send_packet('D', "VSN", false, data, sizeof(data));
196 :
197 14553 : } else if (strncmp(id, "PA2", 3) == 0) {
198 2 : const uint8_t data[] { 'S', 'I', 'M', '_', 'T', 'P' };
199 2 : send_packet('G', "PA2", false, data, sizeof(data));
200 :
201 14551 : } else if (strncmp(id, "SDC", 3) == 0) {
202 : // card present: any 4 bytes that are NOT all 'N'
203 209 : const uint8_t data[] { '1', '0', '0', '0' };
204 209 : send_packet('D', "SDC", false, data, sizeof(data));
205 :
206 14342 : } else if (strncmp(id, "TRC", 3) == 0) {
207 : // tracking stopped: driver reads _msg_buff[11] as TrackingStatus;
208 : // [11] = data[1] = '0' = 0x30 = STOPPED_TRACKING
209 209 : const uint8_t data[] { '0', '0' };
210 209 : send_packet('D', "TRC", false, data, sizeof(data));
211 :
212 14133 : } else if (strncmp(id, "GIP", 3) == 0 && data_len >= 4) {
213 : // pitch angle command: data[0..3] = 4 uppercase hex chars for int16 centidegrees
214 : // Wire value is (uint16_t)(-degrees(pitch_rad)*100); store as-is for echo in send_attitude()
215 : uint32_t tmp;
216 2082 : if (hex_chars_to_uint32((const char*)&_buf[10], 4, tmp)) {
217 2082 : _commanded_pitch_cd = (int16_t)tmp;
218 : }
219 :
220 14133 : } else if (strncmp(id, "GIY", 3) == 0 && data_len >= 4) {
221 : // body-frame yaw angle command
222 : uint32_t tmp;
223 0 : if (hex_chars_to_uint32((const char*)&_buf[10], 4, tmp)) {
224 0 : _commanded_yaw_cd = (int16_t)tmp;
225 : }
226 : }
227 : // all other commands (YPR, GIR, PTZ, LAT, LON, ALT, etc.) absorbed silently
228 : }
229 :
230 2761 : void Topotek::send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len)
231 : {
232 2761 : const uint8_t total = 12 + len;
233 2761 : if (total > PACKETLEN_MAX) {
234 25 : return;
235 : }
236 :
237 : uint8_t pkt[PACKETLEN_MAX];
238 2761 : uint8_t ofs = 0;
239 :
240 2761 : pkt[ofs++] = '#';
241 2761 : pkt[ofs++] = 'T';
242 2761 : pkt[ofs++] = 'P';
243 2761 : pkt[ofs++] = _reply_address;
244 2761 : pkt[ofs++] = (uint8_t)addr2;
245 2761 : pkt[ofs++] = hex2char(len & 0x0f); // data length as single ASCII hex char
246 2761 : pkt[ofs++] = write ? 'w' : 'r';
247 2761 : pkt[ofs++] = (uint8_t)id[0];
248 2761 : pkt[ofs++] = (uint8_t)id[1];
249 2761 : pkt[ofs++] = (uint8_t)id[2];
250 :
251 32105 : for (uint8_t i = 0; i < len; i++) {
252 29344 : pkt[ofs++] = data[i];
253 : }
254 :
255 : // checksum: byte sum of all preceding bytes, encoded as 2 uppercase ASCII hex chars
256 2761 : const uint8_t crc = crc_sum_of_bytes(pkt, ofs);
257 2761 : pkt[ofs++] = hex2char((crc >> 4) & 0x0f);
258 2761 : pkt[ofs++] = hex2char(crc & 0x0f);
259 :
260 2761 : if (_reply_address != connected_interface()) {
261 : // the packet is sent out of an interface the autopilot is not
262 : // connected to, so it is lost
263 25 : return;
264 : }
265 :
266 2736 : write_to_autopilot((const char*)pkt, ofs);
267 : }
268 :
269 7017 : void Topotek::uint16_to_hex4(uint16_t val, uint8_t buf[4])
270 : {
271 7017 : buf[0] = hex2char((val >> 12) & 0x0f);
272 7017 : buf[1] = hex2char((val >> 8) & 0x0f);
273 7017 : buf[2] = hex2char((val >> 4) & 0x0f);
274 7017 : buf[3] = hex2char((val ) & 0x0f);
275 7017 : }
276 :
277 : #endif // AP_SIM_TOPOTEK_ENABLED
|
