123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187 |
- #pragma once
- #include <AP_Common/AP_Common.h>
- #include <AP_HAL/AP_HAL.h>
- #include <AP_Param/AP_Param.h>
- #include <GCS_MAVLink/GCS.h>
- #include "AP_RangeFinder_Params.h"
- #define RANGEFINDER_MAX_INSTANCES 10
- #define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10
- #define RANGEFINDER_PREARM_ALT_MAX_CM 200
- #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
- #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 0
- #else
- #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 50
- #endif
- class AP_RangeFinder_Backend;
- class RangeFinder
- {
- friend class AP_RangeFinder_Backend;
-
- friend class AP_RangeFinder_UAVCAN;
- public:
- RangeFinder();
-
- RangeFinder(const RangeFinder &other) = delete;
- RangeFinder &operator=(const RangeFinder&) = delete;
-
- enum RangeFinder_Type {
- RangeFinder_TYPE_NONE = 0,
- RangeFinder_TYPE_ANALOG = 1,
- RangeFinder_TYPE_MBI2C = 2,
- RangeFinder_TYPE_PLI2C = 3,
- RangeFinder_TYPE_PX4 = 4,
- RangeFinder_TYPE_PX4_PWM= 5,
- RangeFinder_TYPE_BBB_PRU= 6,
- RangeFinder_TYPE_LWI2C = 7,
- RangeFinder_TYPE_LWSER = 8,
- RangeFinder_TYPE_BEBOP = 9,
- RangeFinder_TYPE_MAVLink = 10,
- RangeFinder_TYPE_ULANDING= 11,
- RangeFinder_TYPE_LEDDARONE = 12,
- RangeFinder_TYPE_MBSER = 13,
- RangeFinder_TYPE_TRI2C = 14,
- RangeFinder_TYPE_PLI2CV3= 15,
- RangeFinder_TYPE_VL53L0X = 16,
- RangeFinder_TYPE_NMEA = 17,
- RangeFinder_TYPE_WASP = 18,
- RangeFinder_TYPE_BenewakeTF02 = 19,
- RangeFinder_TYPE_BenewakeTFmini = 20,
- RangeFinder_TYPE_PLI2CV3HP = 21,
- RangeFinder_TYPE_PWM = 22,
- RangeFinder_TYPE_BLPing = 23,
- RangeFinder_TYPE_UAVCAN = 24,
- RangeFinder_TYPE_BenewakeTFminiPlus = 25,
- RangeFinder_TYPE_Lanbao = 26,
- };
- enum RangeFinder_Function {
- FUNCTION_LINEAR = 0,
- FUNCTION_INVERTED = 1,
- FUNCTION_HYPERBOLA = 2
- };
- enum RangeFinder_Status {
- RangeFinder_NotConnected = 0,
- RangeFinder_NoData,
- RangeFinder_OutOfRangeLow,
- RangeFinder_OutOfRangeHigh,
- RangeFinder_Good
- };
-
- struct RangeFinder_State {
- uint16_t distance_cm;
- uint16_t voltage_mv;
- enum RangeFinder_Status status;
- uint8_t range_valid_count;
- uint32_t last_reading_ms;
- const struct AP_Param::GroupInfo *var_info;
- };
- static const struct AP_Param::GroupInfo *backend_var_info[RANGEFINDER_MAX_INSTANCES];
-
- static const struct AP_Param::GroupInfo var_info[];
- void set_log_rfnd_bit(uint32_t log_rfnd_bit) { _log_rfnd_bit = log_rfnd_bit; }
-
- uint8_t num_sensors(void) const {
- return num_instances;
- }
-
- bool prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const;
-
- void init(enum Rotation orientation_default);
-
-
- void update(void);
-
- void handle_msg(const mavlink_message_t &msg);
-
- bool has_orientation(enum Rotation orientation) const;
-
- AP_RangeFinder_Backend *find_instance(enum Rotation orientation) const;
- AP_RangeFinder_Backend *get_backend(uint8_t id) const;
-
-
- uint16_t distance_cm_orient(enum Rotation orientation) const;
- uint16_t voltage_mv_orient(enum Rotation orientation) const;
- int16_t max_distance_cm_orient(enum Rotation orientation) const;
- int16_t min_distance_cm_orient(enum Rotation orientation) const;
- int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
- MAV_DISTANCE_SENSOR get_mav_distance_sensor_type_orient(enum Rotation orientation) const;
- RangeFinder_Status status_orient(enum Rotation orientation) const;
- bool has_data_orient(enum Rotation orientation) const;
- uint8_t range_valid_count_orient(enum Rotation orientation) const;
- const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
- uint32_t last_reading_ms(enum Rotation orientation) const;
-
- void set_rfnd_bit(uint32_t log_rfnd_bit) { _log_rfnd_bit = log_rfnd_bit; }
-
- void set_estimated_terrain_height(float height) {
- estimated_terrain_height = height;
- }
- static RangeFinder *get_singleton(void) { return _singleton; }
- protected:
- AP_RangeFinder_Params params[RANGEFINDER_MAX_INSTANCES];
- private:
- static RangeFinder *_singleton;
- RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
- AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
- uint8_t num_instances;
- float estimated_terrain_height;
- Vector3f pos_offset_zero;
- void convert_params(void);
- void detect_instance(uint8_t instance, uint8_t& serial_instance);
- bool _add_backend(AP_RangeFinder_Backend *driver);
- uint32_t _log_rfnd_bit = -1;
- void Log_RFND();
- };
- namespace AP {
- RangeFinder *rangefinder();
- };
|