52 #include <px4_platform_common/module.h> 53 #include <px4_platform_common/module_params.h> 54 #include <px4_platform_common/tasks.h> 57 #define PGA460_DEFAULT_PORT "/dev/ttyS6" 58 #define MAX_DETECTABLE_DISTANCE 3.0f 59 #define MIN_DETECTABLE_DISTANCE 0.05f 60 #define MAX_DETECTABLE_TEMPERATURE 100.0f 61 #define MIN_DETECTABLE_TEMPERATURE -20.0f 62 #define MODE_SET_THRESH 0.6f 63 #define MODE_SET_HYST 0.0f 64 #define MAX_SAMPLE_DEVIATION 0.15f 65 #define NUM_SAMPLES_CONSISTENT 5 67 #define POLL_RATE_US 0ULL 69 #define MODE_SHORT_RANGE P1BL 70 #define MODE_LONG_RANGE P2BL 77 #define P1BL 0x00 // Burst and Listen (Preset 1) 78 #define P2BL 0x01 // Burst and Listen (Preset 2) 79 #define P1LO 0x02 // Listen Only (Preset 1) 80 #define P2LO 0x03 // Listen Only (Preset 2) 81 #define TNLM 0x04 // Temperature and Noise-level measurement 82 #define UMR 0x05 // Ultrasonic Measurement Result 83 #define TNLR 0x06 // Temperature and noise level result 84 #define TEDD 0x07 // Transducer echo data dump 85 #define SD 0x08 // System diagnostics 86 #define SRR 0x09 // Register read 87 #define SRW 0x0A // Register write 88 #define EEBR 0x0B // EEPROM bulk read 89 #define EEBW 0x0C // EEPROM bulk write 90 #define TVGBR 0x0D // Time-varying-gain bulk read 91 #define TVGBW 0x0E // Time-varying-gain bulk write 92 #define THRBR 0x0F // Threshold bulk read 93 #define THRBW 0x10 // Threshold bulk write 96 #define BC_P1BL 0x11 // Burst and listen (Preset 1) 97 #define BC_P2BL 0x12 // Burst and listen (Preset 2) 98 #define BC_P1LO 0x13 // Listen only (Preset 1) 99 #define BC_P2LO 0x14 // Listen only (Preset 2) 100 #define BC_TNLM 0x15 // Temperature and noise-level measurement 101 #define BC_SRW 0x16 // Register write 102 #define BC_EEBW 0x17 // EEPROM bulk write 103 #define BC_TVGBW 0x18 // Time varying-gain bulk write 104 #define BC_THRBW 0x19 // Threshold bulk write 107 #define EE_CNTRL_ADDR 0x40 108 #define EE_UNLOCK_ST1 0x68 109 #define EE_UNLOCK_ST2 0x69 112 #define USER_DATA1 0xAA //reg addr 0x0 113 #define USER_DATA2 0x0 //reg addr 0x1 114 #define USER_DATA3 0x0 //reg addr 0x2 115 #define USER_DATA4 0x0 //reg addr 0x3 116 #define USER_DATA5 0x0 //reg addr 0x4 117 #define USER_DATA6 0x0 //reg addr 0x5 118 #define USER_DATA7 0x0 //reg addr 0x6 119 #define USER_DATA8 0x0 //reg addr 0x7 120 #define USER_DATA9 0x0 //reg addr 0x8 121 #define USER_DATA10 0x0 //reg addr 0x9 122 #define USER_DATA11 0x0 //reg addr 0x0A 123 #define USER_DATA12 0x0 //reg addr 0x0B 124 #define USER_DATA13 0x0 //reg addr 0x0C 125 #define USER_DATA14 0x0 //reg addr 0x0D 126 #define USER_DATA15 0x0 //reg addr 0x0E 127 #define USER_DATA16 0x0 //reg addr 0x0F 128 #define USER_DATA17 0x0 //reg addr 0x10 129 #define USER_DATA18 0x0 //reg addr 0x11 130 #define USER_DATA19 0x0 //reg addr 0x12 131 #define USER_DATA20 0x0 //reg addr 0x13 132 #define TVGAIN0 0x9D //reg addr 0x14 133 #define TVGAIN1 0xBD //reg addr 0x15 134 #define TVGAIN2 0xEF //reg addr 0x16 135 #define TVGAIN3 0x31 //reg addr 0x17 136 #define TVGAIN4 0x48 //reg addr 0x18 137 #define TVGAIN5 0x67 //reg addr 0x19 138 #define TVGAIN6 0xAC //reg addr 0x1A 139 #define INIT_GAIN 0x40 //reg addr 0x1B 140 #define FREQUENCY (uint8_t)(5*(_resonant_frequency - 30.0f)) //reg addr 0x1C 141 #define DEADTIME 0xF0 //reg addr 0x1D 142 #define PULSE_P1 0x0C //reg addr 0x1E 143 #define PULSE_P2 0x1F //reg addr 0x1F 144 #define CURR_LIM_P1 0x7F //reg addr 0x20 145 #define CURR_LIM_P2 0x7F //reg addr 0x21 146 #define REC_LENGTH 0x44 //reg addr 0x22 147 #define FREQ_DIAG 0x1B //reg addr 0x23 148 #define SAT_FDIAG_TH 0x2C //reg addr 0x24 149 #define FVOLT_DEC 0x7C //reg addr 0x25 150 #define DECPL_TEMP 0xDF //reg addr 0x26 151 #define DSP_SCALE 0x0 //reg addr 0x27 152 #define TEMP_TRIM 0x0 //reg addr 0x28 153 #define P1_GAIN_CTRL 0x0 //reg addr 0x29 154 #define P2_GAIN_CTRL 0x8 //reg addr 0x2A 155 #define EE_CRC 0x29 //reg addr 0x2B 158 #define EE_CNTRL 0x0 //reg addr 0x40 160 #define BPF_A2_MSB 0x85 //reg addr 0x41 161 #define BPF_A2_LSB 0xEA //reg addr 0x42 162 #define BPF_A3_MSB 0xF9 //reg addr 0x43 163 #define BPF_A3_LSB 0xA5 //reg addr 0x44 164 #define BPF_B1_MSB 0x3 //reg addr 0x45 165 #define BPF_B1_LSB 0x2D //reg addr 0x46 166 #define LPF_A2_MSB 0x7E //reg addr 0x47 167 #define LPF_A2_LSB 0x67 //reg addr 0x48 168 #define LPF_B1_MSB 0x0 //reg addr 0x49 169 #define LPF_B1_LSB 0xCD //reg addr 0x4A 171 #define TEST_MUX 0x0 //reg addr 0x4B 172 #define DEV_STAT0 0x80 //reg addr 0x4C 173 #define DEV_STAT1 0x0 //reg addr 0x4D 176 #define P1_THR_0 0x54 //reg addr 0x5F 177 #define P1_THR_1 0x5C //reg addr 0x60 178 #define P1_THR_2 0xBD //reg addr 0x61 179 #define P1_THR_3 0xE0 //reg addr 0x62 180 #define P1_THR_4 0x6 //reg addr 0x63 181 #define P1_THR_5 0xCF //reg addr 0x64 182 #define P1_THR_6 0xEE //reg addr 0x65 183 #define P1_THR_7 0x8E //reg addr 0x66 184 #define P1_THR_8 0x84 //reg addr 0x67 185 #define P1_THR_9 0xB6 //reg addr 0x68 186 #define P1_THR_10 0x5A //reg addr 0x69 187 #define P1_THR_11 0xFF //reg addr 0x6A 188 #define P1_THR_12 0xFF //reg addr 0x6B 189 #define P1_THR_13 0xFF //reg addr 0x6C 190 #define P1_THR_14 0xFF //reg addr 0x6D 191 #define P1_THR_15 0x0 //reg addr 0x6E 192 #define P2_THR_0 0xBA //reg addr 0x6F 193 #define P2_THR_1 0xCF //reg addr 0x70 194 #define P2_THR_2 0xFF //reg addr 0x71 195 #define P2_THR_3 0xF5 //reg addr 0x72 196 #define P2_THR_4 0x1A //reg addr 0x73 197 #define P2_THR_5 0x5F //reg addr 0x74 198 #define P2_THR_6 0xFA //reg addr 0x75 199 #define P2_THR_7 0xD6 //reg addr 0x76 200 #define P2_THR_8 0xB6 //reg addr 0x77 201 #define P2_THR_9 0x35 //reg addr 0x78 202 #define P2_THR_10 0xDF //reg addr 0x79 203 #define P2_THR_11 0xFF //reg addr 0x7A 204 #define P2_THR_12 0xFF //reg addr 0x7B 205 #define P2_THR_13 0xFF //reg addr 0x7C 206 #define P2_THR_14 0xFF //reg addr 0x7D 207 #define P2_THR_15 0x0 //reg addr 0x7E 208 #define THR_CRC 0x1D //reg addr 0x7F 241 static int print_usage(
const char *reason =
nullptr);
246 static int task_spawn(
int argc,
char *argv[]);
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...
static PGA460 * instantiate(int argc, char *argv[])
Instantiates the pga460 object.
static int task_spawn(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 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...
PGA460(const char *port=PGA460_DEFAULT_PORT)
int flash_eeprom()
Send the program command to the EEPROM to start the flash process.
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...
High-resolution timer with callouts and timekeeping.
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...
int initialize_thresholds()
Writes the user defined paramaters to device register map.
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.
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.
float _resonant_frequency
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
uint8_t read_register(const uint8_t reg)
Reads a register.
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.