From 88460f4406a53748ab4b059f41c90455cc1b0d38 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 13 Nov 2019 20:10:53 +0900 Subject: [PATCH] AP_Proximity: backup lightware SF40C driver to v09 --- libraries/AP_Proximity/AP_Proximity.cpp | 8 ++--- libraries/AP_Proximity/AP_Proximity.h | 2 +- ...pp => AP_Proximity_LightWareSF40C_v09.cpp} | 34 +++++++++---------- ...0C.h => AP_Proximity_LightWareSF40C_v09.h} | 4 +-- 4 files changed, 24 insertions(+), 24 deletions(-) rename libraries/AP_Proximity/{AP_Proximity_LightWareSF40C.cpp => AP_Proximity_LightWareSF40C_v09.cpp} (92%) rename libraries/AP_Proximity/{AP_Proximity_LightWareSF40C.h => AP_Proximity_LightWareSF40C_v09.h} (96%) diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 6c85ab6915..7bd0e3ee7b 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -14,7 +14,7 @@ */ #include "AP_Proximity.h" -#include "AP_Proximity_LightWareSF40C.h" +#include "AP_Proximity_LightWareSF40C_v09.h" #include "AP_Proximity_RPLidarA2.h" #include "AP_Proximity_TeraRangerTower.h" #include "AP_Proximity_TeraRangerTowerEvo.h" @@ -280,10 +280,10 @@ void AP_Proximity::detect_instance(uint8_t instance) switch (get_type(instance)) { case Type::None: return; - case Type::SF40C: - if (AP_Proximity_LightWareSF40C::detect()) { + case Type::SF40C_v09: + if (AP_Proximity_LightWareSF40C_v09::detect()) { state[instance].instance = instance; - drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance]); + drivers[instance] = new AP_Proximity_LightWareSF40C_v09(*this, state[instance]); return; } break; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 1555ef6771..72b024bf3a 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -40,7 +40,7 @@ public: // Proximity driver types enum class Type { None = 0, - SF40C = 1, + SF40C_v09 = 1, MAV = 2, TRTOWER = 3, RangeFinder = 4, diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp similarity index 92% rename from libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp rename to libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp index 69cd2c3fff..bca01b37b4 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp @@ -14,7 +14,7 @@ */ #include -#include "AP_Proximity_LightWareSF40C.h" +#include "AP_Proximity_LightWareSF40C_v09.h" #include #include #include @@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the proximity sensor */ -AP_Proximity_LightWareSF40C::AP_Proximity_LightWareSF40C(AP_Proximity &_frontend, +AP_Proximity_LightWareSF40C_v09::AP_Proximity_LightWareSF40C_v09(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state) { @@ -38,13 +38,13 @@ AP_Proximity_LightWareSF40C::AP_Proximity_LightWareSF40C(AP_Proximity &_frontend } // detect if a Lightware proximity sensor is connected by looking for a configured serial port -bool AP_Proximity_LightWareSF40C::detect() +bool AP_Proximity_LightWareSF40C_v09::detect() { return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr; } // update the state of the sensor -void AP_Proximity_LightWareSF40C::update(void) +void AP_Proximity_LightWareSF40C_v09::update(void) { if (uart == nullptr) { return; @@ -70,17 +70,17 @@ void AP_Proximity_LightWareSF40C::update(void) } // get maximum and minimum distances (in meters) of primary sensor -float AP_Proximity_LightWareSF40C::distance_max() const +float AP_Proximity_LightWareSF40C_v09::distance_max() const { return 100.0f; } -float AP_Proximity_LightWareSF40C::distance_min() const +float AP_Proximity_LightWareSF40C_v09::distance_min() const { return 0.20f; } // initialise sensor (returns true if sensor is succesfully initialised) -bool AP_Proximity_LightWareSF40C::initialise() +bool AP_Proximity_LightWareSF40C_v09::initialise() { // set motor direction once per second if (_motor_direction > 1) { @@ -110,7 +110,7 @@ bool AP_Proximity_LightWareSF40C::initialise() } // initialise sector angles using user defined ignore areas -void AP_Proximity_LightWareSF40C::init_sectors() +void AP_Proximity_LightWareSF40C_v09::init_sectors() { // use defaults if no ignore areas defined uint8_t ignore_area_count = get_ignore_area_count(); @@ -170,7 +170,7 @@ void AP_Proximity_LightWareSF40C::init_sectors() } // set speed of rotating motor -void AP_Proximity_LightWareSF40C::set_motor_speed(bool on_off) +void AP_Proximity_LightWareSF40C_v09::set_motor_speed(bool on_off) { // exit immediately if no uart if (uart == nullptr) { @@ -191,7 +191,7 @@ void AP_Proximity_LightWareSF40C::set_motor_speed(bool on_off) } // set spin direction of motor -void AP_Proximity_LightWareSF40C::set_motor_direction() +void AP_Proximity_LightWareSF40C_v09::set_motor_direction() { // exit immediately if no uart if (uart == nullptr) { @@ -212,7 +212,7 @@ void AP_Proximity_LightWareSF40C::set_motor_direction() } // set forward direction (to allow rotating lidar) -void AP_Proximity_LightWareSF40C::set_forward_direction() +void AP_Proximity_LightWareSF40C_v09::set_forward_direction() { // exit immediately if no uart if (uart == nullptr) { @@ -233,7 +233,7 @@ void AP_Proximity_LightWareSF40C::set_forward_direction() } // request new data if required -void AP_Proximity_LightWareSF40C::request_new_data() +void AP_Proximity_LightWareSF40C_v09::request_new_data() { if (uart == nullptr) { return; @@ -261,7 +261,7 @@ void AP_Proximity_LightWareSF40C::request_new_data() } // send request for sensor health -void AP_Proximity_LightWareSF40C::send_request_for_health() +void AP_Proximity_LightWareSF40C_v09::send_request_for_health() { if (uart == nullptr) { return; @@ -273,7 +273,7 @@ void AP_Proximity_LightWareSF40C::send_request_for_health() } // send request for distance from the next sector -bool AP_Proximity_LightWareSF40C::send_request_for_distance() +bool AP_Proximity_LightWareSF40C_v09::send_request_for_distance() { if (uart == nullptr) { return false; @@ -301,7 +301,7 @@ bool AP_Proximity_LightWareSF40C::send_request_for_distance() } // check for replies from sensor, returns true if at least one message was processed -bool AP_Proximity_LightWareSF40C::check_for_reply() +bool AP_Proximity_LightWareSF40C_v09::check_for_reply() { if (uart == nullptr) { return false; @@ -368,7 +368,7 @@ bool AP_Proximity_LightWareSF40C::check_for_reply() } // process reply -bool AP_Proximity_LightWareSF40C::process_reply() +bool AP_Proximity_LightWareSF40C_v09::process_reply() { if (uart == nullptr) { return false; @@ -436,7 +436,7 @@ bool AP_Proximity_LightWareSF40C::process_reply() } // clear buffers ahead of processing next message -void AP_Proximity_LightWareSF40C::clear_buffers() +void AP_Proximity_LightWareSF40C_v09::clear_buffers() { element_len[0] = 0; element_len[1] = 0; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h similarity index 96% rename from libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h rename to libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h index ce1a076435..a34f309710 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h @@ -5,12 +5,12 @@ #define PROXIMITY_SF40C_TIMEOUT_MS 200 // requests timeout after 0.2 seconds -class AP_Proximity_LightWareSF40C : public AP_Proximity_Backend +class AP_Proximity_LightWareSF40C_v09 : public AP_Proximity_Backend { public: // constructor - AP_Proximity_LightWareSF40C(AP_Proximity &_frontend, + AP_Proximity_LightWareSF40C_v09(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); // static detection function