ardupilot/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp

90 lines
2.8 KiB
C++
Raw Normal View History

2023-08-01 13:07:59 -03:00
#include "AP_RangeFinder_config.h"
#if AP_RANGEFINDER_NRA24_CAN_ENABLED
2023-08-01 13:07:59 -03:00
#include "AP_RangeFinder_NRA24_CAN.h"
2023-07-19 05:00:46 -03:00
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/AP_HAL.h>
RangeFinder_MultiCAN *AP_RangeFinder_NRA24_CAN::multican_NRA24;
// constructor
AP_RangeFinder_NRA24_CAN::AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params)
{
if (multican_NRA24 == nullptr) {
multican_NRA24 = new RangeFinder_MultiCAN(AP_CAN::Protocol::NanoRadar_NRA24, "NRA24 MultiCAN");
if (multican_NRA24 == nullptr) {
AP_BoardConfig::allocation_error("Rangefinder_MultiCAN");
}
}
{
// add to linked list of drivers
WITH_SEMAPHORE(multican_NRA24->sem);
auto *prev = multican_NRA24->drivers;
next = prev;
multican_NRA24->drivers = this;
}
}
// update the state of the sensor
void AP_RangeFinder_NRA24_CAN::update(void)
{
WITH_SEMAPHORE(_sem);
if (get_reading(state.distance_m)) {
// update range_valid state based on distance measured
state.last_reading_ms = AP_HAL::millis();
update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
if (AP_HAL::millis() - last_heartbeat_ms > read_timeout_ms()) {
// no heartbeat, must be disconnected
set_status(RangeFinder::Status::NotConnected);
} else {
// Have heartbeat, just no data. Probably because this sensor doesn't output data when there is no relative motion infront of the radar.
// This case has special pre-arm check handling
set_status(RangeFinder::Status::NoData);
}
}
}
// handler for incoming frames
bool AP_RangeFinder_NRA24_CAN::handle_frame(AP_HAL::CANFrame &frame)
{
WITH_SEMAPHORE(_sem);
const uint32_t id = frame.id;
if (!is_correct_id(get_radar_id(id))) {
return false;
}
switch (id & 0xFU) {
case 0xAU:
// heart beat in the form of Radar Status. The contents of this message aren't really useful so we won't parse them for now
last_heartbeat_ms = AP_HAL::millis();
break;
case 0xCU:
{
// Target Information
const float dist_m = (frame.data[2] * 0x100U + frame.data[3]) * 0.01;
const uint8_t snr = frame.data[7] - 128;
if ((snr_min != 0 && snr < uint16_t(snr_min.get()))) {
// too low signal strength
return false;
}
2023-07-19 05:00:46 -03:00
accumulate_distance_m(dist_m);
}
break;
default:
// not parsing these messages
break;
}
return true;
}
#endif // AP_RANGEFINDER_NRA24_CAN_ENABLED