From 16d3f536a716b69a7cf85d47133a29335464dac3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 9 May 2022 20:18:10 -0400 Subject: [PATCH] WIP: optical_flow sensor & vehicle --- msg/CMakeLists.txt | 3 +- ...tical_flow.msg => sensor_optical_flow.msg} | 8 ++ msg/vehicle_optical_flow.msg | 29 +++++ src/drivers/optical_flow/paa3905/PAA3905.cpp | 8 +- src/drivers/optical_flow/paa3905/PAA3905.hpp | 4 +- src/drivers/optical_flow/paw3902/PAW3902.cpp | 10 +- src/drivers/optical_flow/paw3902/PAW3902.hpp | 4 +- src/drivers/optical_flow/pmw3901/PMW3901.cpp | 2 +- src/drivers/optical_flow/pmw3901/PMW3901.hpp | 4 +- src/drivers/optical_flow/px4flow/px4flow.cpp | 6 +- .../optical_flow/thoneflow/thoneflow.cpp | 8 +- .../thoneflow/thoneflow_parser.cpp | 2 +- .../optical_flow/thoneflow/thoneflow_parser.h | 4 +- src/drivers/uavcan/sensors/flow.cpp | 4 +- src/drivers/uavcan/sensors/flow.hpp | 2 +- .../uavcannode/Publishers/FlowMeasurement.hpp | 4 +- src/modules/ekf2/EKF2.cpp | 8 +- src/modules/ekf2/EKF2.hpp | 4 +- .../BlockLocalPositionEstimator.hpp | 4 +- src/modules/mavlink/mavlink_receiver.cpp | 6 +- src/modules/mavlink/mavlink_receiver.h | 6 +- .../mavlink/streams/OPTICAL_FLOW_RAD.hpp | 6 +- src/modules/replay/ReplayEkf2.cpp | 4 +- src/modules/sensors/CMakeLists.txt | 2 + src/modules/sensors/sensors.cpp | 6 + .../vehicle_optical_flow/CMakeLists.txt | 38 +++++++ .../VehicleOpticalFlow.cpp | 104 ++++++++++++++++++ .../VehicleOpticalFlow.hpp | 96 ++++++++++++++++ src/modules/simulator/simulator.h | 4 +- src/modules/simulator/simulator_mavlink.cpp | 2 +- 30 files changed, 338 insertions(+), 54 deletions(-) rename msg/{optical_flow.msg => sensor_optical_flow.msg} (99%) create mode 100644 msg/vehicle_optical_flow.msg create mode 100644 src/modules/sensors/vehicle_optical_flow/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp create mode 100644 src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 423dc6130b..91afabc4e7 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -118,7 +118,8 @@ set(msg_files obstacle_distance.msg offboard_control_mode.msg onboard_computer_status.msg - optical_flow.msg + sensor_optical_flow.msg + vehicle_optical_flow.msg orbit_status.msg parameter_update.msg ping.msg diff --git a/msg/optical_flow.msg b/msg/sensor_optical_flow.msg similarity index 99% rename from msg/optical_flow.msg rename to msg/sensor_optical_flow.msg index 4b3796ff9f..a18153c8e4 100644 --- a/msg/optical_flow.msg +++ b/msg/sensor_optical_flow.msg @@ -4,16 +4,24 @@ uint64 timestamp # time since system start (microseconds) uint8 sensor_id # id of the sensor emitting the flow value + float32 pixel_flow_x_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis float32 pixel_flow_y_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis + float32 gyro_x_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data. float32 gyro_y_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data. float32 gyro_z_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data. + float32 ground_distance_m # Altitude / distance to ground in meters + uint32 integration_timespan # accumulation timespan in microseconds + uint32 time_since_last_sonar_update # time since last sonar update in microseconds + uint16 frame_count_since_last_readout # number of accumulated frames in timespan + int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius + uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably diff --git a/msg/vehicle_optical_flow.msg b/msg/vehicle_optical_flow.msg new file mode 100644 index 0000000000..e07665144d --- /dev/null +++ b/msg/vehicle_optical_flow.msg @@ -0,0 +1,29 @@ +# Optical flow in XYZ body frame in SI units. +# http://en.wikipedia.org/wiki/International_System_of_Units + +uint64 timestamp # time since system start (microseconds) + +uint8 sensor_id # id of the sensor emitting the flow value + +float32 pixel_flow_x_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis +float32 pixel_flow_y_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis + +float32 gyro_x_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data. +float32 gyro_y_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data. +float32 gyro_z_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data. + +float32 ground_distance_m # Altitude / distance to ground in meters + +uint32 integration_timespan # accumulation timespan in microseconds + +uint32 time_since_last_sonar_update # time since last sonar update in microseconds + +uint16 frame_count_since_last_readout # number of accumulated frames in timespan + +int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius + +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + +float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably +float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index a9a306d803..299d14eff8 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -535,7 +535,7 @@ void PAA3905::RunImpl() return; } - optical_flow_s report{}; + sensor_optical_flow_s report{}; report.timestamp = timestamp_sample; //report.device_id = get_device_id(); @@ -565,15 +565,15 @@ void PAA3905::RunImpl() switch (_mode) { case Mode::Bright: - report.mode = optical_flow_s::MODE_BRIGHT; + report.mode = sensor_optical_flow_s::MODE_BRIGHT; break; case Mode::LowLight: - report.mode = optical_flow_s::MODE_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; break; case Mode::SuperLowLight: - report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; break; } diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index dd15f548aa..f6668d378c 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include using namespace time_literals; using namespace PixArt_PAA3905; @@ -99,7 +99,7 @@ private: void ResetAccumulatedData(); - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index ea3b66d7ae..624b7f5bdd 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -788,7 +788,7 @@ void PAW3902::RunImpl() return; } - optical_flow_s report{}; + sensor_optical_flow_s report{}; report.timestamp = timestamp_sample; //report.device_id = get_device_id(); @@ -818,15 +818,15 @@ void PAW3902::RunImpl() switch (_mode) { case Mode::Bright: - report.mode = optical_flow_s::MODE_BRIGHT; + report.mode = sensor_optical_flow_s::MODE_BRIGHT; break; case Mode::LowLight: - report.mode = optical_flow_s::MODE_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; break; case Mode::SuperLowLight: - report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; break; } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index ab8d574f3d..b006147635 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include using namespace time_literals; using namespace PixArt_PAW3902JF; @@ -97,7 +97,7 @@ private: void ResetAccumulatedData(); - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp index 78809cf554..722a442f10 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -326,7 +326,7 @@ PMW3901::RunImpl() delta_x = (float)_flow_sum_x / 385.0f; // proportional factor + convert from pixels to radians delta_y = (float)_flow_sum_y / 385.0f; // proportional factor + convert from pixels to radians - optical_flow_s report{}; + sensor_optical_flow_s report{}; report.timestamp = timestamp; report.pixel_flow_x_integral = static_cast(delta_x); diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp index 13d850a66b..5266f5d1c8 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.hpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include /* Configuration Constants */ @@ -84,7 +84,7 @@ private: const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz) - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index c1c59ed7a9..e1a1f94f1d 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include /* Configuration Constants */ #define I2C_FLOW_ADDRESS_DEFAULT 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 @@ -98,7 +98,7 @@ private: bool _sensor_ok{false}; bool _collect_phase{false}; - uORB::PublicationMulti _px4flow_topic{ORB_ID(optical_flow)}; + uORB::PublicationMulti _px4flow_topic{ORB_ID(sensor_optical_flow)}; uORB::PublicationMulti _distance_sensor_topic{ORB_ID(distance_sensor)}; perf_counter_t _sample_perf; @@ -279,7 +279,7 @@ PX4FLOW::collect() } - optical_flow_s report{}; + sensor_optical_flow_s report{}; report.timestamp = hrt_absolute_time(); report.pixel_flow_x_integral = static_cast(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians diff --git a/src/drivers/optical_flow/thoneflow/thoneflow.cpp b/src/drivers/optical_flow/thoneflow/thoneflow.cpp index 743e809baf..d51c621bd4 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow.cpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include "thoneflow_parser.h" @@ -81,7 +81,7 @@ private: hrt_abstime _last_read; - optical_flow_s _report; + sensor_optical_flow_s _report; orb_advert_t _optical_flow_pub; perf_counter_t _sample_perf; @@ -233,7 +233,7 @@ Thoneflow::init() _report.integration_timespan = 10526; // microseconds /* Get a publish handle on the optical flow topic */ - _optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report); + _optical_flow_pub = orb_advertise(ORB_ID(sensor_optical_flow), &_report); if (_optical_flow_pub == nullptr) { PX4_ERR("Failed to create optical_flow object"); @@ -312,7 +312,7 @@ Thoneflow::collect() float zeroval = 0.0f; rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval); - orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report); + orb_publish(ORB_ID(sensor_optical_flow), _optical_flow_pub, &_report); } /* Bytes left to parse */ diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp index 28cc5a5693..745ad0542a 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp @@ -61,7 +61,7 @@ const char *parser_state[] = { #endif bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *flow) + sensor_optical_flow_s *flow) { bool parsed_packet = false; diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h index fd27b8c355..d63906fdc6 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h @@ -33,7 +33,7 @@ #pragma once -#include +#include // Data Format for ThoneFlow 3901U // =============================== @@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE { }; bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *report); + sensor_optical_flow_s *report); diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index f9cef4c551..4a0f6e408c 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -38,7 +38,7 @@ const char *const UavcanFlowBridge::NAME = "flow"; UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) : - UavcanSensorBridgeBase("uavcan_flow", ORB_ID(optical_flow)), + UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow)), _sub_flow(node) { } @@ -58,7 +58,7 @@ UavcanFlowBridge::init() void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure &msg) { - optical_flow_s flow{}; + sensor_optical_flow_s flow{}; // We're only given an 8 bit field for sensor ID; just use the UAVCAN node ID flow.sensor_id = msg.getSrcNodeID().get(); diff --git a/src/drivers/uavcan/sensors/flow.hpp b/src/drivers/uavcan/sensors/flow.hpp index 94aec78a0e..3a5484269e 100644 --- a/src/drivers/uavcan/sensors/flow.hpp +++ b/src/drivers/uavcan/sensors/flow.hpp @@ -37,7 +37,7 @@ #include -#include +#include #include diff --git a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp index 7780bd85f0..fbbac48edf 100644 --- a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp @@ -52,7 +52,7 @@ class FlowMeasurement : public: FlowMeasurement(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_optical_flow)), uavcan::Publisher(node) { _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, 0.f}}; @@ -80,7 +80,7 @@ public: void BroadcastAnyUpdates() override { // optical_flow -> com::hex::equipment::flow::Measurement - optical_flow_s optical_flow; + sensor_optical_flow_s optical_flow; if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) { com::hex::equipment::flow::Measurement measurement{}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 166a08f1af..4c530e4b03 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1680,14 +1680,14 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF flow sample bool new_optical_flow = false; - const unsigned last_generation = _optical_flow_sub.get_last_generation(); - optical_flow_s optical_flow; + const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation(); + vehicle_optical_flow_s optical_flow; - if (_optical_flow_sub.update(&optical_flow)) { + if (_vehicle_optical_flow_sub.update(&optical_flow)) { if (_msg_missed_optical_flow_perf == nullptr) { _msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed"); - } else if (_optical_flow_sub.get_last_generation() != last_generation + 1) { + } else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) { perf_count(_msg_missed_optical_flow_perf); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index fe7fbdca63..0e933b4168 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include #include @@ -91,6 +90,7 @@ #include #include #include +#include #include #include #include @@ -252,12 +252,12 @@ private: uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; - uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 196d6bcebb..12f169360a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -270,7 +270,7 @@ private: uORB::SubscriptionData _sub_land{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionData _sub_att{ORB_ID(vehicle_attitude)}; uORB::SubscriptionData _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)}; - uORB::SubscriptionData _sub_flow{ORB_ID(optical_flow)}; + uORB::SubscriptionData _sub_flow{ORB_ID(vehicle_optical_flow)}; uORB::SubscriptionData _sub_gps{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionData _sub_visual_odom{ORB_ID(vehicle_visual_odometry)}; uORB::SubscriptionData _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 84471a2287..6b5d765582 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -799,7 +799,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); - optical_flow_s f{}; + sensor_optical_flow_s f{}; f.timestamp = hrt_absolute_time(); f.time_since_last_sonar_update = flow.time_delta_distance_us; @@ -858,7 +858,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) mavlink_hil_optical_flow_t flow; mavlink_msg_hil_optical_flow_decode(msg, &flow); - optical_flow_s f{}; + sensor_optical_flow_s f{}; f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; f.integration_timespan = flow.integration_time_us; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index aae57f257e..548ce1bec8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -86,7 +86,7 @@ #include #include #include -#include +#include #include #include #include @@ -307,7 +307,7 @@ private: uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; - uORB::Publication _flow_pub{ORB_ID(optical_flow)}; + uORB::Publication _flow_pub{ORB_ID(sensor_optical_flow)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)}; diff --git a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp index ebbe35ea1f..d5f14cbd28 100644 --- a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp +++ b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -34,7 +34,7 @@ #ifndef OPTICAL_FLOW_RAD_HPP #define OPTICAL_FLOW_RAD_HPP -#include +#include class MavlinkStreamOpticalFlowRad : public MavlinkStream { @@ -55,11 +55,11 @@ public: private: explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; + uORB::Subscription _optical_flow_sub{ORB_ID(vehicle_optical_flow)}; bool send() override { - optical_flow_s flow; + vehicle_optical_flow_s flow; if (_optical_flow_sub.update(&flow)) { mavlink_optical_flow_rad_t msg{}; diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 941718323b..c86e6d5f05 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include #include @@ -91,7 +91,7 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(distance_sensor)) { _distance_sensor_msg_id = msg_id; - } else if (sub.orb_meta == ORB_ID(optical_flow)) { + } else if (sub.orb_meta == ORB_ID(vehicle_optical_flow)) { _optical_flow_msg_id = msg_id; } else if (sub.orb_meta == ORB_ID(vehicle_air_data)) { diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0eb8a7e0a9..46222e712c 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -40,6 +40,7 @@ add_subdirectory(vehicle_air_data) add_subdirectory(vehicle_gps_position) add_subdirectory(vehicle_imu) add_subdirectory(vehicle_magnetometer) +add_subdirectory(vehicle_optical_flow) px4_add_module( MODULE modules__sensors @@ -62,4 +63,5 @@ px4_add_module( vehicle_gps_position vehicle_imu vehicle_magnetometer + vehicle_optical_flow ) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 30ba6aeaac..4003ea46be 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -78,6 +78,7 @@ #include "vehicle_gps_position/VehicleGPSPosition.hpp" #include "vehicle_imu/VehicleIMU.hpp" #include "vehicle_magnetometer/VehicleMagnetometer.hpp" +#include "vehicle_optical_flow/VehicleOpticalFlow.hpp" using namespace sensors; using namespace time_literals; @@ -181,6 +182,7 @@ private: VehicleAirData *_vehicle_air_data{nullptr}; VehicleMagnetometer *_vehicle_magnetometer{nullptr}; VehicleGPSPosition *_vehicle_gps_position{nullptr}; + VehicleOpticalFlow _vehicle_optical_flow; VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; @@ -264,6 +266,7 @@ Sensors::Sensors(bool hil_enabled) : _airspeed_validator.set_equal_value_threshold(100); parameters_update(); + _vehicle_optical_flow.Start(); InitializeVehicleAirData(); InitializeVehicleGPSPosition(); @@ -280,6 +283,7 @@ Sensors::~Sensors() _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); + _vehicle_optical_flow.Stop(); if (_vehicle_air_data) { _vehicle_air_data->Stop(); @@ -800,6 +804,8 @@ int Sensors::print_status() } } + _vehicle_optical_flow.PrintStatus(); + return 0; } diff --git a/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt new file mode 100644 index 0000000000..ba8761aa00 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(vehicle_optical_flow + VehicleOpticalFlow.cpp + VehicleOpticalFlow.hpp +) +target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp new file mode 100644 index 0000000000..9163250f22 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "VehicleOpticalFlow.hpp" + +#include + +namespace sensors +{ + +using namespace matrix; +using namespace time_literals; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +VehicleOpticalFlow::VehicleOpticalFlow() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ +} + +VehicleOpticalFlow::~VehicleOpticalFlow() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleOpticalFlow::Start() +{ + ScheduleNow(); + return true; +} + +void VehicleOpticalFlow::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } +} + +void VehicleOpticalFlow::ParametersUpdate() +{ + // Check if parameters have changed + if (_params_sub.updated()) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + } +} + +void VehicleOpticalFlow::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + + // reschedule timeout + ScheduleDelayed(100_ms); + + perf_end(_cycle_perf); +} + +void VehicleOpticalFlow::PrintStatus() +{ + +} + +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp new file mode 100644 index 0000000000..d3c164897b --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "data_validator/DataValidatorGroup.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sensors +{ +class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + VehicleOpticalFlow(); + ~VehicleOpticalFlow() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(); + void SensorCorrectionsUpdate(bool force = false); + + static constexpr int MAX_SENSOR_COUNT = 3; + + uORB::Publication _vehicle_optical_flowpub{ORB_ID(vehicle_optical_flow)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { + {this, ORB_ID(sensor_optical_flow), 0}, + {this, ORB_ID(sensor_optical_flow), 1}, + {this, ORB_ID(sensor_optical_flow), 2} + }; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + bool _advertised[MAX_SENSOR_COUNT] {}; + + uint8_t _priority[MAX_SENSOR_COUNT] {}; + + int8_t _selected_sensor_sub_index{-1}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sens_baro_qnh + ) +}; +}; // namespace sensors diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 8cdd807672..6ce1f589bc 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -61,7 +61,7 @@ #include #include #include -#include +#include #include #include #include @@ -188,7 +188,7 @@ private: // uORB publisher handlers uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; - uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _flow_pub{ORB_ID(sensor_optical_flow)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d7eebd68ff..f4fdeae9c8 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1285,7 +1285,7 @@ void Simulator::check_failure_injections() int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink) { - optical_flow_s flow = {}; + sensor_optical_flow_s flow{}; flow.sensor_id = flow_mavlink->sensor_id; flow.timestamp = hrt_absolute_time(); flow.time_since_last_sonar_update = 0;