Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: Add proximity support

This commit is contained in:
rishabsingh3003 2022-09-28 01:15:07 +05:30 committed by Andrew Tridgell
parent ea1fb96e7b
commit 27f55ae626
6 changed files with 130 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Tools/AP_Periph/AP_Periph.cpp
View File

@ -214,6 +214,17 @@ void AP_Periph_FW::init()
}
#endif
#ifdef HAL_PERIPH_ENABLE_PRX
if (proximity.get_type(0) != AP_Proximity::Type::None && g.proximity_port >= 0) {
auto *uart = hal.serial(g.proximity_port);
if (uart != nullptr) {
uart->begin(g.proximity_baud);
serial_manager.set_protocol_and_baud(g.proximity_port, AP_SerialManager::SerialProtocol_Lidar360, g.proximity_baud);
proximity.init();
}
}
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
pwm_hardpoint_init();
#endif

6
Tools/AP_Periph/AP_Periph.h
View File

@ -11,6 +11,7 @@
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_EFI/AP_EFI.h>
#include <AP_MSP/AP_MSP.h>
#include <AP_MSP/msp.h>
@ -90,6 +91,7 @@ public:
void can_airspeed_update();
void can_rangefinder_update();
void can_battery_update();
void can_proximity_update();
void load_parameters();
void prepare_reboot();
@ -188,6 +190,10 @@ public:
uint32_t last_sample_ms;
#endif
#ifdef HAL_PERIPH_ENABLE_PRX
AP_Proximity proximity;
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
void pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp);
void pwm_hardpoint_init();

34
Tools/AP_Periph/Parameters.cpp
View File

@ -490,6 +490,40 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
GOBJECT(efi, "EFI", AP_EFI),
#endif
#ifdef HAL_PERIPH_ENABLE_PRX
// @Param: PRX_BAUDRATE
// @DisplayName: Proximity Sensor serial baudrate
// @Description: Proximity Sensor serial baudrate.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,230:230400,256:256000,460:460800,500:500000,921:921600,1500:1500000
// @Increment: 1
// @User: Standard
// @RebootRequired: True
GSCALAR(proximity_baud, "PRX_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
// @Param: PRX_PORT
// @DisplayName: Proximity Sensor Serial Port
// @Description: This is the serial port number where SERIALx_PROTOCOL will be set to Proximity Sensor.
// @Range: 0 10
// @Increment: 1
// @User: Advanced
// @RebootRequired: True
GSCALAR(proximity_port, "PRX_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
// @Param: PRX_MAX_RATE
// @DisplayName: Proximity Sensor max rate
// @Description: This is the maximum rate we send Proximity Sensor data in Hz. Zero means no limit
// @Units: Hz
// @Range: 0 200
// @Increment: 1
// @User: Advanced
GSCALAR(proximity_max_rate, "PRX_MAX_RATE", 50),
// Proximity driver
// @Group: PRX
// @Path: ../libraries/AP_RangeFinder/AP_Proximity.cpp
GOBJECT(proximity, "PRX", AP_Proximity),
#endif
AP_VAREND
};

11
Tools/AP_Periph/Parameters.h
View File

@ -63,6 +63,10 @@ public:
k_param_can_slcan_cport,
k_param_temperature_sensor,
k_param_esc_command_timeout_ms,
k_param_proximity,
k_param_proximity_baud,
k_param_proximity_port,
k_param_proximity_max_rate,
};
AP_Int16 format_version;
@ -96,6 +100,13 @@ public:
AP_Int16 rangefinder_max_rate;
#endif
#ifdef HAL_PERIPH_ENABLE_PRX
AP_Int32 proximity_baud;
AP_Int8 proximity_port;
AP_Int16 proximity_max_rate;
#endif
#ifdef HAL_PERIPH_ENABLE_ADSB
AP_Int32 adsb_baudrate;
AP_Int8 adsb_port;

67
Tools/AP_Periph/can.cpp
View File

@ -1734,6 +1734,7 @@ void AP_Periph_FW::can_update()
can_baro_update();
can_airspeed_update();
can_rangefinder_update();
can_proximity_update();
#if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY)
can_buzzer_update();
#endif
@ -2333,6 +2334,72 @@ void AP_Periph_FW::can_rangefinder_update(void)
}
void AP_Periph_FW::can_proximity_update()
{
#ifdef HAL_PERIPH_ENABLE_PRX
if (proximity.get_type(0) == AP_Proximity::Type::None) {
return;
}
uint32_t now = AP_HAL::native_millis();
static uint32_t last_update_ms;
if (g.proximity_max_rate > 0 &&
now - last_update_ms < 1000/g.proximity_max_rate) {
// limit to max rate
return;
}
last_update_ms = now;
proximity.update();
AP_Proximity::Status status = proximity.get_status();
if (status <= AP_Proximity::Status::NoData) {
// don't send any data
return;
}
ardupilot_equipment_proximity_sensor_Proximity pkt {};
const uint8_t obstacle_count = proximity.get_obstacle_count();
// if no objects return
if (obstacle_count == 0) {
return;
}
// calculate maximum roll, pitch values from objects
for (uint8_t i=0; i<obstacle_count; i++) {
if (!proximity.get_obstacle_info(i, pkt.yaw, pkt.pitch, pkt.distance)) {
// not a valid obstacle
continue;
}
pkt.sensor_id = proximity.get_address(0);
switch (status) {
case AP_Proximity::Status::NotConnected:
pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED;
break;
case AP_Proximity::Status::Good:
pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD;
break;
case AP_Proximity::Status::NoData:
default:
pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA;
break;
}
uint8_t buffer[ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_MAX_SIZE] {};
uint16_t total_size = ardupilot_equipment_proximity_sensor_Proximity_encode(&pkt, buffer, !periph.canfdout());
canard_broadcast(ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_SIGNATURE,
ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
#endif
}
#ifdef HAL_PERIPH_ENABLE_ADSB
/*
map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message

1
Tools/AP_Periph/wscript
View File

@ -67,6 +67,7 @@ def build(bld):
'AP_Stats',
'AP_EFI',
'AP_CheckFirmware',
'AP_Proximity',
]
bld.ap_stlib(
name= 'AP_Periph_libs',