diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp new file mode 100644 index 0000000000..2abfcbef73 --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp @@ -0,0 +1,109 @@ +#include + +#if HAL_WITH_UAVCAN + +#include + +#include "AP_OpticalFlow_HereFlow.h" + +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +#define debug_flow_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { hal.console->printf(fmt, ##args); }} while (0) + +//UAVCAN Frontend Registry Binder +UC_REGISTRY_BINDER(MeasurementCb, com::hex::equipment::flow::Measurement); + +uint8_t AP_OpticalFlow_HereFlow::_node_id = 0; +AP_OpticalFlow_HereFlow* AP_OpticalFlow_HereFlow::_driver = nullptr; +AP_UAVCAN* AP_OpticalFlow_HereFlow::_ap_uavcan = nullptr; +/* + constructor - registers instance at top Flow driver + */ +AP_OpticalFlow_HereFlow::AP_OpticalFlow_HereFlow(OpticalFlow &flow) : + OpticalFlow_backend(flow) +{ + if (_driver) { + AP_HAL::panic("Only one instance of Flow supported!"); + } + _driver = this; +} + +//links the HereFlow messages to the backend +void AP_OpticalFlow_HereFlow::subscribe_msgs(AP_UAVCAN* ap_uavcan) +{ + if (ap_uavcan == nullptr) { + return; + } + + auto* node = ap_uavcan->get_node(); + + uavcan::Subscriber *measurement_listener; + measurement_listener = new uavcan::Subscriber(*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 +void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb) +{ + if (_driver == nullptr) { + return; + } + //protect from data coming from duplicate sensors, + //as we only handle one Here Flow at a time as of now + if (_ap_uavcan == nullptr) { + _ap_uavcan = ap_uavcan; + _node_id = node_id; + } + + if (_ap_uavcan == ap_uavcan && _node_id == node_id) { + WITH_SEMAPHORE(_driver->_sem); + _driver->new_data = true; + _driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]); + _driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]); + _driver->integral_time = cb.msg->integration_interval; + _driver->surface_quality = cb.msg->quality; + } +} + +void AP_OpticalFlow_HereFlow::update() +{ + _push_state(); +} + +// Read the sensor +void AP_OpticalFlow_HereFlow::_push_state(void) +{ + WITH_SEMAPHORE(_sem); + if (!new_data) { + return; + } + struct OpticalFlow::OpticalFlow_state state; + const Vector2f flowScaler = _flowScaler(); + //setup scaling based on parameters + float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; + float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; + float integralToRate = 1.0f / integral_time; + //Convert to Raw Flow measurement to Flow Rate measurement + state.flowRate = Vector2f(flowRate.x * flowScaleFactorX, + flowRate.y * flowScaleFactorY) * integralToRate; + state.bodyRate = bodyRate * integralToRate; + state.surface_quality = surface_quality; + _applyYaw(state.flowRate); + _applyYaw(state.bodyRate); + // hal.console->printf("DRV: %u %f %f\n", state.surface_quality, flowRate.length(), bodyRate.length()); + _update_frontend(state); + new_data = false; +} + +#endif // HAL_WITH_UAVCAN + diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h new file mode 100644 index 0000000000..3777f38c20 --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h @@ -0,0 +1,35 @@ +#pragma once +#include "OpticalFlow_backend.h" + +#if HAL_WITH_UAVCAN + +#include + +class MeasurementCb; + +class AP_OpticalFlow_HereFlow : public OpticalFlow_backend { +public: + AP_OpticalFlow_HereFlow(OpticalFlow &flow); + + void init() override {} + + void update() override; + + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + + static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb); + +private: + + Vector2f flowRate, bodyRate; + uint8_t surface_quality; + float integral_time; + bool new_data; + static uint8_t _node_id; + + static AP_OpticalFlow_HereFlow* _driver; + static AP_UAVCAN* _ap_uavcan; + void _push_state(void); + +}; +#endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index 5a828bf10f..2036a40f6e 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -6,6 +6,7 @@ #include "AP_OpticalFlow_PX4Flow.h" #include "AP_OpticalFlow_CXOF.h" #include "AP_OpticalFlow_MAV.h" +#include "AP_OpticalFlow_HereFlow.h" #include extern const AP_HAL::HAL& hal; @@ -24,7 +25,7 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = { // @Param: _TYPE // @DisplayName: Optical flow sensor type // @Description: Optical flow sensor type - // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink + // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:UAVCAN // @User: Standard // @RebootRequired: True AP_GROUPINFO("_TYPE", 0, OpticalFlow, _type, (int8_t)OPTICAL_FLOW_TYPE_DEFAULT), @@ -132,6 +133,11 @@ void OpticalFlow::init(uint32_t log_bit) case OpticalFlowType::MAVLINK: backend = AP_OpticalFlow_MAV::detect(*this); break; + case OpticalFlowType::UAVCAN: +#if HAL_WITH_UAVCAN + backend = new AP_OpticalFlow_HereFlow(*this); +#endif + break; case OpticalFlowType::SITL: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL backend = new AP_OpticalFlow_SITL(*this); @@ -150,7 +156,6 @@ void OpticalFlow::update(void) if (!enabled()) { return; } - if (backend != nullptr) { backend->update(); } diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h index 471733f959..796e23e0ad 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.h +++ b/libraries/AP_OpticalFlow/OpticalFlow.h @@ -49,6 +49,7 @@ public: BEBOP = 3, CXOF = 4, MAVLINK = 5, + UAVCAN = 6, SITL = 10 };