mirror of https://github.com/ArduPilot/ardupilot
98 lines
3.1 KiB
C++
98 lines
3.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
|
||
|
|
|
||
|
|
/*
|
||
|
|
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
|
||
|
|
if (rangefinder.num_sensors() == 0) {
|
||
|
|
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();
|
||
|
|
static uint32_t last_update_ms;
|
||
|
|
if (g.rangefinder_max_rate > 0 &&
|
||
|
|
now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {
|
||
|
|
// limit to max rate
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
last_update_ms = now;
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
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
|