Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Opticalflow: replace libuavcan with libcanard based driver

Browse Source
This commit is contained in:
bugobliterator 2023-01-04 23:22:18 +11:00 committed by Andrew Tridgell
parent 6a1460efb0
commit bcecda5c3f
2 changed files with 11 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
View File

@ -6,14 +6,10 @@
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <com/hex/equipment/flow/Measurement.hpp>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
//UAVCAN Frontend Registry Binder
UC_REGISTRY_BINDER(MeasurementCb, com::hex::equipment::flow::Measurement);
uint8_t AP_OpticalFlow_HereFlow::_node_id = 0; uint8_t AP_OpticalFlow_HereFlow::_node_id = 0;
AP_OpticalFlow_HereFlow* AP_OpticalFlow_HereFlow::_driver = nullptr; AP_OpticalFlow_HereFlow* AP_OpticalFlow_HereFlow::_driver = nullptr;
AP_UAVCAN* AP_OpticalFlow_HereFlow::_ap_uavcan = nullptr; AP_UAVCAN* AP_OpticalFlow_HereFlow::_ap_uavcan = nullptr;
@ -36,20 +32,13 @@ void AP_OpticalFlow_HereFlow::subscribe_msgs(AP_UAVCAN* ap_uavcan)
return; return;
} }
auto* node = ap_uavcan->get_node(); if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_measurement, ap_uavcan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("measurement_sub");
uavcan::Subscriber<com::hex::equipment::flow::Measurement, MeasurementCb> *measurement_listener;
measurement_listener = new uavcan::Subscriber<com::hex::equipment::flow::Measurement, MeasurementCb>(*node);
// Register method to handle incoming HereFlow measurement
const int measurement_listener_res = measurement_listener->start(MeasurementCb(ap_uavcan, &handle_measurement));
if (measurement_listener_res < 0) {
AP_HAL::panic("UAVCAN Flow subscriber start problem\n\r");
return;
} }
} }
//updates driver states based on received HereFlow messages //updates driver states based on received HereFlow messages
void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb) void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const com_hex_equipment_flow_Measurement &msg)
{ {
if (_driver == nullptr) { if (_driver == nullptr) {
return; return;
@ -58,16 +47,16 @@ void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t n
//as we only handle one Here Flow at a time as of now //as we only handle one Here Flow at a time as of now
if (_ap_uavcan == nullptr) { if (_ap_uavcan == nullptr) {
_ap_uavcan = ap_uavcan; _ap_uavcan = ap_uavcan;
_node_id = node_id; _node_id = transfer.source_node_id;
} }
if (_ap_uavcan == ap_uavcan && _node_id == node_id) { if (_ap_uavcan == ap_uavcan && _node_id == transfer.source_node_id) {
WITH_SEMAPHORE(_driver->_sem); WITH_SEMAPHORE(_driver->_sem);
_driver->new_data = true; _driver->new_data = true;
_driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]); _driver->flowRate = Vector2f(msg.flow_integral[0], msg.flow_integral[1]);
_driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]); _driver->bodyRate = Vector2f(msg.rate_gyro_integral[0], msg.rate_gyro_integral[1]);
_driver->integral_time = cb.msg->integration_interval; _driver->integral_time = msg.integration_interval;
_driver->surface_quality = cb.msg->quality; _driver->surface_quality = msg.quality;
} }
} }

4
libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h
View File

@ -10,8 +10,6 @@
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
class MeasurementCb;
class AP_OpticalFlow_HereFlow : public OpticalFlow_backend { class AP_OpticalFlow_HereFlow : public OpticalFlow_backend {
public: public:
AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow); AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow);
@ -22,7 +20,7 @@ public:
static void subscribe_msgs(AP_UAVCAN* ap_uavcan); static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb); static void handle_measurement(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const com_hex_equipment_flow_Measurement &msg);
private: private: