diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 1ba08b6228..ef50a70290 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -23,7 +23,8 @@ #include "AP_Proximity_RangeFinder.h" #include "AP_Proximity_MAV.h" #include "AP_Proximity_LightWareSF40C.h" -#include "AP_Proximity_LightWareSF45B.h" +#include "AP_Proximity_LightWareSF45B_Serial.h" +#include "AP_Proximity_LightWareSF45B_I2C.h" #include "AP_Proximity_SITL.h" #include "AP_Proximity_AirSimSITL.h" #include "AP_Proximity_Cygbot_D1.h" @@ -32,7 +33,6 @@ #include "AP_Proximity_LD06.h" #include "AP_Proximity_MR72_CAN.h" - #include extern const AP_HAL::HAL &hal; @@ -197,11 +197,11 @@ void AP_Proximity::init() } break; #endif -#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED +#if AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED case Type::SF45B: - if (AP_Proximity_LightWareSF45B::detect(serial_instance)) { + if (AP_Proximity_LightWareSF45B_Serial::detect(serial_instance)) { state[instance].instance = instance; - drivers[instance] = NEW_NOTHROW AP_Proximity_LightWareSF45B(*this, state[instance], params[instance], serial_instance); + drivers[instance] = NEW_NOTHROW AP_Proximity_LightWareSF45B_Serial(*this, state[instance], params[instance], serial_instance); serial_instance++; } break; @@ -252,6 +252,23 @@ void AP_Proximity::init() serial_instance++; } break; +#endif +#if AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED + case Type::SF45B_I2C: + uint8_t addr = PROXIMITY_SF45B_I2C_ADDRESS; + if (params[instance].address != 0) { + addr = params[instance].address; + } + FOREACH_I2C(i) { + drivers[instance] = AP_Proximity_LightWareSF45B_I2C::detect(*this, state[instance], params[instance], + hal.i2c_mgr->get_device(i, addr)); + + if (drivers[instance] != nullptr) { + state[instance].instance = instance; + break; + } + } + break; #endif } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 61a52e0270..72a276eed8 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -66,7 +66,7 @@ public: #if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED SF40C = 7, #endif -#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED +#if AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED SF45B = 8, #endif #if AP_PROXIMITY_SITL_ENABLED @@ -89,6 +89,9 @@ public: #endif #if AP_PROXIMITY_MR72_ENABLED MR72 = 17, +#endif +#if AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED + SF45B_I2C = 18, #endif }; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h index 203c59bea6..c5704ec95b 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h @@ -2,35 +2,22 @@ #include "AP_Proximity_config.h" -#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED - -#include "AP_Proximity_LightWareSerial.h" - +#if AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED || AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED +#include "AP_Proximity.h" +#include "AP_Proximity_Backend.h" #include -class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial -{ +static const uint32_t PROXIMITY_SF45B_TIMEOUT_MS = 200; +static const uint32_t PROXIMITY_SF45B_REINIT_INTERVAL_MS = 5000; // re-initialise sensor after this many milliseconds +static const float PROXIMITY_SF45B_COMBINE_READINGS_DEG = 5.0f; // combine readings from within this many degrees to improve efficiency +static const uint32_t PROXIMITY_SF45B_DESIRED_FIELDS = ((uint32_t)1 << 0 | (uint32_t)1 << 8); // first return (unfiltered), yaw angle +static const uint16_t PROXIMITY_SF45B_DESIRED_FIELD_COUNT = 2; // DISTANCE_DATA_CM message should contain two fields +class AP_Proximity_LightWareSF45B +{ public: // constructor - AP_Proximity_LightWareSF45B(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state, - AP_Proximity_Params& _params, - uint8_t serial_instance) : - AP_Proximity_LightWareSerial(_frontend, _state, _params, serial_instance) {} - - uint16_t rxspace() const override { - return 1280; - }; - - // update state - void update(void) override; - - // get maximum and minimum distances (in meters) of sensor - float distance_max() const override { return 50.0f; } - float distance_min() const override { return 0.20f; } - -private: + AP_Proximity_LightWareSF45B() {} // message ids enum class MessageID : uint8_t { @@ -57,6 +44,7 @@ private: LOST_SIGNAL_COUNTER = 76, BAUD_RATE = 79, I2C_ADDRESS = 80, + SCAN_SPEED = 85, STEPPER_STATUS = 93, SCAN_ON_STARTUP = 94, SCAN_ENABLE = 96, @@ -65,20 +53,8 @@ private: SCAN_HIGH_ANGLE = 99 }; - // initialise sensor - void initialise(); - - // request start of streaming of distances - void request_stream_start(); - - // check and process replies from sensor - void process_replies(); - - // process the latest message held in the msg structure - void process_message(); - // convert an angle (in degrees) to a mini sector number - uint8_t convert_angle_to_minisector(float angle_deg) const; + uint8_t convert_angle_to_minisector(float angle_deg) const { return wrap_360(angle_deg + (PROXIMITY_SF45B_COMBINE_READINGS_DEG * 0.5f)) / PROXIMITY_SF45B_COMBINE_READINGS_DEG; }; // internal variables uint32_t _last_init_ms; // system time of last re-initialisation @@ -97,14 +73,6 @@ private: float _minisector_distance; // shortest distance (in meters) in mini sector float _minisector_angle; // angle (in degrees) of shortest distance in mini sector bool _minisector_distance_valid; // true if mini sector has at least one valid distance - - // state of sensor - struct { - uint8_t update_rate; // sensor reported update rate enum from UPDATE_RATE message - uint32_t streaming_fields; // sensor reported bitmask of fields sent in DISTANCE_DATA_CM message - uint32_t stream_data_type; // sensor reported stream value. 5 if DISTANCE_DATA_CM messages are being streamed - } _sensor_state; - }; -#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_I2C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_I2C.cpp new file mode 100644 index 0000000000..524811318a --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_I2C.cpp @@ -0,0 +1,229 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + The Lightware SF45B serial interface is described on this wiki page + http://support.lightware.co.za/sf45/#/commands +*/ + +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED + +#include "AP_Proximity_LightWareSF45B_I2C.h" +#include + +#include +#include +#include +#include + +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +AP_Proximity_LightWareSF45B_I2C::AP_Proximity_LightWareSF45B_I2C(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params &_params, + AP_HAL::OwnPtr dev) + : AP_Proximity_Backend(_frontend, _state, _params), _dev(std::move(dev)) +{ +} + +AP_Proximity_Backend* AP_Proximity_LightWareSF45B_I2C::detect(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params &_params, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + + AP_Proximity_LightWareSF45B_I2C *sensor = new AP_Proximity_LightWareSF45B_I2C(_frontend, _state, _params, std::move(dev)); + if (!sensor || !sensor->initialise()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +// configure the sensor and check if it is saved +bool AP_Proximity_LightWareSF45B_I2C::initialise(void) +{ + _dev->get_semaphore()->take_blocking(); + + if (!configure_sensor()) { + _dev->get_semaphore()->give(); + return false; + } + + hal.scheduler->delay(100); + + if (!get_configuration()) { + _dev->get_semaphore()->give(); + return false; + } + + float reading_m, yaw; + if (!get_reading(reading_m, yaw)) { + _dev->get_semaphore()->give(); + return false; + } + + _dev->get_semaphore()->give(); + _dev->register_periodic_callback(2500, FUNCTOR_BIND_MEMBER(&AP_Proximity_LightWareSF45B_I2C::timer, void)); + + return true; +} + +// configure sensor to send distances and servo angle only +bool AP_Proximity_LightWareSF45B_I2C::configure_sensor() +{ + struct PACKED { + uint8_t message_id = uint8_t(MessageID::DISTANCE_OUTPUT); + uint32_t data = PROXIMITY_SF45B_DESIRED_FIELDS; + } cmd; + + if (!_dev->transfer((uint8_t *) &cmd, sizeof(cmd), nullptr, 0)) { + return false; + } + + return true; +} + +// get configuration from sensor +bool AP_Proximity_LightWareSF45B_I2C::get_configuration() +{ + const uint8_t cmd = uint8_t(MessageID::DISTANCE_OUTPUT); + if (!_dev->transfer(&cmd, sizeof(cmd), nullptr, 0)) { + return false; + } + + hal.scheduler->delay(100); + + uint8_t response[4]; + if (_dev->transfer(nullptr, 0, response, sizeof(response))) { + const uint32_t configuration = le32toh_ptr(response); + if (configuration != PROXIMITY_SF45B_DESIRED_FIELDS) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "SF45B: Unexpected configuration %08x", (unsigned int)configuration); + return false; + } + return true; + } + + return false; +} + +// trigger a distance reading +bool AP_Proximity_LightWareSF45B_I2C::get_reading(float &reading_m, float &yaw_angle_deg) +{ + const uint8_t cmd = uint8_t(MessageID::DISTANCE_DATA_CM); + _dev->transfer(&cmd, sizeof(cmd), nullptr, 0); + + struct PACKED { + int16_t first_distance_cm; + int16_t yaw_angle_cdeg; + } packet; + + const bool ret = _dev->transfer(nullptr, 0, reinterpret_cast(&packet), sizeof(packet)); + + if (ret) { + reading_m = _distance_filt.apply(packet.first_distance_cm*0.01f); + yaw_angle_deg = packet.yaw_angle_cdeg * 0.01f; + } + + return ret; +} + +// timer function +void AP_Proximity_LightWareSF45B_I2C::timer(void) +{ + float dist_m, yaw_angle_deg; + if (get_reading(dist_m, yaw_angle_deg)) { + WITH_SEMAPHORE(_sem); + distance_m = dist_m; + yaw_deg = yaw_angle_deg; + new_distance = true; + _last_distance_received_ms = AP_HAL::millis(); + } +} + +void AP_Proximity_LightWareSF45B_I2C::update(void) +{ + WITH_SEMAPHORE(_sem); + + if (new_distance) { + process_message(distance_m, yaw_deg); + new_distance = false; + } + + if ((_last_distance_received_ms == 0) || ((AP_HAL::millis() - _last_distance_received_ms) > PROXIMITY_SF45B_TIMEOUT_MS)) { + set_status(AP_Proximity::Status::NoData); + } else { + set_status(AP_Proximity::Status::Good); + } +} + +void AP_Proximity_LightWareSF45B_I2C::process_message(float dist_m, float angle) +{ + _last_distance_received_ms = AP_HAL::millis(); + const float angle_deg = correct_angle_for_orientation(angle); + const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg); + + if (face != _face) { + if (_face_distance_valid) { + frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance, state.instance); + } else { + // mark previous face invalid + frontend.boundary.reset_face(_face, state.instance); + } + // record updated face + _face = face; + _face_yaw_deg = 0; + _face_distance = INT16_MAX; + _face_distance_valid = false; + } + + // if distance is from a new minisector then update obstacle database using angle and distance from previous minisector + const uint8_t minisector = convert_angle_to_minisector(angle_deg); + if (minisector != _minisector) { + if ((_minisector != UINT8_MAX) && _minisector_distance_valid) { + database_push(_minisector_angle, _minisector_distance); + } + // init mini sector + _minisector = minisector; + _minisector_angle = 0; + _minisector_distance = INT16_MAX; + _minisector_distance_valid = false; + } + + if (!ignore_reading(angle_deg, dist_m) && (dist_m >= distance_min()) && (dist_m <= distance_max())) { + if (!_face_distance_valid || (dist_m < _face_distance)) { + _face_yaw_deg = angle_deg; + _face_distance = dist_m; + _face_distance_valid = true; + } + + if (dist_m < _minisector_distance) { + _minisector_angle = angle_deg; + _minisector_distance = dist_m; + _minisector_distance_valid = true; + } + } +} + +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_I2C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_I2C.h new file mode 100644 index 0000000000..d836a196f2 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_I2C.h @@ -0,0 +1,60 @@ +#pragma once + +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED + +#include "AP_Proximity_LightWareSF45B.h" + +#include + +static const uint8_t PROXIMITY_SF45B_I2C_ADDRESS = 0x66; + +class AP_Proximity_LightWareSF45B_I2C : public AP_Proximity_Backend, public AP_Proximity_LightWareSF45B +{ +public: + + // static detection function + static AP_Proximity_Backend *detect(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params& _params, + AP_HAL::OwnPtr dev); + + // update state + void update(void) override; + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const override { return 50.0f; } + float distance_min() const override { return 0.20f; } + +private: + + // constructor + AP_Proximity_LightWareSF45B_I2C(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params, AP_HAL::OwnPtr dev); + + // initialise sensor + bool initialise(); + + // timer function + void timer(void); + + // configure sensor to send distances and servo angle only + bool configure_sensor(); + + // get configuration from sensor + bool get_configuration(); + + // trigger a distance reading + bool get_reading(float &reading_m, float &ang_deg); + + // process the latest message held in the msg structure + void process_message(float distance_m, float angle_deg); + + AP_HAL::OwnPtr _dev; + + float distance_m; + float yaw_deg; + bool new_distance; // true if we have a new distance +}; + +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_Serial.cpp similarity index 84% rename from libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp rename to libraries/AP_Proximity/AP_Proximity_LightWareSF45B_Serial.cpp index 1911ea9126..f92b986810 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_Serial.cpp @@ -18,9 +18,9 @@ #include "AP_Proximity_config.h" -#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED +#if AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED -#include "AP_Proximity_LightWareSF45B.h" +#include "AP_Proximity_LightWareSF45B_Serial.h" #include #include @@ -28,16 +28,11 @@ extern const AP_HAL::HAL& hal; -static const uint32_t PROXIMITY_SF45B_TIMEOUT_MS = 200; -static const uint32_t PROXIMITY_SF45B_REINIT_INTERVAL_MS = 5000; // re-initialise sensor after this many milliseconds -static const float PROXIMITY_SF45B_COMBINE_READINGS_DEG = 5.0f; // combine readings from within this many degrees to improve efficiency static const uint32_t PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM = 5; static const uint8_t PROXIMITY_SF45B_DESIRED_UPDATE_RATE = 6; // 1:48hz, 2:55hz, 3:64hz, 4:77hz, 5:97hz, 6:129hz, 7:194hz, 8:388hz -static const uint32_t PROXIMITY_SF45B_DESIRED_FIELDS = ((uint32_t)1 << 0 | (uint32_t)1 << 8); // first return (unfiltered), yaw angle -static const uint16_t PROXIMITY_SF45B_DESIRED_FIELD_COUNT = 2; // DISTANCE_DATA_CM message should contain two fields // update the state of the sensor -void AP_Proximity_LightWareSF45B::update(void) +void AP_Proximity_LightWareSF45B_Serial::update(void) { if (_uart == nullptr) { return; @@ -58,7 +53,7 @@ void AP_Proximity_LightWareSF45B::update(void) } // initialise sensor -void AP_Proximity_LightWareSF45B::initialise() +void AP_Proximity_LightWareSF45B_Serial::initialise() { // check sensor is configured correctly _init_complete = (_sensor_state.stream_data_type == PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM) && @@ -77,7 +72,7 @@ void AP_Proximity_LightWareSF45B::initialise() } // request start of streaming of distances -void AP_Proximity_LightWareSF45B::request_stream_start() +void AP_Proximity_LightWareSF45B_Serial::request_stream_start() { // request output rate send_message((uint8_t)MessageID::UPDATE_RATE, true, &PROXIMITY_SF45B_DESIRED_UPDATE_RATE, sizeof(PROXIMITY_SF45B_DESIRED_UPDATE_RATE)); @@ -90,7 +85,7 @@ void AP_Proximity_LightWareSF45B::request_stream_start() } // check for replies from sensor -void AP_Proximity_LightWareSF45B::process_replies() +void AP_Proximity_LightWareSF45B_Serial::process_replies() { if (_uart == nullptr) { return; @@ -110,7 +105,7 @@ void AP_Proximity_LightWareSF45B::process_replies() } // process the latest message held in the _msg structure -void AP_Proximity_LightWareSF45B::process_message() +void AP_Proximity_LightWareSF45B_Serial::process_message() { // process payload switch ((MessageID)_msg.msgid) { @@ -197,10 +192,4 @@ void AP_Proximity_LightWareSF45B::process_message() } } -// convert an angle (in degrees) to a mini sector number -uint8_t AP_Proximity_LightWareSF45B::convert_angle_to_minisector(float angle_deg) const -{ - return wrap_360(angle_deg + (PROXIMITY_SF45B_COMBINE_READINGS_DEG * 0.5f)) / PROXIMITY_SF45B_COMBINE_READINGS_DEG; -} - -#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_Serial.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_Serial.h new file mode 100644 index 0000000000..316286d01a --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B_Serial.h @@ -0,0 +1,55 @@ +#pragma once + +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED + +#include "AP_Proximity_LightWareSerial.h" +#include "AP_Proximity_LightWareSF45B.h" + +class AP_Proximity_LightWareSF45B_Serial : public AP_Proximity_LightWareSerial, public AP_Proximity_LightWareSF45B +{ + +public: + // constructor + AP_Proximity_LightWareSF45B_Serial(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params& _params, + uint8_t serial_instance) : + AP_Proximity_LightWareSerial(_frontend, _state, _params, serial_instance) {} + + uint16_t rxspace() const override { + return 1280; + }; + + // update state + void update(void) override; + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const override { return 50.0f; } + float distance_min() const override { return 0.20f; } + +private: + + // initialise sensor + void initialise(); + + // request start of streaming of distances + void request_stream_start(); + + // check and process replies from sensor + void process_replies(); + + // process the latest message held in the msg structure + void process_message(); + + // state of sensor + struct { + uint8_t update_rate; // sensor reported update rate enum from UPDATE_RATE message + uint32_t streaming_fields; // sensor reported bitmask of fields sent in DISTANCE_DATA_CM message + uint32_t stream_data_type; // sensor reported stream value. 5 if DISTANCE_DATA_CM messages are being streamed + } _sensor_state; + +}; + +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Params.cpp b/libraries/AP_Proximity/AP_Proximity_Params.cpp index ad85936067..69a3f86c4b 100644 --- a/libraries/AP_Proximity/AP_Proximity_Params.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06, 17: MR72_CAN + // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B_Serial,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06, 17: MR72_CAN, 18:LightwareSF45B_I2C // @RebootRequired: True // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index a05e94b653..9c52b8c698 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -33,10 +33,13 @@ #define AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif -#ifndef AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED -#define AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#ifndef AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED +#define AP_PROXIMITY_LIGHTWARE_SF45B_SERIAL_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED +#define AP_PROXIMITY_LIGHTWARE_SF45B_I2C_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif #ifndef AP_PROXIMITY_MR72_ENABLED #define AP_PROXIMITY_MR72_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_MAX_CAN_PROTOCOL_DRIVERS