command.h 4.32 KB
Newer Older
1 2 3 4 5 6 7 8 9
/**  @file     command.h
  *  @author   Adi Singh, Christopher Lang
  *  @date     March 2019
  */

#ifndef COMMANDS_H
#define COMMANDS_H

#include <toposens_driver/TsDriverConfig.h>
10
#define USE_INTERNAL_TEMPERATURE -100.0
11 12 13 14

namespace toposens_driver
{

15
/** Defined in TS firmware and exposed as public constants to
16
 *  be accessed by the Command API. Ranges and default values
17 18 19 20 21 22
 *  are stated in the package's cfg file.
 *  The enumeration values are used as dynamic reconfiguration levels by the dynamic reconfiguration server, which
 *  matches parameters based their level bit mask. In case of multiple parameter updates, the bit masks are
 *  concatenated by an OR operation.
 */
enum TsParam
Christopher Lang's avatar
Christopher Lang committed
23
{
24 25 26 27 28 29
  UseExternalTemperature  = 0b00000001, /**< Switch between internal temperature sensor value or user-defined temp value. */
  EchoRejectionThreshold  = 0b00000010, /**< Minimum amplitude for an echo to be considered valid [0 to 20]. */
  NoiseIndicatorThreshold = 0b00000100, /**< Normalized noise level on ADC signals to mark processed points as noisy. */
  NumberOfPulses          = 0b00001000, /**< Number of ultrasonic pulses emitted in every transmission cycle [0 to 20]. */
  PeakDetectionWindow     = 0b00010000, /**< Kernel size applied on ADC signals for peak detection [1 to 100]. */
  ExternalTemperature     = 0b00100000, /**< Temperature value used to calibrate speed-of-sound. */
Sebastian Dengler's avatar
Sebastian Dengler committed
30
  ScanMode                = 0b01000000, /**< Scan Mode of Sensor: 0 for scanning continuously; 1 for scanning once; 2 for listening only (once) */
31 32
};

Christopher Lang's avatar
Christopher Lang committed
33 34 35 36 37 38
enum TsService
{
  FirmwareConfiguration,
  FirmwareVersion
};

39 40 41 42 43 44 45 46 47 48 49 50 51 52
/** @brief Generates firmware-compatible commands for tuning
 *  performance parameters.
 *
 *  @details Firmware defines commands in two formats, singular
 *  and dimensional. Singular commands expect one parameter value,
 *  dimensional commands expect XYZ limits defining a spatial cuboid.
 *  Currently, only the voxel filtering command is dimensionally formatted.
 *
 *  Note that all desired values are transmitted as zero-padded 5-byte
 *  strings, and the first byte is always reserved for the arithmetic
 *  sign: 0 for positive values, - for negative values.
 *
 *  Note that modifications on these parameters are done on the sensor chip, not in this driver
 */
Adi Singh's avatar
Adi Singh committed
53 54
class Command
{
55
  public:
Adi Singh's avatar
Adi Singh committed
56
    /** Builds a command message accepted by the TS firmware.
Christopher Lang's avatar
Christopher Lang committed
57
     *  @param param Setting name from the enumerated TsParam list.
Adi Singh's avatar
Adi Singh committed
58 59
     *  @param value Desired integer value for sensor parameter.
     */
60
    Command(TsParam param, float value);
61

Christopher Lang's avatar
Christopher Lang committed
62 63 64 65 66
    /** Builds a command message accepted by the TS firmware to invoke a service.
     *  @param param Service name from the enumerated TsService list.
     */
    Command(TsService service);

67 68 69
    /**
     * Returns this object's parameter enumeration value denoting the firmware parameter to be updated.
     * @return parameter
70
     */
71
    TsParam getParam(){ return _param; }
72

Christopher Lang's avatar
Christopher Lang committed
73 74 75 76 77 78
    /**
     * Returns this object's parameter value denoting the parameter value to be set.
     * @return value
     */
    float getValue(){ return _value; }

79 80 81 82 83
    /** Returns the latest command message produced by generate().
     *  @returns Pointer to a char array containing command.
     */
    char* getBytes() { return _bytes; }

84 85 86 87 88 89
    /** Looks up command name defined by the TsDriver configuration to
     *  given setting parameters.
     *  @returns Corresponding dynamic reconfigure name.
     */
    std::string getParamName();

90

91
  private:
Adi Singh's avatar
Adi Singh committed
92 93 94 95
    const int MAX_VALUE =  9999;
    const int MIN_VALUE = -9999;

    char _bytes[50];   /**< Large enough buffer to hold a well-formed command.*/
96
    static const char kPrefix = 'C'; /**< Designates a string as a firmware command.*/
97
    TsParam _param; /**< Parameter to be encoded in command bytes.*/
Christopher Lang's avatar
Christopher Lang committed
98
    float _value; /**< Parameeter value to be encoded in command bytes. */
99

100 101 102 103 104
    /** Looks up command keys defined by the TS firmware corresponding to
     *  given setting parameters.
     *  @param param Setting name from the enumerated command list.
     *  @returns Corresponding firmware parameter key.
     */
105
    std::string _getKey(TsParam param);
Adi Singh's avatar
Adi Singh committed
106

Christopher Lang's avatar
Christopher Lang committed
107 108 109 110 111 112
    /** Looks up service keys defined by the TS firmware.
     *  @param param service name from the enumerated command list.
     *  @returns Corresponding firmware service key.
     */
    std::string _getKey(TsService service);

Adi Singh's avatar
Adi Singh committed
113
};
114 115 116 117

} // namespace toposens_driver

#endif