Line data Source code
1 : /*
2 : * AP_VOLZ_PROTOCOL.cpp
3 : *
4 : * Created on: Oct 31, 2017
5 : * Author: guy
6 : */
7 : #include "AP_Volz_Protocol.h"
8 :
9 : #if AP_VOLZ_ENABLED
10 :
11 : #include <AP_HAL/AP_HAL.h>
12 :
13 : #include <AP_SerialManager/AP_SerialManager.h>
14 : #include <SRV_Channel/SRV_Channel.h>
15 : #include <AP_BoardConfig/AP_BoardConfig.h>
16 :
17 : #include <AP_Servo_Telem/AP_Servo_Telem.h>
18 :
19 : // Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default)
20 : #define PWM_POSITION_MIN 1000
21 : #define ANGLE_POSITION_MIN -100.0
22 : #define EXTENDED_POSITION_MIN 0x0080
23 :
24 : // Extended Position Data Format defines +100 as 0x0F80 decimal 3968, we map this to a PWM of 2000 (if range is default)
25 : #define PWM_POSITION_MAX 2000
26 : #define ANGLE_POSITION_MAX 100.0
27 : #define EXTENDED_POSITION_MAX 0x0F80
28 :
29 : #define UART_BUFSIZE_RX 128
30 : #define UART_BUFSIZE_TX 128
31 :
32 : extern const AP_HAL::HAL& hal;
33 :
34 : const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = {
35 : // @Param: MASK
36 : // @DisplayName: Channel Bitmask
37 : // @Description: Enable of volz servo protocol to specific channels
38 : // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16,16:Channel17,17:Channel18,18:Channel19,19:Channel20,20:Channel21,21:Channel22,22:Channel23,23:Channel24,24:Channel25,25:Channel26,26:Channel27,28:Channel29,29:Channel30,30:Channel31,31:Channel32
39 : // @User: Standard
40 : AP_GROUPINFO("MASK", 1, AP_Volz_Protocol, bitmask, 0),
41 :
42 : // @Param: RANGE
43 : // @DisplayName: Range of travel
44 : // @Description: Range to map between 1000 and 2000 PWM. Default value of 200 gives full +-100 deg range of extended position command. This results in 0.2 deg movement per US change in PWM. If the full range is not needed it can be reduced to increase resolution. 40 deg range gives 0.04 deg movement per US change in PWM, this is higher resolution than possible with the VOLZ protocol so further reduction in range will not improve resolution. Reduced range does allow PWMs outside the 1000 to 2000 range, with 40 deg range 750 PWM results in a angle of -30 deg, 2250 would be +30 deg. This is still limited by the 200 deg maximum range of the actuator.
45 : // @Units: deg
46 : AP_GROUPINFO("RANGE", 2, AP_Volz_Protocol, range, 200),
47 :
48 : AP_GROUPEND
49 : };
50 :
51 : // constructor
52 2094 : AP_Volz_Protocol::AP_Volz_Protocol(void)
53 : {
54 : // set defaults from the parameter table
55 2094 : AP_Param::setup_object_defaults(this, var_info);
56 2094 : }
57 :
58 2056 : void AP_Volz_Protocol::init(void)
59 : {
60 2056 : if (uint32_t(bitmask.get()) == 0) {
61 : // No servos enabled
62 2053 : return;
63 : }
64 :
65 3 : AP_SerialManager &serial_manager = AP::serialmanager();
66 3 : port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Volz,0);
67 3 : if (port == nullptr) {
68 : // No port configured
69 0 : return;
70 : }
71 :
72 : // update baud param in case user looks at it
73 3 : serial_manager.set_and_default_baud(AP_SerialManager::SerialProtocol_Volz, 0, 115200);
74 :
75 : // Create thread to handle output
76 3 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Volz_Protocol::loop, void),
77 : "Volz",
78 : 1024, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
79 0 : AP_BoardConfig::allocation_error("Volz thread");
80 : }
81 : }
82 :
83 : #if AP_SERVO_TELEM_ENABLED
84 : // Request telem data, cycling through each servo and telem item
85 7011 : void AP_Volz_Protocol::request_telem()
86 : {
87 : // Request the queued item, making sure the servo is enabled
88 7011 : if ((uint32_t(bitmask.get()) & (1U<<telem.actuator_id)) != 0) {
89 : // Assemble command
90 6524 : CMD cmd {};
91 6524 : cmd.ID = telem.types[telem.request_type];
92 6524 : cmd.actuator_id = telem.actuator_id + 1;
93 6524 : send_command(cmd);
94 :
95 : // Increment the request type
96 6524 : telem.request_type++;
97 6524 : if (telem.request_type < ARRAY_SIZE(telem.types)) {
98 : // Request the next telem type from the same actuators on the next call
99 4350 : return;
100 : }
101 : }
102 :
103 : // Requested all items from a id or invalid id.
104 : // start again with a new id
105 2661 : telem.request_type = 0;
106 :
107 : // Same logic as `send_position_cmd`
108 27642 : for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) {
109 27155 : const uint8_t index = (telem.actuator_id + 1 + i) % ARRAY_SIZE(telem.data);
110 27155 : if ((uint32_t(bitmask.get()) & (1U<<index)) == 0) {
111 24981 : continue;
112 : }
113 2174 : telem.actuator_id = index;
114 2174 : break;
115 : }
116 : }
117 : #endif
118 :
119 3 : void AP_Volz_Protocol::loop()
120 : {
121 3 : const uint32_t baudrate = 115200;
122 3 : port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_RTS_DE);
123 3 : port->begin(baudrate, UART_BUFSIZE_RX, UART_BUFSIZE_TX);
124 3 : port->set_unbuffered_writes(true);
125 :
126 : // Calculate the amount of time it should take to send a command
127 : // Multiply by 10 to convert from bit rate to byte rate (8 data bits + start and stop bits)
128 : // B/s to s/B, 1000000 converts to microseconds, multiply by number of bytes
129 : // 6 bytes at 11520 bytes per second takes 520 us
130 3 : const uint16_t send_us = (sizeof(CMD) * 1000000 * 10) / baudrate;
131 :
132 : // receive packet is same length as sent, double to allow some time for the servo respond
133 3 : const uint16_t receive_us = send_us * 2;
134 :
135 : // This gives a total time of 1560us, message rate of 641 Hz.
136 : // One servo at 641Hz, two at 320.5 each, three at 213.7 each ect...
137 : // Note that we send a telem request every time servo sending is complete. This is like a extra servo.
138 : // So for a single servo position commands are at 320.5Hz and telem at 320.5Hz.
139 :
140 46167 : while (port != nullptr) {
141 :
142 : // Wait the expected amount of time for the send and receive to complete so we don't step on the response
143 46167 : hal.scheduler->delay_microseconds(send_us + receive_us);
144 :
145 46164 : while (port->txspace() < sizeof(CMD)) {
146 : // Wait until there is enough space to transmit a full command
147 0 : hal.scheduler->delay_microseconds(100);
148 : }
149 :
150 : #if AP_SERVO_TELEM_ENABLED
151 : // Send a command for each servo, then one telem request
152 46164 : const uint8_t num_servos = __builtin_popcount(bitmask.get());
153 46164 : if (sent_count < num_servos) {
154 39153 : send_position_cmd();
155 39153 : sent_count++;
156 :
157 : } else {
158 7011 : request_telem();
159 7011 : sent_count = 0;
160 : }
161 :
162 : {
163 : // Read in telem data if available
164 46164 : WITH_SEMAPHORE(telem.sem);
165 46164 : read_telem();
166 46164 : }
167 :
168 : #else // No telem, send only
169 : send_position_cmd();
170 : #endif
171 : }
172 0 : }
173 :
174 : // Send position commands from PWM, cycle through each servo
175 39153 : void AP_Volz_Protocol::send_position_cmd()
176 : {
177 :
178 : // loop for all channels
179 208788 : for (uint8_t i=0; i<ARRAY_SIZE(servo_pwm); i++) {
180 : // Send each channels in turn
181 208788 : const uint8_t index = (last_sent_index + 1 + i) % ARRAY_SIZE(servo_pwm);
182 208788 : if ((uint32_t(bitmask.get()) & (1U<<index)) == 0) {
183 : // Not configured to send
184 169635 : continue;
185 : }
186 39153 : last_sent_index = index;
187 :
188 : // Get PWM from saved array
189 39153 : const uint16_t pwm = servo_pwm[index];
190 39153 : if (pwm == 0) {
191 : // Never use zero PWM, the check in update should ensure this never happens
192 : // If we were to use zero the range extrapolation would result in a -100 deg angle request
193 0 : continue;
194 : }
195 :
196 : // Map PWM to angle, this is a un-constrained interpolation
197 : // ratio = 0 at PWM_POSITION_MIN to 1 at PWM_POSITION_MAX
198 39153 : const float ratio = (float(pwm) - PWM_POSITION_MIN) / (PWM_POSITION_MAX - PWM_POSITION_MIN);
199 : // Convert ratio to +-0.5 and multiply by stroke
200 39153 : const float angle = (ratio - 0.5) * constrain_float(range, 0.0, 200.0);
201 :
202 : // Map angle to command out of full range, add 0.5 so that float to int truncation rounds correctly
203 39153 : const uint16_t value = linear_interpolate(EXTENDED_POSITION_MIN, EXTENDED_POSITION_MAX, angle, ANGLE_POSITION_MIN, ANGLE_POSITION_MAX) + 0.5;
204 :
205 : // prepare Volz protocol data.
206 : CMD cmd;
207 39153 : cmd.ID = CMD_ID::SET_EXTENDED_POSITION;
208 39153 : cmd.actuator_id = index + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 ....
209 39153 : cmd.arg1 = HIGHBYTE(value);
210 39153 : cmd.arg2 = LOWBYTE(value);
211 :
212 39153 : send_command(cmd);
213 :
214 : #if AP_SERVO_TELEM_ENABLED
215 : {
216 : // Update the commanded angle
217 39153 : WITH_SEMAPHORE(telem.sem);
218 : static_assert(ARRAY_SIZE(servo_pwm) == ARRAY_SIZE(telem.data), "actuator index invalid for telem data array");
219 39153 : telem.data[index].desired_angle = angle;
220 39153 : }
221 : #endif
222 :
223 39153 : break;
224 : }
225 39153 : }
226 :
227 : #if AP_SERVO_TELEM_ENABLED
228 45675 : void AP_Volz_Protocol::process_response(const CMD &cmd)
229 : {
230 : // Convert to 0 indexed
231 45675 : const uint8_t index = cmd.actuator_id - 1;
232 45675 : if (index >= ARRAY_SIZE(telem.data)) {
233 : // Invalid ID
234 0 : return;
235 : }
236 :
237 45675 : switch (cmd.ID) {
238 39152 : case CMD_ID::EXTENDED_POSITION_RESPONSE:
239 : // Map back to angle
240 39152 : telem.data[index].angle = linear_interpolate(ANGLE_POSITION_MIN, ANGLE_POSITION_MAX, UINT16_VALUE(cmd.arg1, cmd.arg2), EXTENDED_POSITION_MIN, EXTENDED_POSITION_MAX);
241 39152 : break;
242 :
243 2175 : case CMD_ID::CURRENT_RESPONSE:
244 : // Current is reported in 20mA increments (0.02A)
245 2175 : telem.data[index].primary_current = cmd.arg1 * 0.02;
246 2175 : telem.data[index].secondary_current = cmd.arg2 * 0.02;
247 2175 : break;
248 :
249 2175 : case CMD_ID::VOLTAGE_RESPONSE:
250 : // Voltage is reported in 200mv increments (0.2v)
251 2175 : telem.data[index].primary_voltage = cmd.arg1 * 0.2;
252 2175 : telem.data[index].secondary_voltage = cmd.arg2 * 0.2;
253 2175 : break;
254 :
255 2173 : case CMD_ID::TEMPERATURE_RESPONSE:
256 : // Temperature is reported relative to -50 deg C
257 2173 : telem.data[index].motor_temp_deg = -50 + cmd.arg1;
258 2173 : telem.data[index].pcb_temp_deg = -50 + cmd.arg2;
259 2173 : break;
260 :
261 0 : default:
262 : // This should never happen
263 0 : return;
264 : }
265 :
266 45675 : telem.data[index].last_response_ms = AP_HAL::millis();
267 : }
268 :
269 : // Return true if the given ID is a valid response
270 45675 : bool AP_Volz_Protocol::is_response(uint8_t ID) const
271 : {
272 45675 : switch(ID) {
273 45675 : case (uint8_t)CMD_ID::EXTENDED_POSITION_RESPONSE:
274 : case (uint8_t)CMD_ID::CURRENT_RESPONSE:
275 : case (uint8_t)CMD_ID::VOLTAGE_RESPONSE:
276 : case (uint8_t)CMD_ID::TEMPERATURE_RESPONSE:
277 45675 : return true;
278 :
279 0 : default:
280 0 : break;
281 : }
282 :
283 0 : return false;
284 : }
285 :
286 46164 : void AP_Volz_Protocol::read_telem()
287 : {
288 : // Try and read data a few times, this could be a while loop, using a for loop gives a upper bound to run time
289 91839 : for (uint8_t attempts = 0; attempts < sizeof(telem.cmd_buffer) * 4; attempts++) {
290 :
291 91839 : uint32_t n = port->available();
292 91839 : if (n == 0) {
293 : // No data available
294 46164 : return;
295 : }
296 45675 : if (telem.buffer_offset < sizeof(telem.cmd_buffer)) {
297 : // Read enough bytes to fill buffer
298 45675 : ssize_t nread = port->read(&telem.cmd_buffer.data[telem.buffer_offset], MIN(n, unsigned(sizeof(telem.cmd_buffer)-telem.buffer_offset)));
299 45675 : if (nread <= 0) {
300 : // Read failed
301 0 : return;
302 : }
303 45675 : telem.buffer_offset += nread;
304 : }
305 :
306 : // Check for valid response start byte
307 45675 : if (!is_response(telem.cmd_buffer.data[0])) {
308 :
309 : // Search for a valid response start byte
310 : uint8_t cmd_start;
311 0 : for (cmd_start = 1; cmd_start < telem.buffer_offset; cmd_start++) {
312 0 : if (is_response(telem.cmd_buffer.data[cmd_start])) {
313 : // Found one
314 0 : break;
315 : }
316 : }
317 :
318 : // Shift buffer to put start on valid byte, or if no valid byte was found clear
319 0 : const uint8_t n_move = telem.buffer_offset - cmd_start;
320 0 : if (n_move > 0) {
321 : // No need to move 0 bytes
322 0 : memmove(&telem.cmd_buffer.data[0], &telem.cmd_buffer.data[cmd_start], n_move);
323 : }
324 0 : telem.buffer_offset = 0;
325 :
326 : // Since the buffer is the same length as a full command, we can never get a valid packet after shifting
327 : // Always need to read in some more data
328 0 : continue;
329 0 : }
330 :
331 45675 : if (telem.buffer_offset < sizeof(telem.cmd_buffer)) {
332 : // Not enough data to make up packet
333 0 : continue;
334 : }
335 :
336 : // Have valid ID and enough data, check crc
337 45675 : if (UINT16_VALUE(telem.cmd_buffer.crc1, telem.cmd_buffer.crc2) != calculate_crc(telem.cmd_buffer)) {
338 : // Probably lost sync shift by one and try again
339 0 : memmove(&telem.cmd_buffer.data[0], &telem.cmd_buffer.data[1], telem.buffer_offset - 1);
340 0 : telem.buffer_offset -= 1;
341 0 : continue;
342 : }
343 :
344 : // Valid packet passed crc check
345 45675 : process_response(telem.cmd_buffer);
346 :
347 : // zero offset and continue
348 45675 : telem.buffer_offset = 0;
349 : }
350 :
351 : // Used up all attempts without running out of data.
352 : // Really should not end up here
353 : }
354 : #endif // AP_SERVO_TELEM_ENABLED
355 :
356 : // Called each time the servo outputs are sent
357 41692164 : void AP_Volz_Protocol::update()
358 : {
359 41692164 : if (!initialised) {
360 : // One time setup
361 2056 : initialised = true;
362 2056 : init();
363 : }
364 :
365 41692164 : if (port == nullptr) {
366 : // no point if we don't have a valid port
367 41683107 : return;
368 : }
369 :
370 : // take semaphore and loop for all channels
371 298881 : for (uint8_t i=0; i<ARRAY_SIZE(servo_pwm); i++) {
372 289824 : const SRV_Channel *c = SRV_Channels::srv_channel(i);
373 289824 : if (c == nullptr) {
374 0 : continue;
375 : }
376 : // 0 PMW should stop outputting, for example in "safe"
377 : // There is no way to de-power, move to trim
378 289824 : const uint16_t pwm = c->get_output_pwm();
379 289824 : servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm;
380 : }
381 :
382 : #if AP_SERVO_TELEM_ENABLED
383 : // Report telem data
384 9057 : AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
385 9057 : if (servo_telem != nullptr) {
386 9057 : const uint32_t now_ms = AP_HAL::millis();
387 :
388 9057 : WITH_SEMAPHORE(telem.sem);
389 298881 : for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) {
390 289824 : if ((telem.data[i].last_response_ms == 0) || ((now_ms - telem.data[i].last_response_ms) > 5000)) {
391 : // Never seen telem, or not had a response for more than 5 seconds
392 235504 : continue;
393 : }
394 :
395 54320 : const AP_Servo_Telem::TelemetryData telem_data {
396 54320 : .command_position = telem.data[i].desired_angle,
397 54320 : .measured_position = telem.data[i].angle,
398 54320 : .voltage = telem.data[i].primary_voltage,
399 54320 : .current = telem.data[i].primary_current,
400 54320 : .motor_temperature_cdeg = int16_t(telem.data[i].motor_temp_deg * 100),
401 54320 : .pcb_temperature_cdeg = int16_t(telem.data[i].pcb_temp_deg * 100),
402 : .present_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
403 : AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
404 : AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
405 : AP_Servo_Telem::TelemetryData::Types::CURRENT |
406 : AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
407 : AP_Servo_Telem::TelemetryData::Types::PCB_TEMP
408 54320 : };
409 :
410 54320 : servo_telem->update_telem_data(i, telem_data);
411 : }
412 9057 : }
413 : #endif // AP_SERVO_TELEM_ENABLED
414 : }
415 :
416 : // Return the crc for a given command packet
417 91352 : uint16_t AP_Volz_Protocol::calculate_crc(const CMD &cmd) const
418 : {
419 91352 : uint16_t crc = 0xFFFF;
420 :
421 : // calculate Volz CRC value according to protocol definition
422 456760 : for(uint8_t i=0; i<4; i++) {
423 : // take input data into message that will be transmitted.
424 365408 : crc = (cmd.data[i] << 8) ^ crc;
425 :
426 3288672 : for(uint8_t j=0; j<8; j++) {
427 2923264 : if (crc & 0x8000) {
428 1518036 : crc = (crc << 1) ^ 0x8005;
429 : } else {
430 1405228 : crc = crc << 1;
431 : }
432 : }
433 : }
434 :
435 91352 : return crc;
436 : }
437 :
438 : // calculate CRC for volz serial protocol and send the data.
439 45677 : void AP_Volz_Protocol::send_command(CMD &cmd)
440 : {
441 45677 : const uint16_t crc = calculate_crc(cmd);
442 :
443 : // add CRC result to the message
444 45677 : cmd.crc1 = HIGHBYTE(crc);
445 45677 : cmd.crc2 = LOWBYTE(crc);
446 45677 : port->write(cmd.data, sizeof(cmd));
447 45677 : }
448 :
449 : #endif // AP_VOLZ_ENABLED
|
