AP_Proximity: add and use defines for all Proximity backends

This commit is contained in:
Peter Barker 2023-04-05 22:25:54 +10:00 committed by Peter Barker
parent 92dea59620
commit c110ac489c
25 changed files with 191 additions and 85 deletions

View File

@ -121,10 +121,12 @@ void AP_Proximity::init()
// instantiate backends
uint8_t serial_instance = 0;
(void)serial_instance; // in case no serial backends are compiled in
for (uint8_t instance=0; instance<PROXIMITY_MAX_INSTANCES; instance++) {
switch (get_type(instance)) {
case Type::None:
break;
#if AP_PROXIMITY_RPLIDARA2_ENABLED
case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect(serial_instance)) {
state[instance].instance = instance;
@ -132,11 +134,14 @@ void AP_Proximity::init()
serial_instance++;
}
break;
#endif
#if AP_PROXIMITY_MAV_ENABLED
case Type::MAV:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]);
break;
#endif
#if AP_PROXIMITY_TERARANGERTOWER_ENABLED
case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect(serial_instance)) {
state[instance].instance = instance;
@ -144,6 +149,8 @@ void AP_Proximity::init()
serial_instance++;
}
break;
#endif
#if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect(serial_instance)) {
state[instance].instance = instance;
@ -151,12 +158,14 @@ void AP_Proximity::init()
serial_instance++;
}
break;
#endif
#if AP_PROXIMITY_RANGEFINDER_ENABLED
case Type::RangeFinder:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
break;
#endif
#if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect(serial_instance)) {
state[instance].instance = instance;
@ -164,7 +173,8 @@ void AP_Proximity::init()
serial_instance++;
}
break;
#endif
#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect(serial_instance)) {
state[instance].instance = instance;
@ -172,7 +182,7 @@ void AP_Proximity::init()
serial_instance++;
}
break;
#endif
#if AP_PROXIMITY_CYGBOT_ENABLED
case Type::CYGBOT_D1:
if (AP_Proximity_Cygbot_D1::detect(serial_instance)) {
@ -182,17 +192,18 @@ void AP_Proximity::init()
}
break;
# endif
#if AP_PROXIMITY_DRONECAN_ENABLED
case Type::DroneCAN:
num_instances = instance+1;
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#endif
#if AP_PROXIMITY_SITL_ENABLED
case Type::SITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]);
break;
#endif
#if AP_PROXIMITY_AIRSIMSITL_ENABLED
case Type::AirSimSITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);

View File

@ -48,21 +48,39 @@ public:
enum class Type {
None = 0,
// 1 was SF40C_v09
#if AP_PROXIMITY_MAV_ENABLED
MAV = 2,
#endif
#if AP_PROXIMITY_TERARANGERTOWER_ENABLED
TRTOWER = 3,
#endif
#if AP_PROXIMITY_RANGEFINDER_ENABLED
RangeFinder = 4,
#endif
#if AP_PROXIMITY_RPLIDARA2_ENABLED
RPLidarA2 = 5,
#endif
#if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
TRTOWEREVO = 6,
#endif
#if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
SF40C = 7,
#endif
#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
SF45B = 8,
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#endif
#if AP_PROXIMITY_SITL_ENABLED
SITL = 10,
#endif
#if AP_PROXIMITY_AIRSIMSITL_ENABLED
AirSimSITL = 12,
#endif
#if AP_PROXIMITY_CYGBOT_ENABLED
CYGBOT_D1 = 13,
#endif
#if AP_PROXIMITY_DRONECAN_ENABLED
DroneCAN = 14,
#endif
};
enum class Status {

View File

@ -13,15 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_AIRSIMSITL_ENABLED
#include "AP_Proximity_AirSimSITL.h"
#if HAL_PROXIMITY_ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_HAL/AP_HAL.h>
#include <stdio.h>
extern const AP_HAL::HAL& hal;
#define PROXIMITY_MAX_RANGE 100.0f
#define PROXIMITY_ACCURACY 0.1f // minimum distance (in meters) between objects sent to object database
@ -98,6 +95,4 @@ bool AP_Proximity_AirSimSITL::get_upward_distance(float &distance) const
return false;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_AIRSIMSITL_ENABLED

View File

@ -15,11 +15,12 @@
#pragma once
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_AIRSIMSITL_ENABLED
#include "AP_Proximity_Backend.h"
#if HAL_PROXIMITY_ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
class AP_Proximity_AirSimSITL : public AP_Proximity_Backend
@ -44,6 +45,6 @@ private:
AP_Proximity_Temp_Boundary temp_boundary;
};
#endif // CONFIG_HAL_BOARD
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_AIRSIMSITL_ENABLED

View File

@ -1,6 +1,8 @@
#include "AP_Proximity_Cygbot_D1.h"
#include "AP_Proximity_config.h"
#if HAL_PROXIMITY_ENABLED && AP_PROXIMITY_CYGBOT_ENABLED
#if AP_PROXIMITY_CYGBOT_ENABLED
#include "AP_Proximity_Cygbot_D1.h"
// update the state of the sensor
void AP_Proximity_Cygbot_D1::update()
@ -182,4 +184,4 @@ void AP_Proximity_Cygbot_D1::reset()
_temp_boundary.reset();
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_CYGBOT_ENABLED

View File

@ -1,8 +1,9 @@
#pragma once
#include "AP_Proximity.h"
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_CYGBOT_ENABLED
#if (HAL_PROXIMITY_ENABLED && AP_PROXIMITY_CYGBOT_ENABLED)
#include "AP_Proximity_Backend_Serial.h"
#define CYGBOT_MAX_MSG_SIZE 350
@ -80,4 +81,4 @@ private:
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_CYGBOT_ENABLED

View File

@ -191,5 +191,4 @@ void AP_Proximity_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const C
}
}
#endif // AP_PROXIMITY_DRONECAN_ENABLED

View File

@ -1,15 +1,13 @@
#pragma once
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_DRONECAN_ENABLED
#include "AP_Proximity_Backend.h"
#include <AP_DroneCAN/AP_DroneCAN.h>
#ifndef AP_PROXIMITY_DRONECAN_ENABLED
#define AP_PROXIMITY_DRONECAN_ENABLED (HAL_ENABLE_DRONECAN_DRIVERS && HAL_PROXIMITY_ENABLED)
#endif
#if AP_PROXIMITY_DRONECAN_ENABLED
class AP_Proximity_DroneCAN : public AP_Proximity_Backend
{
public:

View File

@ -13,9 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
#include "AP_Proximity_LightWareSF40C.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
@ -421,4 +424,4 @@ uint16_t AP_Proximity_LightWareSF40C::buff_to_uint16(uint8_t b0, uint8_t b1) con
return leval;
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED

View File

@ -1,8 +1,10 @@
#pragma once
#include "AP_Proximity_Backend_Serial.h"
#include "AP_Proximity_config.h"
#if HAL_PROXIMITY_ENABLED
#if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
#include "AP_Proximity_Backend_Serial.h"
#define PROXIMITY_SF40C_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
#define PROXIMITY_SF40C_PAYLOAD_LEN_MAX 256 // maximum payload size we can accept (in some configurations sensor may send as large as 1023)
@ -152,4 +154,4 @@ private:
uint16_t buff_to_uint16(uint8_t b0, uint8_t b1) const;
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED

View File

@ -16,8 +16,11 @@
http://support.lightware.co.za/sf45/#/commands
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
#include "AP_Proximity_LightWareSF45B.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
@ -200,4 +203,4 @@ uint8_t AP_Proximity_LightWareSF45B::convert_angle_to_minisector(float angle_deg
return wrap_360(angle_deg + (PROXIMITY_SF45B_COMBINE_READINGS_DEG * 0.5f)) / PROXIMITY_SF45B_COMBINE_READINGS_DEG;
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED

View File

@ -1,8 +1,11 @@
#pragma once
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
#include "AP_Proximity_LightWareSerial.h"
#if HAL_PROXIMITY_ENABLED
#include <Filter/Filter.h>
class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial
@ -104,4 +107,4 @@ private:
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED

View File

@ -13,9 +13,11 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Proximity_MAV.h"
#include "AP_Proximity_config.h"
#if HAL_PROXIMITY_ENABLED
#if AP_PROXIMITY_MAV_ENABLED
#include "AP_Proximity_MAV.h"
#include <AP_HAL/AP_HAL.h>
#include <ctype.h>
#include <stdio.h>
@ -272,4 +274,4 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
return;
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_MAV_ENABLED

View File

@ -1,8 +1,10 @@
#pragma once
#include "AP_Proximity_Backend.h"
#include "AP_Proximity_config.h"
#if HAL_PROXIMITY_ENABLED
#if AP_PROXIMITY_MAV_ENABLED
#include "AP_Proximity_Backend.h"
class AP_Proximity_MAV : public AP_Proximity_Backend
{
@ -46,4 +48,4 @@ private:
float _distance_upward; // upward distance in meters
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_MAV_ENABLED

View File

@ -26,9 +26,12 @@
*
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_RPLIDARA2_ENABLED
#include "AP_Proximity_RPLidarA2.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <ctype.h>
#include <stdio.h>
@ -361,4 +364,4 @@ void AP_Proximity_RPLidarA2::parse_response_data()
}
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_RPLIDARA2_ENABLED

View File

@ -29,9 +29,11 @@
#pragma once
#include "AP_Proximity_Backend_Serial.h"
#include "AP_Proximity_config.h"
#if HAL_PROXIMITY_ENABLED
#if AP_PROXIMITY_RPLIDARA2_ENABLED
#include "AP_Proximity_Backend_Serial.h"
class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
{
@ -121,4 +123,4 @@ private:
} payload;
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_RPLIDARA2_ENABLED

View File

@ -13,9 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_RANGEFINDER_ENABLED
#include "AP_Proximity_RangeFinder.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <ctype.h>
#include <stdio.h>
@ -94,4 +97,4 @@ bool AP_Proximity_RangeFinder::get_upward_distance(float &distance) const
return false;
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_RANGEFINDER_ENABLED

View File

@ -1,8 +1,10 @@
#pragma once
#include "AP_Proximity_Backend.h"
#include "AP_Proximity_config.h"
#if HAL_PROXIMITY_ENABLED
#if AP_PROXIMITY_RANGEFINDER_ENABLED
#include "AP_Proximity_Backend.h"
#define PROXIMITY_RANGEFIDER_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
@ -35,4 +37,4 @@ private:
float _distance_upward = -1; // upward distance in meters, negative if the last reading was out of range
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_RANGEFINDER_ENABLED

View File

@ -13,14 +13,14 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_SITL_ENABLED
#include "AP_Proximity_SITL.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_Param/AP_Param.h>
#include <AC_Fence/AC_Fence.h>
#include <stdio.h>
@ -135,6 +135,4 @@ bool AP_Proximity_SITL::get_upward_distance(float &distance) const
return true;
}
#endif // CONFIG_HAL_BOARD
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_SITL_ENABLED

View File

@ -1,12 +1,13 @@
#pragma once
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_SITL_ENABLED
#include "AP_Proximity.h"
#if HAL_PROXIMITY_ENABLED
#include "AP_Proximity_Backend.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#include <AC_Fence/AC_PolyFence_loader.h>
#include <AP_Common/Location.h>
class AP_Proximity_SITL : public AP_Proximity_Backend
@ -35,6 +36,5 @@ private:
bool get_distance_to_fence(float angle_deg, float &distance) const;
};
#endif // CONFIG_HAL_BOARD
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_SITL_ENABLED

View File

@ -13,9 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_TERARANGERTOWER_ENABLED
#include "AP_Proximity_TeraRangerTower.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/crc.h>
#include <ctype.h>
@ -109,4 +112,4 @@ void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_
_last_distance_received_ms = AP_HAL::millis();
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_TERARANGERTOWER_ENABLED

View File

@ -1,8 +1,11 @@
#pragma once
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_TERARANGERTOWER_ENABLED
#include "AP_Proximity_Backend_Serial.h"
#if HAL_PROXIMITY_ENABLED
#define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds
class AP_Proximity_TeraRangerTower : public AP_Proximity_Backend_Serial
@ -33,4 +36,4 @@ private:
uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_TERARANGERTOWER_ENABLED

View File

@ -13,10 +13,13 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity_TeraRangerTowerEvo.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_Math/crc.h>
#include <ctype.h>
#include <stdio.h>
@ -164,4 +167,4 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint
_last_distance_received_ms = AP_HAL::millis();
}
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED

View File

@ -1,8 +1,11 @@
#pragma once
#include "AP_Proximity_config.h"
#if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
#include "AP_Proximity.h"
#if HAL_PROXIMITY_ENABLED
#include "AP_Proximity_Backend_Serial.h"
#define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds
@ -59,4 +62,4 @@ private:
// const uint8_t REFRESH_600_HZ[5] = { (uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x03, (uint8_t)0x06, (uint8_t)0xDF};
};
#endif // HAL_PROXIMITY_ENABLED
#endif // AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED

View File

@ -1,11 +1,57 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <GCS_MAVLink/GCS_config.h>
#include <AP_RangeFinder/AP_RangeFinder_config.h>
#ifndef HAL_PROXIMITY_ENABLED
#define HAL_PROXIMITY_ENABLED (!HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024)
#endif
#ifndef AP_PROXIMITY_CYGBOT_ENABLED
#define AP_PROXIMITY_CYGBOT_ENABLED HAL_PROXIMITY_ENABLED
#ifndef AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
#define AP_PROXIMITY_BACKEND_DEFAULT_ENABLED HAL_PROXIMITY_ENABLED
#endif
#ifndef AP_PROXIMITY_AIRSIMSITL_ENABLED
#define AP_PROXIMITY_AIRSIMSITL_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#ifndef AP_PROXIMITY_CYGBOT_ENABLED
#define AP_PROXIMITY_CYGBOT_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_PROXIMITY_DRONECAN_ENABLED
#define AP_PROXIMITY_DRONECAN_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS
#endif
#ifndef AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
#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
#endif
#ifndef AP_PROXIMITY_MAV_ENABLED
#define AP_PROXIMITY_MAV_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_GCS_ENABLED
#endif
#ifndef AP_PROXIMITY_RANGEFINDER_ENABLED
#define AP_PROXIMITY_RANGEFINDER_ENABLED AP_RANGEFINDER_ENABLED && AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_PROXIMITY_RPLIDARA2_ENABLED
#define AP_PROXIMITY_RPLIDARA2_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_PROXIMITY_SITL_ENABLED
#define AP_PROXIMITY_SITL_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#ifndef AP_PROXIMITY_TERARANGERTOWER_ENABLED
#define AP_PROXIMITY_TERARANGERTOWER_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
#define AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
#endif