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

AP_Periph: Add support for up to two rangefinders

This commit is contained in:
Gone4Dirt 2024-03-28 13:21:36 +00:00 committed by Andrew Tridgell
parent dce2492321
commit 853bfbf64d
6 changed files with 155 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
Tools/AP_Periph/AP_Periph.cpp
View File

@ -227,15 +227,20 @@ void AP_Periph_FW::init()
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
if (rangefinder.get_type(0) != RangeFinder::Type::NONE) {
if (g.rangefinder_port >= 0) {
bool have_rangefinder = false;
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
if ((rangefinder.get_type(i) != RangeFinder::Type::NONE) && (g.rangefinder_port[i] >= 0)) {
// init uart for serial rangefinders
auto *uart = hal.serial(g.rangefinder_port);
auto *uart = hal.serial(g.rangefinder_port[i]);
if (uart != nullptr) {
uart->begin(g.rangefinder_baud);
serial_manager.set_protocol_and_baud(g.rangefinder_port, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud);
uart->begin(g.rangefinder_baud[i]);
serial_manager.set_protocol_and_baud(g.rangefinder_port[i], AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud[i]);
have_rangefinder = true;
}
}
}
if (have_rangefinder) {
// Can only call rangefinder init once, subsequent inits are blocked
rangefinder.init(ROTATION_NONE);
}
#endif
@ -419,7 +424,14 @@ void AP_Periph_FW::update()
hal.serial(0)->printf("BARO H=%u P=%.2f T=%.2f\n", baro.healthy(), baro.get_pressure(), baro.get_temperature());
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
hal.serial(0)->printf("RNG %u %ucm\n", rangefinder.num_sensors(), rangefinder.distance_cm_orient(ROTATION_NONE));
hal.serial(0)->printf("Num RNG sens %u\n", rangefinder.num_sensors());
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
AP_RangeFinder_Backend *backend = rangefinder.get_backend(i);
if (backend == nullptr) {
continue;
}
hal.serial(0)->printf("RNG %u %ucm\n", i, uint16_t(backend->distance()*100));
}
#endif
hal.scheduler->delay(1);
#endif

15
Tools/AP_Periph/AP_Periph.h
View File

@ -12,6 +12,7 @@
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_EFI/AP_EFI.h>
#include <AP_KDECAN/AP_KDECAN.h>
@ -168,7 +169,9 @@ public:
void send_relposheading_msg();
void can_baro_update();
void can_airspeed_update();
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
void can_rangefinder_update();
#endif
void can_battery_update();
void can_battery_send_cells(uint8_t instance);
void can_proximity_update();
@ -279,7 +282,8 @@ public:
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
RangeFinder rangefinder;
uint32_t last_sample_ms;
uint32_t last_rangefinder_update_ms;
uint32_t last_rangefinder_sample_ms[RANGEFINDER_MAX_INSTANCES];
#endif
#ifdef HAL_PERIPH_ENABLE_PROXIMITY
@ -452,6 +456,15 @@ public:
return (uint8_t(g.debug.get()) & (1U<<uint8_t(option))) != 0;
}
enum class PeriphOptions {
PROBE_CONTINUOUS = 1U<<0,
};
// check if a periph option is set
bool option_is_set(const PeriphOptions opt) const {
return (uint32_t(g.options.get()) & uint32_t(opt)) != 0;
}
// show stack as DEBUG msgs
void show_stack_free();

35
Tools/AP_Periph/Parameters.cpp
View File

@ -69,6 +69,10 @@ extern const AP_HAL::HAL &hal;
#define APD_ESC_SERIAL_1 -1
#endif
#ifndef AP_PERIPH_PROBE_CONTINUOUS
#define AP_PERIPH_PROBE_CONTINUOUS 0
#endif
/*
* AP_Periph parameter definitions
*
@ -305,7 +309,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
// @Increment: 1
// @User: Standard
// @RebootRequired: True
GSCALAR(rangefinder_baud, "RNGFND_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
GARRAY(rangefinder_baud, 0, "RNGFND_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
// @Param: RNGFND_PORT
// @DisplayName: Rangefinder Serial Port
@ -314,7 +318,27 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
// @Increment: 1
// @User: Advanced
// @RebootRequired: True
GSCALAR(rangefinder_port, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
GARRAY(rangefinder_port, 0, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
#if RANGEFINDER_MAX_INSTANCES > 1
// @Param: RNGFND2_BAUDRATE
// @DisplayName: Rangefinder serial baudrate
// @Description: Rangefinder 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
GARRAY(rangefinder_baud, 1, "RNGFND2_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
// @Param: RNGFND2_PORT
// @DisplayName: Rangefinder Serial Port
// @Description: This is the serial port number where SERIALx_PROTOCOL will be set to Rangefinder.
// @Range: 0 10
// @Increment: 1
// @User: Advanced
// @RebootRequired: True
GARRAY(rangefinder_port, 1, "RNGFND2_PORT", -1),
#endif
// @Param: RNGFND_MAX_RATE
// @DisplayName: Rangefinder max rate
@ -661,6 +685,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
GSCALAR(temperature_msg_rate, "TEMP_MSG_RATE", 0),
#endif
// @Param: OPTIONS
// @DisplayName: AP Periph Options
// @Description: Bitmask of AP Periph Options
// @Bitmask: 0: Enable continuous sensor probe
// @User: Standard
GSCALAR(options, "OPTIONS", AP_PERIPH_PROBE_CONTINUOUS),
AP_VAREND
};

13
Tools/AP_Periph/Parameters.h
View File

@ -25,7 +25,7 @@ public:
k_param_airspeed,
k_param_rangefinder,
k_param_flash_bootloader,
k_param_rangefinder_baud,
k_param_rangefinder_baud0,
k_param_adsb_baudrate,
k_param_hardpoint_id,
k_param_hardpoint_rate,
@ -36,7 +36,7 @@ public:
k_param_serial_number,
k_param_adsb_port,
k_param_servo_channels,
k_param_rangefinder_port,
k_param_rangefinder_port0,
k_param_gps_port,
k_param_msp_port,
k_param_notify,
@ -91,6 +91,9 @@ public:
k_param_serial_options,
k_param_relay,
k_param_temperature_msg_rate,
k_param_rangefinder_baud1,
k_param_rangefinder_port1,
k_param_options,
};
AP_Int16 format_version;
@ -119,8 +122,8 @@ public:
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
AP_Int32 rangefinder_baud;
AP_Int8 rangefinder_port;
AP_Int32 rangefinder_baud[RANGEFINDER_MAX_INSTANCES];
AP_Int8 rangefinder_port[RANGEFINDER_MAX_INSTANCES];
AP_Int16 rangefinder_max_rate;
#endif
@ -216,6 +219,8 @@ public:
static constexpr uint8_t can_fdmode = 0;
#endif
AP_Int32 options;
AP_Int8 can_terminate[HAL_NUM_CAN_IFACES];
AP_Int8 node_stats;

134
Tools/AP_Periph/rangefinder.cpp
View File

@ -12,6 +12,8 @@
#define AP_PERIPH_PROBE_CONTINUOUS 0
#endif
extern const AP_HAL::HAL &hal;
/*
update CAN rangefinder
*/
@ -21,7 +23,8 @@ void AP_Periph_FW::can_rangefinder_update(void)
return;
}
#if AP_PERIPH_PROBE_CONTINUOUS
if (rangefinder.num_sensors() == 0) {
// We only allow continuous probing for rangefinders while vehicle is disarmed. Probing is currently inefficient and leads to longer loop times.
if ((rangefinder.num_sensors() == 0) && !hal.util->get_soft_armed() && option_is_set(PeriphOptions::PROBE_CONTINUOUS)) {
uint32_t now = AP_HAL::millis();
static uint32_t last_probe_ms;
if (now - last_probe_ms >= 1000) {
@ -31,67 +34,88 @@ void AP_Periph_FW::can_rangefinder_update(void)
}
#endif
uint32_t now = AP_HAL::millis();
static uint32_t last_update_ms;
if (g.rangefinder_max_rate > 0 &&
now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {
now - last_rangefinder_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {
// limit to max rate
return;
}
last_update_ms = now;
// update all rangefinder instances
rangefinder.update();
RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE);
if (status <= RangeFinder::Status::NoData) {
// don't send any data
return;
}
const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE);
if (last_sample_ms == sample_ms) {
return;
}
last_sample_ms = sample_ms;
uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE);
uavcan_equipment_range_sensor_Measurement pkt {};
pkt.sensor_id = rangefinder.get_address(0);
switch (status) {
case RangeFinder::Status::OutOfRangeLow:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE;
break;
case RangeFinder::Status::OutOfRangeHigh:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;
break;
case RangeFinder::Status::Good:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;
break;
default:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED;
break;
// cycle through each rangefinder instance to find one to send
// equipment.range_sensor only uses 3 CAN frames so we just send all available sensor measurements.
for (uint8_t i = 0; i <= rangefinder.num_sensors(); i++) {
if (rangefinder.get_type(i) == RangeFinder::Type::NONE) {
continue;
}
AP_RangeFinder_Backend *backend = rangefinder.get_backend(i);
if (backend == nullptr) {
continue;
}
RangeFinder::Status status = backend->status();
if (status <= RangeFinder::Status::NoData) {
// don't send any data for this instance
continue;
}
const uint32_t sample_ms = backend->last_reading_ms();
if (last_rangefinder_sample_ms[i] == sample_ms) {
// don't same the same reading again
continue;
}
last_rangefinder_sample_ms[i] = sample_ms;
uavcan_equipment_range_sensor_Measurement pkt {};
pkt.sensor_id = rangefinder.get_address(i);
switch (status) {
case RangeFinder::Status::OutOfRangeLow:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE;
break;
case RangeFinder::Status::OutOfRangeHigh:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;
break;
case RangeFinder::Status::Good:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;
break;
default:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED;
break;
}
switch (backend->get_mav_distance_sensor_type()) {
case MAV_DISTANCE_SENSOR_LASER:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;
break;
case MAV_DISTANCE_SENSOR_ULTRASOUND:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR;
break;
case MAV_DISTANCE_SENSOR_RADAR:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR;
break;
default:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED;
break;
}
float dist_m = backend->distance();
pkt.range = dist_m;
uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
last_rangefinder_update_ms = now;
}
switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) {
case MAV_DISTANCE_SENSOR_LASER:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;
break;
case MAV_DISTANCE_SENSOR_ULTRASOUND:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR;
break;
case MAV_DISTANCE_SENSOR_RADAR:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR;
break;
default:
pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED;
break;
}
pkt.range = dist_cm * 0.01;
uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
#endif // HAL_PERIPH_ENABLE_RANGEFINDER

2
Tools/AP_Periph/serial_tunnel.cpp
View File

@ -50,7 +50,7 @@ int8_t AP_Periph_FW::get_default_tunnel_serial_port(void) const
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
if (uart_num == -1) {
uart_num = g.rangefinder_port;
uart_num = g.rangefinder_port[0];
}
#endif
#ifdef HAL_PERIPH_ENABLE_ADSB