PX4 Firmware
PX4 Autopilot Software http://px4.io
pga460.h
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.h
36  * @author Jacob Dahl <jacob.dahl@tealdrones.com>
37  *
38  * Driver for the TI PGA460 Ultrasonic Signal Processor and Transducer Driver
39  */
40 
41 #ifndef _PGA460_H
42 #define _PGA460_H
43 
44 #include <cstring>
45 #include <termios.h>
46 
47 #include <drivers/device/device.h>
48 #include <drivers/drv_hrt.h>
49 
51 
52 #include <px4_platform_common/module.h>
53 #include <px4_platform_common/module_params.h>
54 #include <px4_platform_common/tasks.h>
55 
56 
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
66 // #define POLL_RATE_US 50000ULL
67 #define POLL_RATE_US 0ULL
68 
69 #define MODE_SHORT_RANGE P1BL
70 #define MODE_LONG_RANGE P2BL
71 
72 #define SYNCBYTE 0x55
73 
74 // Define UART commands by name
75 
76 // Single Address
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
94 
95 // Broadcast -- device will execute command irrespecive of UART address field
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
105 
106 // Addresses and Settings
107 #define EE_CNTRL_ADDR 0x40
108 #define EE_UNLOCK_ST1 0x68
109 #define EE_UNLOCK_ST2 0x69
110 
111 // EEPROM -- non-volatile
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
156 
157 // Register-based -- volatile
158 #define EE_CNTRL 0x0 //reg addr 0x40
159 
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
170 
171 #define TEST_MUX 0x0 //reg addr 0x4B
172 #define DEV_STAT0 0x80 //reg addr 0x4C
173 #define DEV_STAT1 0x0 //reg addr 0x4D
174 
175 // Register-based -- volatile
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
209 
210 class PGA460 : public ModuleBase<PGA460>
211 {
212 public:
213 
214  PGA460(const char *port = PGA460_DEFAULT_PORT);
215 
216  virtual ~PGA460();
217 
218  /**
219  * @see ModuleBase::custom_command().
220  * @brief main Main entry point to the module that should be
221  * called directly from the module's main method.
222  * @param argc The input argument count.
223  * @param argv Pointer to the input argument array.
224  * @return Returns 0 iff successful, -1 otherwise.
225  */
226  static int custom_command(int argc, char *argv[]);
227 
228  /**
229  * @see ModuleBase::instantiate().
230  * @brief Instantiates the pga460 object.
231  * @param argc The input argument count.
232  * @param argv Pointer to the input argument array.
233  */
234  static PGA460 *instantiate(int argc, char *argv[]);
235 
236  /**
237  * @see ModuleBase::print_usage().
238  * @brief Prints the module usage to the nuttshell console.
239  * @param reason The requested reason for printing to console.
240  */
241  static int print_usage(const char *reason = nullptr);
242 
243  /**
244  * @see ModuleBase::task_spawn().
245  */
246  static int task_spawn(int argc, char *argv[]);
247 
248  /**
249  * @brief Closes the serial port.
250  * @return Returns 0 if success or ERRNO.
251  */
252  int close_serial();
253 
254  /**
255  * @brief Opens the serial port.
256  * @return Returns true if the open was successful or ERRNO.
257  */
258  int open_serial();
259 
260  /**
261  * @brief Reports the diagnostic data from device status registers 1 and 2 if there is anything to report.
262  */
263  void print_device_status();
264  /**
265  * @brief Reports the diagnostic data the diagnostic byte (first byte from slave).
266  * @param diagnostic_byte The diagnostic byte that contains the bitflags.
267  */
268  void print_diagnostics(const uint8_t diagnostic_byte);
269 
270  /**
271  * Diagnostics - print some basic information about the driver.
272  */
273  int print_status() override;
274 
275  /**
276  * @brief Reads the threshold registers.
277  * @return Returns true if the threshold registers are set to default
278  */
280 
281  /**
282  * @see ModuleBase::run().
283  */
284  void run() override;
285 
286  /**
287  * @brief Reads the EEPROM and checks to see if it matches the default parameters.
288  * @note This method is only called once at boot.
289  * @return Returns PX4_OK if the EEPROM has default values, PX4_ERROR otherwise.
290  */
291  int read_eeprom();
292 
293  /**
294  * @brief Writes the user defined paramaters to device EEPROM.
295  * @return Returns true if the EEPROM was successfully written to.
296  */
297  int write_eeprom();
298 
299  /**
300  * @brief Reads a register.
301  * @param reg The register to read from.
302  * @return Returns the value of the register at the specified address.
303  */
304  uint8_t read_register(const uint8_t reg);
305 
306  /**
307  * @brief Writes a value to a register.
308  * @param reg The register address to write to.
309  * @param val The value to write.
310  * @return Returns true for success or false for fail.
311  */
312  int write_register(const uint8_t reg, const uint8_t val);
313 
314 private:
315 
316  /**
317  * @brief Calculates the checksum of the transmitted commmand + data.
318  * @param data Pointer to the data a checksum will be calculated for.
319  * @param size The size of the data (bytes) the checksum will be calculated for.
320  * @return Returns the single byte checksum.
321  */
322  uint8_t calc_checksum(uint8_t *data, const uint8_t size);
323 
324  /**
325  * @brief Calculates the distance from the measurement time of flight (time_of_flight) and current temperature.
326  * @param time_of_flight The reported time of flight in ms from the device.
327  * @return Returns the distance measurement in meters.
328  */
329  float calculate_object_distance(uint16_t time_of_flight);
330 
331  /**
332  * @brief Collects the data in the serial port rx buffer, does math, and publishes the value to uORB
333  * @return Returns the measurment results in format: (u16)time_of_flight, (u8)width, (u8)amplitude
334  */
335  uint32_t collect_results();
336 
337  /**
338  * @brief Send the program command to the EEPROM to start the flash process.
339  */
340  int flash_eeprom();
341 
342  /**
343  * @brief Writes program defined threshold defaults to the register map and checks/writes the EEPROM.
344  * @return Returns PX4_OK upon success or PX4_ERROR on fail.
345  */
347 
348  /**
349  * @brief Writes the user defined paramaters to device register map.
350  * @return Returns true if the thresholds were successfully written.
351  */
352  int initialize_thresholds();
353 
354  /**
355  * @brief Measurement is read from UART RX buffer and published to the uORB distance sensor topic.
356  */
357  int request_results();
358 
359  /**
360  * @brief Checks the measurement from last report and sets the range distance mode (long range , short range).
361  * @return Returns the either P1Bl or P1B2. Preset 1 (P1BL) is short range mode and preset two (P2BL) is long range mode.
362  */
363  uint8_t set_range_mode();
364 
365  /**
366  * @brief Commands the device to perform an ultrasonic measurement.
367  */
368  int take_measurement(const uint8_t mode);
369 
370  /*
371  * @brief Gets a temperature measurement in degrees C.
372  * @return Returns the temperature measurement in degrees C.
373  */
374  float get_temperature();
375 
376  /**
377  * @brief Send the unlock command to the EEPROM to enable reading and writing -- not needed w/ bulk write
378  */
379  int unlock_eeprom();
380 
381  /**
382  * @brief Commands the device to publish the measurement results to uORB.
383  * @param dist The calculated distance to the object.
384  */
385  void uORB_publish_results(const float dist);
386 
387  /** @orb_advert_t orb_advert_t uORB advertisement topic. */
389 
390  /** @param _fd Returns the file descriptor from px4_open(). */
391  int _fd{-1};
392 
393  /** @param _port Stores the port name. */
394  char _port[20] {};
395 
396  /** @param _previous_report_distance The previous reported sensor distance. */
398 
399  /** @param _previous_valid_report_distance The previous valid reported sensor distance. */
401 
402  /** @param _resonant_frequency The sensor resonant (transmit) frequency. */
403  float _resonant_frequency{41.0f};
404 
405  /** @param _mode_long_range Flag for long range mode. If false, sensor is in short range mode. */
407 
408  /** @param _start_loop The starting value for the loop time of the main loop. */
409  uint64_t _start_loop{0};
410 };
411 
412 #endif
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
static PGA460 * instantiate(int argc, char *argv[])
Instantiates the pga460 object.
Definition: pga460.cpp:101
static int task_spawn(int argc, char *argv[])
Definition: pga460.cpp:748
int read_threshold_registers()
Reads the threshold registers.
Definition: pga460.cpp:609
int close_serial()
Closes the serial port.
Definition: pga460.cpp:85
void uORB_publish_results(const float dist)
Commands the device to publish the measurement results to uORB.
Definition: pga460.cpp:767
int write_register(const uint8_t reg, const uint8_t val)
Writes a value to a register.
Definition: pga460.cpp:879
orb_advert_t _distance_sensor_topic
orb_advert_t uORB advertisement topic.
Definition: pga460.h:388
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
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
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
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
High-resolution timer with callouts and timekeeping.
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
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
int initialize_thresholds()
Writes the user defined paramaters to device register map.
Definition: pga460.cpp:131
int open_serial()
Opens the serial port.
Definition: pga460.cpp:293
uint8_t set_range_mode()
Checks the measurement from last report and sets the range distance mode (long range ...
Definition: pga460.cpp:718
int unlock_eeprom()
Send the unlock command to the EEPROM to enable reading and writing – not needed w/ bulk write...
Definition: pga460.cpp:819
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
int print_status() override
Diagnostics - print some basic information about the driver.
Definition: pga460.cpp:467
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
float _resonant_frequency
Definition: pga460.h:403
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int _fd
Definition: pga460.h:391
uint8_t read_register(const uint8_t reg)
Reads a register.
Definition: pga460.cpp:563
void print_diagnostics(const uint8_t diagnostic_byte)
Reports the diagnostic data the diagnostic byte (first byte from slave).
Definition: pga460.cpp:412
#define PGA460_DEFAULT_PORT
Definition: pga460.h:57
float _previous_valid_report_distance
Definition: pga460.h:400
int write_eeprom()
Writes the user defined paramaters to device EEPROM.
Definition: pga460.cpp:835
mode
Definition: vtol_type.h:76
float get_temperature()
Definition: pga460.cpp:238
void run() override
Definition: pga460.cpp:678