59 uint8_t checksum_input[size];
61 for (
size_t i = 0; i < size; i++) {
62 checksum_input[i] = *
data;
68 for (uint8_t j = 0; j < size; j++) {
69 if ((checksum_input[j] + carry) < carry) {
70 carry = carry + checksum_input[j] + 1;
73 carry = carry + checksum_input[j];
81 carry = (~carry & 0x00FF);
90 PX4_WARN(
"Could not close serial port");
110 PX4_WARN(
"Thresholds not initialized");
133 const uint8_t array_size = 35;
143 uint8_t checksum =
calc_checksum(&settings_buf[1],
sizeof(settings_buf) - 2);
144 settings_buf[array_size - 1] = checksum;
146 int ret =
::write(
_fd, &settings_buf[0],
sizeof(settings_buf));
167 uint8_t buf_rx[6] = {};
168 int bytes_available =
sizeof(buf_rx);
172 int ret =
::read(
_fd, buf_rx + total_bytes,
sizeof(buf_rx));
177 tcflush(
_fd, TCIFLUSH);
178 PX4_ERR(
"read err3: %d", ret);
182 bytes_available -= ret;
186 }
while (bytes_available > 0);
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];
198 uint32_t results = (time_of_flight << 16) | (Width << 8) | (Amplitude << 0);
214 float speed_of_sound = 331.0f + 0.6f * temperature;
215 float millseconds_to_meters = 0.000001f;
218 float object_distance = (float)time_of_flight * millseconds_to_meters * (speed_of_sound / 2.0
f);
220 return object_distance;
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));
242 buf_tx[3] = checksum;
244 int ret =
::write(
_fd, &buf_tx[0],
sizeof(buf_tx));
254 ret =
::write(
_fd, &buf_tx[0],
sizeof(buf_tx) - 2);
262 uint8_t buf_rx[4] = {};
263 int bytes_available =
sizeof(buf_rx);
267 ret =
::read(
_fd, buf_rx + total_bytes,
sizeof(buf_rx));
272 tcflush(
_fd, TCIFLUSH);
273 PX4_ERR(
"read err1: %d", ret);
277 bytes_available -= ret;
281 }
while (bytes_available > 0);
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.5
f) -
288 (juntion_to_ambient_thermal_resistance * supply_current_listening * v_power);
298 PX4_WARN(
"Failed to open serial port");
302 struct termios uart_config;
307 tcgetattr(
_fd, &uart_config);
314 uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | IGNCR | PARMRK | INPCK | ISTRIP | IXON | IXOFF);
316 uart_config.c_iflag |= IGNPAR;
323 uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
328 uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
331 uart_config.c_cflag &= ~(CSIZE | PARENB | CSTOPB | CRTSCTS);
333 uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);
335 uart_config.c_cc[VMIN] = 1;
337 uart_config.c_cc[VTIME] = 0;
339 speed_t speed = 115200;
342 if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
343 PX4_WARN(
"ERR CFG: %d ISPD", termios_state);
347 if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
348 PX4_WARN(
"ERR CFG: %d OSPD\n", termios_state);
352 if ((termios_state = tcsetattr(
_fd, TCSANOW, &uart_config)) < 0) {
353 PX4_WARN(
"ERR baud %d ATTR", termios_state);
365 if ((status_flags1 & 0x0F) || status_flags2) {
366 if (status_flags1 & 0x0F & 1) {
367 PX4_INFO(
"Trim EEPROM space data CRC error");
370 if (status_flags1 & 0x0F & 1 << 1) {
371 PX4_INFO(
"User EEPROM space data CRC error");
374 if (status_flags1 & 0x0F & 1 << 2) {
375 PX4_INFO(
"Threshold map configuration register data CRC error");
378 if (status_flags1 & 0x0F & 1 << 3) {
379 PX4_INFO(
"Wakeup Error");
382 if (status_flags2 & 1) {
383 PX4_INFO(
"VPWR pin under voltage");
386 if (status_flags2 & 1 << 1) {
387 PX4_INFO(
"VPWR pin over voltage");
390 if (status_flags2 & 1 << 2) {
391 PX4_INFO(
"AVDD pin under voltage");
394 if (status_flags2 & 1 << 3) {
395 PX4_INFO(
"AVDD pin over voltage");
398 if (status_flags2 & 1 << 4) {
399 PX4_INFO(
"IOREG pin under voltage");
402 if (status_flags2 & 1 << 5) {
403 PX4_INFO(
"IOREG pin over voltage");
406 if (status_flags2 & 1 << 6) {
407 PX4_INFO(
"Thermal shutdown has occured");
415 if (diagnostic_byte & 1 << 6) {
416 if (diagnostic_byte & 1 << 0) {
417 PX4_INFO(
"Device busy");
420 if (diagnostic_byte & 1 << 1) {
421 PX4_INFO(
"Sync field bit rate too high/low");
424 if (diagnostic_byte & 1 << 2) {
425 PX4_INFO(
"Consecutive sync bit fields do not match");
428 if (diagnostic_byte & 1 << 3) {
429 PX4_INFO(
"Invalid checksum");
432 if (diagnostic_byte & 1 << 4) {
433 PX4_INFO(
"Invalid command");
436 if (diagnostic_byte & 1 << 5) {
437 PX4_INFO(
"General comm erorr");
440 }
else if (diagnostic_byte & 1 << 7) {
441 if (diagnostic_byte & 1 << 0) {
442 PX4_INFO(
"Device busy");
445 if (diagnostic_byte & 1 << 1) {
446 PX4_INFO(
"Threshold settings CRC error");
449 if (diagnostic_byte & 1 << 2) {
450 PX4_INFO(
"Frequency diagnostics error");
453 if (diagnostic_byte & 1 << 3) {
454 PX4_INFO(
"Voltage diagnostics error");
457 if (diagnostic_byte & 1 << 4) {
458 PX4_INFO(
"Always zero....");
461 if (diagnostic_byte & 1 << 5) {
462 PX4_INFO(
"EEPROM CRC or TRIM CRC error");
476 PX4_WARN(
"%s\n", reason);
479 PRINT_MODULE_DESCRIPTION(
482 Ultrasonic range finder driver that handles the communication with the device and publishes the distance via uORB. 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. 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");
506 const int array_size = 43;
507 uint8_t user_settings[array_size] =
512 TVGAIN0,
TVGAIN1,
TVGAIN2,
TVGAIN3,
TVGAIN4,
TVGAIN5,
TVGAIN6,
INIT_GAIN,
FREQUENCY,
DEADTIME,
519 uint8_t buf_rx[array_size + 2] = {};
522 ret =
::write(
_fd, &cmd_buf[0],
sizeof(cmd_buf));
530 int bytes_available =
sizeof(buf_rx);
534 ret =
::read(
_fd, buf_rx + total_bytes,
sizeof(buf_rx));
539 tcflush(
_fd, TCIFLUSH);
540 PX4_ERR(
"read err2: %d", ret);
544 bytes_available -= ret;
548 }
while (bytes_available > 0);
551 int mismatched_bytes = memcmp(buf_rx + 1, user_settings, array_size);
553 if (mismatched_bytes == 0) {
554 PX4_INFO(
"EEPROM has settings.");
558 PX4_INFO(
"EEPROM does not have settings.");
572 buf_tx[3] = checksum;
574 int ret =
::write(
_fd, &buf_tx[0],
sizeof(buf_tx));
582 uint8_t buf_rx[3] = {};
583 int bytes_available =
sizeof(buf_rx);
587 ret =
::read(
_fd, buf_rx + total_bytes,
sizeof(buf_rx));
592 tcflush(
_fd, TCIFLUSH);
593 PX4_ERR(
"read err3: %d", ret);
597 bytes_available -= ret;
601 }
while (bytes_available > 0);
611 const int array_size = 32;
622 int ret =
::write(
_fd, &buf_tx[0],
sizeof(buf_tx));
630 uint8_t buf_rx[array_size + 2] = {};
631 int bytes_available =
sizeof(buf_rx);
635 ret =
::read(
_fd, buf_rx + total_bytes,
sizeof(buf_rx));
640 tcflush(
_fd, TCIFLUSH);
641 PX4_ERR(
"read err3: %d", ret);
645 bytes_available -= ret;
649 }
while (bytes_available > 0);
652 int mismatch = memcmp(buf_rx + 1, user_settings,
sizeof(buf_rx) - 2);
655 PX4_INFO(
"Threshold registers have program settings");
659 PX4_WARN(
"Threshold registers do not have program settings");
668 int ret =
::write(
_fd, &buf_tx[0],
sizeof(buf_tx));
685 PX4_INFO(
"Could not initialize device settings. Exiting.");
693 PX4_WARN(
"Advertise failed.");
699 while (!should_exit()) {
707 px4_usleep(sleep_time);
714 PX4_INFO(
"Exiting.");
737 buf_tx[3] = checksum;
739 int ret =
::write(
_fd, &buf_tx[0],
sizeof(buf_tx));
750 px4_main_t entry_point = (px4_main_t)&run_trampoline;
751 int stack_size = 1256;
753 int task_id = px4_task_spawn_cmd(
"pga460", SCHED_DEFAULT,
754 SCHED_PRIORITY_SLOW_DRIVER, stack_size,
755 entry_point, (
char *
const *)argv);
771 report.
type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
772 report.
orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
778 bool data_is_valid =
false;
779 static uint8_t good_data_counter = 0;
789 if (sample_deviation_valid) {
794 data_is_valid =
true;
798 data_is_valid =
false;
801 }
else if (good_data_counter > 0) {
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));
841 TVGAIN0,
TVGAIN1,
TVGAIN2,
TVGAIN3,
TVGAIN4,
TVGAIN5,
TVGAIN6,
INIT_GAIN,
FREQUENCY,
DEADTIME,
846 uint8_t checksum =
calc_checksum(&settings_buf[1],
sizeof(settings_buf) - 2);
847 settings_buf[45] = checksum;
849 int ret =
::write(
_fd, &settings_buf[0],
sizeof(settings_buf));
864 for (
int i = 0; i < 100; i++) {
868 if (result & 1 << 2) {
869 PX4_INFO(
"EEPROM write successful");
874 PX4_WARN(
"Failed to write to EEPROM");
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;
890 uint8_t ret =
::write(
_fd, &buf_tx[0],
sizeof(buf_tx));
892 if (ret !=
sizeof(buf_tx)) {
float _previous_report_distance
float calculate_object_distance(uint16_t time_of_flight)
Calculates the distance from the measurement time of flight (time_of_flight) and current temperature...
#define NUM_SAMPLES_CONSISTENT
static PGA460 * instantiate(int argc, char *argv[])
Instantiates the pga460 object.
static int task_spawn(int argc, char *argv[])
__EXPORT int pga460_main(int argc, char *argv[])
int read_threshold_registers()
Reads the threshold registers.
int close_serial()
Closes the serial port.
void uORB_publish_results(const float dist)
Commands the device to publish the measurement results to uORB.
int main(int argc, char **argv)
int write_register(const uint8_t reg, const uint8_t val)
Writes a value to a register.
orb_advert_t _distance_sensor_topic
orb_advert_t uORB advertisement topic.
uint32_t collect_results()
Collects the data in the serial port rx buffer, does math, and publishes the value to uORB...
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
#define MIN_DETECTABLE_DISTANCE
PGA460(const char *port=PGA460_DEFAULT_PORT)
int flash_eeprom()
Send the program command to the EEPROM to start the flash process.
#define MAX_DETECTABLE_DISTANCE
int read_eeprom()
Reads the EEPROM and checks to see if it matches the default parameters.
void print_device_status()
Reports the diagnostic data from device status registers 1 and 2 if there is anything to report...
int initialize_device_settings()
Writes program defined threshold defaults to the register map and checks/writes the EEPROM...
int request_results()
Measurement is read from UART RX buffer and published to the uORB distance sensor topic...
static int custom_command(int argc, char *argv[])
main Main entry point to the module that should be called directly from the module's main method...
static void read(bootloader_app_shared_t *pshared)
int initialize_thresholds()
Writes the user defined paramaters to device register map.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
int open_serial()
Opens the serial port.
uint8_t set_range_mode()
Checks the measurement from last report and sets the range distance mode (long range ...
int unlock_eeprom()
Send the unlock command to the EEPROM to enable reading and writing – not needed w/ bulk write...
static int print_usage(const char *reason=nullptr)
Prints the module usage to the nuttshell console.
uint8_t calc_checksum(uint8_t *data, const uint8_t size)
Calculates the checksum of the transmitted commmand + data.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
#define MAX_SAMPLE_DEVIATION
int print_status() override
Diagnostics - print some basic information about the driver.
int take_measurement(const uint8_t mode)
Commands the device to perform an ultrasonic measurement.
#define MAX_DETECTABLE_TEMPERATURE
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
static void write(bootloader_app_shared_t *pshared)
#define MIN_DETECTABLE_TEMPERATURE
uint8_t read_register(const uint8_t reg)
Reads a register.
int orb_unadvertise(orb_advert_t handle)
void print_diagnostics(const uint8_t diagnostic_byte)
Reports the diagnostic data the diagnostic byte (first byte from slave).
#define PGA460_DEFAULT_PORT
float _previous_valid_report_distance
int write_eeprom()
Writes the user defined paramaters to device EEPROM.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).