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

AP_Airspeed: use DEVID to maintain lineup of CAN sensors 

persist DEVID and use it to ensure that we keep the order of DroneCAN
sensors between boots. It still allows for a sensor to be swapped out
for a new one, while keeping slot of the one that hasn't been removed
This commit is contained in:
Andrew Tridgell 2022-10-07 09:28:48 +11:00
parent ab7582af7c
commit cc4a7993bc
5 changed files with 50 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -464,7 +464,7 @@ void AP_Airspeed::init()
break;
case TYPE_UAVCAN:
#if AP_AIRSPEED_UAVCAN_ENABLED
sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i);
sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i, uint32_t(param[i].bus_id.get()));
#endif
break;
case TYPE_NMEA_WATER:
@ -489,7 +489,30 @@ void AP_Airspeed::init()
num_sensors = i+1;
}
}
#if AP_AIRSPEED_UAVCAN_ENABLED
// we need a 2nd pass for DroneCAN sensors so we can match order by DEVID
// the 2nd pass accepts any devid
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (sensor[i] == nullptr && (enum airspeed_type)param[i].type.get() == TYPE_UAVCAN) {
sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i, 0);
if (sensor[i] != nullptr) {
num_sensors = i+1;
}
}
}
#endif // AP_AIRSPEED_UAVCAN_ENABLED
#endif // HAL_AIRSPEED_PROBE_LIST
// set DEVID to zero for any sensors not found. This allows backends to order
// based on previous value of DEVID. This allows for swapping out sensors
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (sensor[i] == nullptr) {
// note we use set() not set_and_save() to allow a sensor to be temporarily
// removed for one boot without losing its slot
param[i].bus_id.set(0);
}
}
}
// read the airspeed sensor

5
libraries/AP_Airspeed/AP_Airspeed_Backend.cpp
View File

@ -54,3 +54,8 @@ bool AP_Airspeed_Backend::bus_is_confgured(void) const
{
return frontend.param[instance].bus.configured();
}
void AP_Airspeed_Backend::set_bus_id(uint32_t id)
{
frontend.param[instance].bus_id.set_and_save(int32_t(id));
}

4
libraries/AP_Airspeed/AP_Airspeed_Backend.h
View File

@ -91,9 +91,7 @@ protected:
}
// set bus ID of this instance, for ARSPD_DEVID parameters
void set_bus_id(uint32_t id) {
frontend.param[instance].bus_id.set(int32_t(id));
}
void set_bus_id(uint32_t id);
enum class DevType {
SITL = 0x01,

33
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp
View File

@ -39,7 +39,7 @@ void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
}
}
AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance)
AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid)
{
WITH_SEMAPHORE(_sem_registry);
@ -47,23 +47,28 @@ AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _
for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) {
const auto bus_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
_detected_modules[i].ap_uavcan->get_driver_index(),
_detected_modules[i].node_id, 0);
if (previous_devid != 0 && previous_devid != bus_id) {
// match with previous ID only
continue;
}
backend = new AP_Airspeed_UAVCAN(_frontend, _instance);
if (backend == nullptr) {
AP::can().log_text(AP_CANManager::LOG_INFO,
LOG_TAG,
"Failed register UAVCAN Airspeed Node %d on Bus %d\n",
_detected_modules[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index());
AP::can().log_text(AP_CANManager::LOG_INFO,
LOG_TAG,
"Failed register UAVCAN Airspeed Node %d on Bus %d\n",
_detected_modules[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index());
} else {
_detected_modules[i].driver = backend;
AP::can().log_text(AP_CANManager::LOG_INFO,
LOG_TAG,
"Registered UAVCAN Airspeed Node %d on Bus %d\n",
_detected_modules[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index());
backend->set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
_detected_modules[i].ap_uavcan->get_driver_index(),
_detected_modules[i].node_id, 0));
AP::can().log_text(AP_CANManager::LOG_INFO,
LOG_TAG,
"Registered UAVCAN Airspeed Node %d on Bus %d\n",
_detected_modules[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index());
backend->set_bus_id(bus_id);
}
break;
}

2
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h
View File

@ -28,7 +28,7 @@ public:
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance);
static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance, uint32_t previous_devid);
private: