mirror of https://github.com/ArduPilot/ardupilot
122 lines
4.1 KiB
C++
122 lines
4.1 KiB
C++
#include "AP_Periph.h"
|
|
|
|
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
|
|
|
|
/*
|
|
rangefinder support
|
|
*/
|
|
|
|
#include <dronecan_msgs.h>
|
|
|
|
#ifndef AP_PERIPH_PROBE_CONTINUOUS
|
|
#define AP_PERIPH_PROBE_CONTINUOUS 0
|
|
#endif
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
/*
|
|
update CAN rangefinder
|
|
*/
|
|
void AP_Periph_FW::can_rangefinder_update(void)
|
|
{
|
|
if (rangefinder.get_type(0) == RangeFinder::Type::NONE) {
|
|
return;
|
|
}
|
|
#if AP_PERIPH_PROBE_CONTINUOUS
|
|
// 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) {
|
|
last_probe_ms = now;
|
|
rangefinder.init(ROTATION_NONE);
|
|
}
|
|
}
|
|
#endif
|
|
uint32_t now = AP_HAL::millis();
|
|
if (g.rangefinder_max_rate > 0 &&
|
|
now - last_rangefinder_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {
|
|
// limit to max rate
|
|
return;
|
|
}
|
|
last_rangefinder_update_ms = now;
|
|
|
|
// update all rangefinder instances
|
|
rangefinder.update();
|
|
|
|
// 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);
|
|
|
|
}
|
|
}
|
|
|
|
#endif // HAL_PERIPH_ENABLE_RANGEFINDER
|