PX4 Firmware
PX4 Autopilot Software http://px4.io
pga460.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file pga460.cpp
36  * @author Jacob Dahl <jacob.dahl@tealdrones.com>
37  *
38  * Driver for the TI PGA460 Ultrasonic Signal Processor and Transducer Driver
39  */
40 
41 #include "pga460.h"
42 
43 
44 PGA460::PGA460(const char *port)
45 {
46  // Store port name.
47  strncpy(_port, port, sizeof(_port) - 1);
48  // Enforce null termination.
49  _port[sizeof(_port) - 1] = '\0';
50 }
51 
53 {
55 }
56 
57 uint8_t PGA460::calc_checksum(uint8_t *data, const uint8_t size)
58 {
59  uint8_t checksum_input[size];
60 
61  for (size_t i = 0; i < size; i++) {
62  checksum_input[i] = *data;
63  data++;
64  }
65 
66  uint16_t carry = 0;
67 
68  for (uint8_t j = 0; j < size; j++) {
69  if ((checksum_input[j] + carry) < carry) {
70  carry = carry + checksum_input[j] + 1;
71 
72  } else {
73  carry = carry + checksum_input[j];
74  }
75 
76  if (carry > 0xFF) {
77  carry = carry - 255;
78  }
79  }
80 
81  carry = (~carry & 0x00FF);
82  return carry;
83 }
84 
86 {
87  int ret = px4_close(_fd);
88 
89  if (ret != 0) {
90  PX4_WARN("Could not close serial port");
91  }
92 
93  return ret;
94 }
95 
96 int PGA460::custom_command(int argc, char *argv[])
97 {
98  return print_usage("Unrecognized command.");
99 }
100 
101 PGA460 *PGA460::instantiate(int argc, char *argv[])
102 {
103  PGA460 *pga460 = new PGA460(PGA460_DEFAULT_PORT);
104  return pga460;
105 }
106 
108 {
109  if (initialize_thresholds() != PX4_OK) {
110  PX4_WARN("Thresholds not initialized");
111  return PX4_ERROR;
112  }
113 
114  px4_usleep(10000);
115 
116  // Read to see if eeprom saved data matches desired data, otherwise overwrite eeprom.
117  if (read_eeprom() != PX4_OK) {
118  write_eeprom();
119  // Allow sufficient time for the device to complete writing to registers.
120  px4_usleep(10000);
121  }
122 
123  // Verify the device is alive.
124  if (read_register(0x00) != USER_DATA1) {
125  return PX4_ERROR;
126  }
127 
128  return PX4_OK;
129 }
130 
132 {
133  const uint8_t array_size = 35;
134  uint8_t settings_buf[array_size] = {SYNCBYTE, BC_THRBW,
141  };
142 
143  uint8_t checksum = calc_checksum(&settings_buf[1], sizeof(settings_buf) - 2);
144  settings_buf[array_size - 1] = checksum;
145 
146  int ret = ::write(_fd, &settings_buf[0], sizeof(settings_buf));
147 
148  if (!ret) {
149  return PX4_ERROR;
150  }
151 
152  // Must wait >50us per datasheet.
153  px4_usleep(100);
154 
155  if (read_threshold_registers() == PX4_OK) {
156  return PX4_OK;
157 
158  } else {
160  return PX4_ERROR;
161  }
162 }
163 
165 {
166 
167  uint8_t buf_rx[6] = {};
168  int bytes_available = sizeof(buf_rx);
169  int total_bytes = 0;
170 
171  do {
172  int ret = ::read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
173 
174  total_bytes += ret;
175 
176  if (ret < 0) {
177  tcflush(_fd, TCIFLUSH);
178  PX4_ERR("read err3: %d", ret);
179  return ret;
180  }
181 
182  bytes_available -= ret;
183 
184  px4_usleep(1000);
185 
186  } while (bytes_available > 0);
187 
188 
189  uint16_t time_of_flight = (buf_rx[1] << 8) + buf_rx[2];
190  uint8_t Width = buf_rx[3];
191  uint8_t Amplitude = buf_rx[4];
192 
193  float object_distance = calculate_object_distance(time_of_flight);
194 
195  uORB_publish_results(object_distance);
196 
197  // B1,2: time_of_flight B3: Width B4: Amplitude
198  uint32_t results = (time_of_flight << 16) | (Width << 8) | (Amplitude << 0);
199 
200  return results;
201 }
202 
203 float PGA460::calculate_object_distance(uint16_t time_of_flight)
204 {
205  float temperature = get_temperature();
206 
207  // Default temperature if no temperature measurement can be obtained.
208  if (temperature > MAX_DETECTABLE_TEMPERATURE ||
209  temperature < MIN_DETECTABLE_TEMPERATURE) {
210  temperature = 20.0f;
211  }
212 
213  // Formula for the speed of sound over temperature: v = 331m/s + 0.6m/s/C * T
214  float speed_of_sound = 331.0f + 0.6f * temperature;
215  float millseconds_to_meters = 0.000001f;
216 
217  // Calculate the object distance in meters.
218  float object_distance = (float)time_of_flight * millseconds_to_meters * (speed_of_sound / 2.0f);
219 
220  return object_distance;
221 }
222 
224 {
225  // Send same unlock code with prog bit set to 1.
226  uint8_t eeprom_write_buf[5] = {SYNCBYTE, SRW, EE_CNTRL_ADDR, EE_UNLOCK_ST2, 0xFF};
227  uint8_t checksum = calc_checksum(&eeprom_write_buf[1], sizeof(eeprom_write_buf) - 2);
228  eeprom_write_buf[4] = checksum;
229  int ret = ::write(_fd, &eeprom_write_buf[0], sizeof(eeprom_write_buf));
230 
231  if (!ret) {
232  return PX4_ERROR;
233  }
234 
235  return PX4_OK;
236 }
237 
239 {
240  uint8_t buf_tx[4] = {SYNCBYTE, TNLM, 0x00, 0xFF};
241  uint8_t checksum = calc_checksum(&buf_tx[0], 3);
242  buf_tx[3] = checksum;
243 
244  int ret = ::write(_fd, &buf_tx[0], sizeof(buf_tx));
245 
246  if (!ret) {
247  return PX4_ERROR;
248  }
249 
250  // The pga460 requires a 2ms delay per the datasheet.
251  px4_usleep(5000);
252 
253  buf_tx[1] = TNLR;
254  ret = ::write(_fd, &buf_tx[0], sizeof(buf_tx) - 2);
255 
256  if (!ret) {
257  return PX4_ERROR;
258  }
259 
260  px4_usleep(10000);
261 
262  uint8_t buf_rx[4] = {};
263  int bytes_available = sizeof(buf_rx);
264  int total_bytes = 0;
265 
266  do {
267  ret = ::read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
268 
269  total_bytes += ret;
270 
271  if (ret < 0) {
272  tcflush(_fd, TCIFLUSH);
273  PX4_ERR("read err1: %d", ret);
274  return ret;
275  }
276 
277  bytes_available -= ret;
278 
279  px4_usleep(1000);
280 
281  } while (bytes_available > 0);
282 
283  // These constants and equations are from the pga460 datasheet, page 50.
284  float juntion_to_ambient_thermal_resistance = 96.1;
285  float v_power = 16.5;
286  float supply_current_listening = 0.012;
287  float temperature = ((buf_rx[1] - 64) / 1.5f) -
288  (juntion_to_ambient_thermal_resistance * supply_current_listening * v_power);
289 
290  return temperature;
291 }
292 
294 {
295  _fd = px4_open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
296 
297  if (_fd < 0) {
298  PX4_WARN("Failed to open serial port");
299  return PX4_ERROR;
300  }
301 
302  struct termios uart_config;
303 
304  int termios_state;
305 
306  // Fill the struct for the new configuration.
307  tcgetattr(_fd, &uart_config);
308 
309  // Input flags - Turn off input processing:
310  // convert break to null byte, no CR to NL translation,
311  // no NL to CR translation, don't mark parity errors or breaks
312  // no input parity check, don't strip high bit off,
313  // no XON/XOFF software flow control
314  uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | IGNCR | PARMRK | INPCK | ISTRIP | IXON | IXOFF);
315 
316  uart_config.c_iflag |= IGNPAR;
317 
318  // Output flags - Turn off output processing:
319  // no CR to NL translation, no NL to CR-NL translation,
320  // no NL to CR translation, no column 0 CR suppression,
321  // no Ctrl-D suppression, no fill characters, no case mapping,
322  // no local output processing
323  uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
324 
325  // No line processing:
326  // echo off, echo newline off, canonical mode off,
327  // extended input processing off, signal chars off
328  uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
329 
330  // No parity, one stop bit, disable flow control.
331  uart_config.c_cflag &= ~(CSIZE | PARENB | CSTOPB | CRTSCTS);
332 
333  uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);
334 
335  uart_config.c_cc[VMIN] = 1;
336 
337  uart_config.c_cc[VTIME] = 0;
338 
339  speed_t speed = 115200;
340 
341  // Set the baud rate.
342  if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
343  PX4_WARN("ERR CFG: %d ISPD", termios_state);
344  return PX4_ERROR;
345  }
346 
347  if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
348  PX4_WARN("ERR CFG: %d OSPD\n", termios_state);
349  return PX4_ERROR;
350  }
351 
352  if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
353  PX4_WARN("ERR baud %d ATTR", termios_state);
354  return PX4_ERROR;
355  }
356 
357  return _fd;
358 }
359 
361 {
362  uint8_t status_flags1 = read_register(0x4C);
363  uint8_t status_flags2 = read_register(0x4D);
364 
365  if ((status_flags1 & 0x0F) || status_flags2) {
366  if (status_flags1 & 0x0F & 1) {
367  PX4_INFO("Trim EEPROM space data CRC error");
368  }
369 
370  if (status_flags1 & 0x0F & 1 << 1) {
371  PX4_INFO("User EEPROM space data CRC error");
372  }
373 
374  if (status_flags1 & 0x0F & 1 << 2) {
375  PX4_INFO("Threshold map configuration register data CRC error");
376  }
377 
378  if (status_flags1 & 0x0F & 1 << 3) {
379  PX4_INFO("Wakeup Error");
380  }
381 
382  if (status_flags2 & 1) {
383  PX4_INFO("VPWR pin under voltage");
384  }
385 
386  if (status_flags2 & 1 << 1) {
387  PX4_INFO("VPWR pin over voltage");
388  }
389 
390  if (status_flags2 & 1 << 2) {
391  PX4_INFO("AVDD pin under voltage");
392  }
393 
394  if (status_flags2 & 1 << 3) {
395  PX4_INFO("AVDD pin over voltage");
396  }
397 
398  if (status_flags2 & 1 << 4) {
399  PX4_INFO("IOREG pin under voltage");
400  }
401 
402  if (status_flags2 & 1 << 5) {
403  PX4_INFO("IOREG pin over voltage");
404  }
405 
406  if (status_flags2 & 1 << 6) {
407  PX4_INFO("Thermal shutdown has occured");
408  }
409  }
410 }
411 
412 void PGA460::print_diagnostics(const uint8_t diagnostic_byte)
413 {
414  // Check the diagnostics bit field.
415  if (diagnostic_byte & 1 << 6) {
416  if (diagnostic_byte & 1 << 0) {
417  PX4_INFO("Device busy");
418  }
419 
420  if (diagnostic_byte & 1 << 1) {
421  PX4_INFO("Sync field bit rate too high/low");
422  }
423 
424  if (diagnostic_byte & 1 << 2) {
425  PX4_INFO("Consecutive sync bit fields do not match");
426  }
427 
428  if (diagnostic_byte & 1 << 3) {
429  PX4_INFO("Invalid checksum");
430  }
431 
432  if (diagnostic_byte & 1 << 4) {
433  PX4_INFO("Invalid command");
434  }
435 
436  if (diagnostic_byte & 1 << 5) {
437  PX4_INFO("General comm erorr");
438  }
439 
440  } else if (diagnostic_byte & 1 << 7) {
441  if (diagnostic_byte & 1 << 0) {
442  PX4_INFO("Device busy");
443  }
444 
445  if (diagnostic_byte & 1 << 1) {
446  PX4_INFO("Threshold settings CRC error");
447  }
448 
449  if (diagnostic_byte & 1 << 2) {
450  PX4_INFO("Frequency diagnostics error");
451  }
452 
453  if (diagnostic_byte & 1 << 3) {
454  PX4_INFO("Voltage diagnostics error");
455  }
456 
457  if (diagnostic_byte & 1 << 4) {
458  PX4_INFO("Always zero....");
459  }
460 
461  if (diagnostic_byte & 1 << 5) {
462  PX4_INFO("EEPROM CRC or TRIM CRC error");
463  }
464  }
465 }
466 
468 {
469  PX4_INFO("Distance: %2.2f", (double)_previous_valid_report_distance);
470  return PX4_OK;
471 }
472 
473 int PGA460::print_usage(const char *reason)
474 {
475  if (reason) {
476  PX4_WARN("%s\n", reason);
477  }
478 
479  PRINT_MODULE_DESCRIPTION(
480  R"DESCR_STR(
481 ### Description
482 Ultrasonic range finder driver that handles the communication with the device and publishes the distance via uORB.
483 
484 ### Implementation
485 This driver is implented as a NuttX task. This Implementation was chosen due to the need for polling on a message
486 via UART, which is not supported in the work_queue. This driver continuously takes range measurements while it is
487 running. A simple algorithm to detect false readings is implemented at the driver levelin an attemptto improve
488 the quality of data that is being published. The driver will not publish data at all if it deems the sensor data
489 to be invalid or unstable.
490 )DESCR_STR");
491 
492  PRINT_MODULE_USAGE_NAME("pga460", "driver");
493  PRINT_MODULE_USAGE_COMMAND("start <device_path>");
494  PRINT_MODULE_USAGE_ARG("device_path", "The pga460 sensor device path, (e.g: /dev/ttyS6", true);
495  PRINT_MODULE_USAGE_COMMAND("status");
496  PRINT_MODULE_USAGE_COMMAND("stop");
497  PRINT_MODULE_USAGE_COMMAND("help");
498 
499  return PX4_OK;
500 }
501 
503 {
504  unlock_eeprom();
505 
506  const int array_size = 43;
507  uint8_t user_settings[array_size] =
515 
516 
517  int ret = -1;
518  uint8_t cmd_buf[2] = {SYNCBYTE, EEBR};
519  uint8_t buf_rx[array_size + 2] = {};
520 
521  // The pga460 responds to this write() call by reporting current eeprom values.
522  ret = ::write(_fd, &cmd_buf[0], sizeof(cmd_buf));
523 
524  if (!ret) {
525  return PX4_ERROR;
526  }
527 
528  px4_usleep(10000);
529 
530  int bytes_available = sizeof(buf_rx);
531  int total_bytes = 0;
532 
533  do {
534  ret = ::read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
535 
536  total_bytes += ret;
537 
538  if(ret < 0) {
539  tcflush(_fd, TCIFLUSH);
540  PX4_ERR("read err2: %d", ret);
541  return ret;
542  }
543 
544  bytes_available -= ret;
545 
546  px4_usleep(1000);
547 
548  } while (bytes_available > 0);
549 
550  // Check the buffers to ensure they match.
551  int mismatched_bytes = memcmp(buf_rx + 1, user_settings, array_size);
552 
553  if (mismatched_bytes == 0) {
554  PX4_INFO("EEPROM has settings.");
555  return PX4_OK;
556  } else {
557  print_diagnostics(buf_rx[0]);
558  PX4_INFO("EEPROM does not have settings.");
559  return PX4_ERROR;
560  }
561 }
562 
563 uint8_t PGA460::read_register(const uint8_t reg)
564 {
565  // must unlock the eeprom registers before you can read or write to them
566  if (reg < 0x40) {
567  unlock_eeprom();
568  }
569 
570  uint8_t buf_tx[4] = {SYNCBYTE, SRR, reg, 0xFF};
571  uint8_t checksum = calc_checksum(&buf_tx[1], 2);
572  buf_tx[3] = checksum;
573 
574  int ret = ::write(_fd, &buf_tx[0], sizeof(buf_tx));
575 
576  if(!ret) {
577  return PX4_ERROR;
578  }
579 
580  px4_usleep(10000);
581 
582  uint8_t buf_rx[3] = {};
583  int bytes_available = sizeof(buf_rx);
584  int total_bytes = 0;
585 
586  do {
587  ret = ::read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
588 
589  total_bytes += ret;
590 
591  if(ret < 0) {
592  tcflush(_fd, TCIFLUSH);
593  PX4_ERR("read err3: %d", ret);
594  return ret;
595  }
596 
597  bytes_available -= ret;
598 
599  px4_usleep(1000);
600 
601  } while (bytes_available > 0);
602 
603  // Prints errors if there are any.
604  print_diagnostics(buf_rx[0]);
605 
606  return buf_rx[1];
607 }
608 
610 {
611  const int array_size = 32;
612  uint8_t user_settings[array_size] = {P1_THR_0, P1_THR_1, P1_THR_2, P1_THR_3, P1_THR_4,
618  };
619 
620  uint8_t buf_tx[2] = {SYNCBYTE, THRBR};
621 
622  int ret = ::write(_fd, &buf_tx[0], sizeof(buf_tx));
623 
624  if(!ret) {
625  return PX4_ERROR;
626  }
627 
628  px4_usleep(10000);
629 
630  uint8_t buf_rx[array_size + 2] = {};
631  int bytes_available = sizeof(buf_rx);
632  int total_bytes = 0;
633 
634  do {
635  ret = ::read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
636 
637  total_bytes += ret;
638 
639  if(ret < 0) {
640  tcflush(_fd, TCIFLUSH);
641  PX4_ERR("read err3: %d", ret);
642  return ret;
643  }
644 
645  bytes_available -= ret;
646 
647  px4_usleep(1000);
648 
649  } while (bytes_available > 0);
650 
651  // Check to ensure the buffers match.
652  int mismatch = memcmp(buf_rx + 1, user_settings, sizeof(buf_rx) - 2);
653 
654  if (mismatch == 0) {
655  PX4_INFO("Threshold registers have program settings");
656  return PX4_OK;
657 
658  } else {
659  PX4_WARN("Threshold registers do not have program settings");
660  print_diagnostics(buf_rx[0]);
661  return PX4_ERROR;
662  }
663 }
664 
666 {
667  uint8_t buf_tx[2] = {SYNCBYTE, UMR};
668  int ret = ::write(_fd, &buf_tx[0], sizeof(buf_tx));
669  px4_usleep(10000);
670 
671  if(!ret) {
672  return PX4_ERROR;
673  }
674 
675  return PX4_OK;
676 }
677 
679 {
680  open_serial();
681  int ret = initialize_device_settings();
682 
683  if(ret != PX4_OK) {
684  close_serial();
685  PX4_INFO("Could not initialize device settings. Exiting.");
686  return;
687  }
688 
689  struct distance_sensor_s report = {};
690  _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &report);
691 
692  if (_distance_sensor_topic == nullptr) {
693  PX4_WARN("Advertise failed.");
694  return;
695  }
696 
698 
699  while (!should_exit()) {
700  // Check last report to determine if we need to switch range modes.
701  uint8_t mode = set_range_mode();
702  take_measurement(mode);
703 
704  // Control rate.
705  uint64_t loop_time = hrt_absolute_time() - _start_loop;
706  uint32_t sleep_time = (loop_time > POLL_RATE_US) ? 0 : POLL_RATE_US - loop_time;
707  px4_usleep(sleep_time);
708 
710  request_results();
711  collect_results();
712  }
713 
714  PX4_INFO("Exiting.");
715  close_serial();
716 }
717 
719 {
720  // Set the ASICs settings depening on the distance read from our last report.
723 
726 
727  }
728 
729  return _ranging_mode;
730 }
731 
732 int PGA460::take_measurement(const uint8_t mode)
733 {
734  // Issue a measurement command to detect one object using Preset 1 Burst/Listen.
735  uint8_t buf_tx[4] = {SYNCBYTE, mode, 0x01, 0xFF};
736  uint8_t checksum = calc_checksum(&buf_tx[1], 2);
737  buf_tx[3] = checksum;
738 
739  int ret = ::write(_fd, &buf_tx[0], sizeof(buf_tx));
740 
741  if(!ret) {
742  return PX4_ERROR;
743  }
744 
745  return PX4_OK;
746 }
747 
748 int PGA460::task_spawn(int argc, char *argv[])
749 {
750  px4_main_t entry_point = (px4_main_t)&run_trampoline;
751  int stack_size = 1256;
752 
753  int task_id = px4_task_spawn_cmd("pga460", SCHED_DEFAULT,
754  SCHED_PRIORITY_SLOW_DRIVER, stack_size,
755  entry_point, (char *const *)argv);
756 
757  if (task_id < 0) {
758  task_id = -1;
759  return -errno;
760  }
761 
762  _task_id = task_id;
763 
764  return PX4_OK;
765 }
766 
767 void PGA460::uORB_publish_results(const float object_distance)
768 {
769  struct distance_sensor_s report = {};
770  report.timestamp = hrt_absolute_time();
771  report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
772  report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
773  report.current_distance = object_distance;
776  report.signal_quality = 0;
777 
778  bool data_is_valid = false;
779  static uint8_t good_data_counter = 0;
780 
781  // If we are within our MIN and MAX thresholds, continue.
782  if (object_distance > MIN_DETECTABLE_DISTANCE && object_distance < MAX_DETECTABLE_DISTANCE) {
783 
784  // Height cannot change by more than MAX_SAMPLE_DEVIATION between measurements.
785  bool sample_deviation_valid = (report.current_distance < _previous_valid_report_distance + MAX_SAMPLE_DEVIATION)
787 
788  // Must have NUM_SAMPLES_CONSISTENT valid samples to be publishing.
789  if (sample_deviation_valid) {
790  good_data_counter++;
791 
792  if (good_data_counter > NUM_SAMPLES_CONSISTENT - 1) {
793  good_data_counter = NUM_SAMPLES_CONSISTENT;
794  data_is_valid = true;
795 
796  } else {
797  // Have not gotten NUM_SAMPLES_CONSISTENT consistently valid samples.
798  data_is_valid = false;
799  }
800 
801  } else if (good_data_counter > 0) {
802  good_data_counter--;
803 
804  } else {
805  // Reset our quality of data estimate after NUM_SAMPLES_CONSISTENT invalid samples.
807  }
808 
810  }
811 
812  if (data_is_valid) {
813  report.signal_quality = 1;
815  orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
816  }
817 }
818 
820 {
821  // Two step EEPROM unlock -- send unlock code w/ prog bit set to 0.
822  // This might actually be wrapped into command 11 (ee bulk write) but I am not sure.
823  uint8_t eeprom_write_buf[5] = {SYNCBYTE, SRW, EE_CNTRL_ADDR, EE_UNLOCK_ST1, 0xFF};
824  uint8_t checksum = calc_checksum(&eeprom_write_buf[1], sizeof(eeprom_write_buf) - 2);
825  eeprom_write_buf[4] = checksum;
826  int ret = ::write(_fd, &eeprom_write_buf[0], sizeof(eeprom_write_buf));
827 
828  if(!ret) {
829  return PX4_ERROR;
830  }
831 
832  return PX4_OK;
833 }
834 
836 {
837  uint8_t settings_buf[46] = {SYNCBYTE, EEBW, USER_DATA1, USER_DATA2, USER_DATA3, USER_DATA4,
844  };
845 
846  uint8_t checksum = calc_checksum(&settings_buf[1], sizeof(settings_buf) - 2);
847  settings_buf[45] = checksum;
848 
849  int ret = ::write(_fd, &settings_buf[0], sizeof(settings_buf));
850 
851  if(!ret) {
852  return PX4_ERROR;
853  }
854 
855  // Needs time, see datasheet timing requirements.
856  px4_usleep(5000);
857  unlock_eeprom();
858  flash_eeprom();
859  px4_usleep(5000);
860 
861  uint8_t result = 0;
862 
863  // Give up to 100ms for ee_cntrl register to reflect a successful eeprom write.
864  for (int i = 0; i < 100; i++) {
865  result = read_register(EE_CNTRL_ADDR);
866  px4_usleep(5000);
867 
868  if (result & 1 << 2) {
869  PX4_INFO("EEPROM write successful");
870  return PX4_OK;
871  }
872  }
873 
874  PX4_WARN("Failed to write to EEPROM");
875  print_diagnostics(result);
876  return PX4_ERROR;
877 }
878 
879 int PGA460::write_register(const uint8_t reg, const uint8_t val)
880 {
881  // Must unlock the eeprom registers before you can read or write to them.
882  if (reg < 0x40) {
883  unlock_eeprom();
884  }
885 
886  uint8_t buf_tx[5] = {SYNCBYTE, SRW, reg, val, 0xFF};
887  uint8_t checksum = calc_checksum(&buf_tx[1], sizeof(buf_tx) - 2);
888  buf_tx[4] = checksum;
889 
890  uint8_t ret = ::write(_fd, &buf_tx[0], sizeof(buf_tx));
891 
892  if (ret != sizeof(buf_tx)) {
893  return PX4_OK;
894  } else {
895  return PX4_ERROR;
896  }
897 }
898 
899 extern "C" __EXPORT int pga460_main(int argc, char *argv[])
900 {
901  return PGA460::main(argc, argv);
902 }
float _previous_report_distance
Definition: pga460.h:397
char _port[20]
Definition: pga460.h:394
float calculate_object_distance(uint16_t time_of_flight)
Calculates the distance from the measurement time of flight (time_of_flight) and current temperature...
Definition: pga460.cpp:203
#define USER_DATA5
Definition: pga460.h:116
#define EE_CNTRL_ADDR
Definition: pga460.h:107
#define P1_THR_13
Definition: pga460.h:189
#define NUM_SAMPLES_CONSISTENT
Definition: pga460.h:65
static PGA460 * instantiate(int argc, char *argv[])
Instantiates the pga460 object.
Definition: pga460.cpp:101
#define P1_THR_3
Definition: pga460.h:179
#define DEADTIME
Definition: pga460.h:141
#define P1_THR_6
Definition: pga460.h:182
static int task_spawn(int argc, char *argv[])
Definition: pga460.cpp:748
#define POLL_RATE_US
Definition: pga460.h:67
#define TVGAIN0
Definition: pga460.h:132
__EXPORT int pga460_main(int argc, char *argv[])
Definition: pga460.cpp:899
#define USER_DATA8
Definition: pga460.h:119
int read_threshold_registers()
Reads the threshold registers.
Definition: pga460.cpp:609
#define TVGAIN3
Definition: pga460.h:135
int close_serial()
Closes the serial port.
Definition: pga460.cpp:85
#define MODE_SET_HYST
Definition: pga460.h:63
void uORB_publish_results(const float dist)
Commands the device to publish the measurement results to uORB.
Definition: pga460.cpp:767
#define P2_THR_1
Definition: pga460.h:193
#define USER_DATA10
Definition: pga460.h:121
int main(int argc, char **argv)
Definition: main.cpp:3
int write_register(const uint8_t reg, const uint8_t val)
Writes a value to a register.
Definition: pga460.cpp:879
#define P1_THR_9
Definition: pga460.h:185
#define USER_DATA14
Definition: pga460.h:125
orb_advert_t _distance_sensor_topic
orb_advert_t uORB advertisement topic.
Definition: pga460.h:388
#define P1_THR_1
Definition: pga460.h:177
Definition: I2C.hpp:51
uint32_t collect_results()
Collects the data in the serial port rx buffer, does math, and publishes the value to uORB...
Definition: pga460.cpp:164
#define P2_THR_14
Definition: pga460.h:206
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
#define USER_DATA18
Definition: pga460.h:129
#define P1_THR_2
Definition: pga460.h:178
#define MODE_LONG_RANGE
Definition: pga460.h:70
#define TVGAIN5
Definition: pga460.h:137
#define UMR
Definition: pga460.h:82
#define MIN_DETECTABLE_DISTANCE
Definition: pga460.h:59
PGA460(const char *port=PGA460_DEFAULT_PORT)
Definition: pga460.cpp:44
int flash_eeprom()
Send the program command to the EEPROM to start the flash process.
Definition: pga460.cpp:223
#define MODE_SHORT_RANGE
Definition: pga460.h:69
#define MAX_DETECTABLE_DISTANCE
Definition: pga460.h:58
#define BC_THRBW
Definition: pga460.h:104
#define EE_UNLOCK_ST2
Definition: pga460.h:109
int read_eeprom()
Reads the EEPROM and checks to see if it matches the default parameters.
Definition: pga460.cpp:502
void print_device_status()
Reports the diagnostic data from device status registers 1 and 2 if there is anything to report...
Definition: pga460.cpp:360
#define USER_DATA13
Definition: pga460.h:124
uint8_t _ranging_mode
Definition: pga460.h:406
int initialize_device_settings()
Writes program defined threshold defaults to the register map and checks/writes the EEPROM...
Definition: pga460.cpp:107
#define P2_THR_6
Definition: pga460.h:198
#define USER_DATA3
Definition: pga460.h:114
#define P1_THR_10
Definition: pga460.h:186
#define CURR_LIM_P1
Definition: pga460.h:144
#define P1_THR_11
Definition: pga460.h:187
uint64_t _start_loop
Definition: pga460.h:409
int request_results()
Measurement is read from UART RX buffer and published to the uORB distance sensor topic...
Definition: pga460.cpp:665
#define P2_THR_15
Definition: pga460.h:207
#define USER_DATA6
Definition: pga460.h:117
static int custom_command(int argc, char *argv[])
main Main entry point to the module that should be called directly from the module&#39;s main method...
Definition: pga460.cpp:96
#define EEBR
Definition: pga460.h:88
#define P1_THR_12
Definition: pga460.h:188
#define FVOLT_DEC
Definition: pga460.h:149
static void read(bootloader_app_shared_t *pshared)
#define SRR
Definition: pga460.h:86
#define PULSE_P2
Definition: pga460.h:143
#define P2_THR_8
Definition: pga460.h:200
#define P2_THR_9
Definition: pga460.h:201
#define P1_THR_7
Definition: pga460.h:183
int initialize_thresholds()
Writes the user defined paramaters to device register map.
Definition: pga460.cpp:131
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
#define EE_UNLOCK_ST1
Definition: pga460.h:108
int open_serial()
Opens the serial port.
Definition: pga460.cpp:293
#define USER_DATA15
Definition: pga460.h:126
#define P2_THR_12
Definition: pga460.h:204
uint8_t set_range_mode()
Checks the measurement from last report and sets the range distance mode (long range ...
Definition: pga460.cpp:718
#define P1_THR_4
Definition: pga460.h:180
int unlock_eeprom()
Send the unlock command to the EEPROM to enable reading and writing – not needed w/ bulk write...
Definition: pga460.cpp:819
#define P1_GAIN_CTRL
Definition: pga460.h:153
Definition: pga460.h:210
uint8_t * data
Definition: dataman.cpp:149
static int print_usage(const char *reason=nullptr)
Prints the module usage to the nuttshell console.
Definition: pga460.cpp:473
uint8_t calc_checksum(uint8_t *data, const uint8_t size)
Calculates the checksum of the transmitted commmand + data.
Definition: pga460.cpp:57
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define TVGAIN4
Definition: pga460.h:136
#define MODE_SET_THRESH
Definition: pga460.h:62
#define PULSE_P1
Definition: pga460.h:142
#define MAX_SAMPLE_DEVIATION
Definition: pga460.h:64
int print_status() override
Diagnostics - print some basic information about the driver.
Definition: pga460.cpp:467
#define P1_THR_0
Definition: pga460.h:176
int take_measurement(const uint8_t mode)
Commands the device to perform an ultrasonic measurement.
Definition: pga460.cpp:732
virtual ~PGA460()
Definition: pga460.cpp:52
#define P2_THR_4
Definition: pga460.h:196
#define MAX_DETECTABLE_TEMPERATURE
Definition: pga460.h:60
#define USER_DATA7
Definition: pga460.h:118
#define P2_THR_5
Definition: pga460.h:197
#define USER_DATA4
Definition: pga460.h:115
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
#define USER_DATA19
Definition: pga460.h:130
#define USER_DATA16
Definition: pga460.h:127
static void write(bootloader_app_shared_t *pshared)
#define TVGAIN6
Definition: pga460.h:138
#define TVGAIN2
Definition: pga460.h:134
int _fd
Definition: pga460.h:391
int px4_open(const char *path, int flags,...)
#define USER_DATA11
Definition: pga460.h:122
#define USER_DATA2
Definition: pga460.h:113
#define MIN_DETECTABLE_TEMPERATURE
Definition: pga460.h:61
uint8_t read_register(const uint8_t reg)
Reads a register.
Definition: pga460.cpp:563
#define INIT_GAIN
Definition: pga460.h:139
#define CURR_LIM_P2
Definition: pga460.h:145
#define USER_DATA20
Definition: pga460.h:131
#define P2_THR_7
Definition: pga460.h:199
#define DECPL_TEMP
Definition: pga460.h:150
#define P2_THR_3
Definition: pga460.h:195
#define P1_THR_5
Definition: pga460.h:181
#define P1_THR_15
Definition: pga460.h:191
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
void print_diagnostics(const uint8_t diagnostic_byte)
Reports the diagnostic data the diagnostic byte (first byte from slave).
Definition: pga460.cpp:412
#define TVGAIN1
Definition: pga460.h:133
#define P2_THR_11
Definition: pga460.h:203
#define PGA460_DEFAULT_PORT
Definition: pga460.h:57
#define P2_THR_13
Definition: pga460.h:205
#define FREQUENCY
Definition: pga460.h:140
#define REC_LENGTH
Definition: pga460.h:146
#define SYNCBYTE
Definition: pga460.h:72
#define P1_THR_8
Definition: pga460.h:184
#define SAT_FDIAG_TH
Definition: pga460.h:148
#define P1_THR_14
Definition: pga460.h:190
#define P2_THR_2
Definition: pga460.h:194
#define EEBW
Definition: pga460.h:89
#define USER_DATA12
Definition: pga460.h:123
#define P2_THR_10
Definition: pga460.h:202
float _previous_valid_report_distance
Definition: pga460.h:400
int write_eeprom()
Writes the user defined paramaters to device EEPROM.
Definition: pga460.cpp:835
#define TNLR
Definition: pga460.h:83
#define SRW
Definition: pga460.h:87
#define USER_DATA9
Definition: pga460.h:120
mode
Definition: vtol_type.h:76
#define TEMP_TRIM
Definition: pga460.h:152
#define DSP_SCALE
Definition: pga460.h:151
float get_temperature()
Definition: pga460.cpp:238
#define USER_DATA17
Definition: pga460.h:128
#define THRBR
Definition: pga460.h:92
void run() override
Definition: pga460.cpp:678
#define P2_THR_0
Definition: pga460.h:192
#define P2_GAIN_CTRL
Definition: pga460.h:154
#define TNLM
Definition: pga460.h:81
#define USER_DATA1
Definition: pga460.h:112
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
#define FREQ_DIAG
Definition: pga460.h:147
int px4_close(int fd)