From 3cee941f649b571fc28ce6c74c35cf0116c3cd93 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Fri, 4 Aug 2023 22:23:54 +0200 Subject: [PATCH] Kconfig subs and controllers for uavcan small changed to revert back to px4::main Make kconfig more readble Combine esc and servo controllers under one option --- src/drivers/uavcan/Kconfig | 75 ++++++++++++++++++++ src/drivers/uavcan/sensors/sensor_bridge.cpp | 73 +++++++++++++++++-- src/drivers/uavcan/uavcan_main.cpp | 51 ++++++++++++- src/drivers/uavcan/uavcan_main.hpp | 44 +++++++++++- 4 files changed, 233 insertions(+), 10 deletions(-) diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index c5f3d1e76a..d2f064e8ba 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -4,6 +4,81 @@ menuconfig DRIVERS_UAVCAN ---help--- Enable support for uavcan +if DRIVERS_UAVCAN + config UAVCAN_ARMING_CONTROLLER + bool "Include arming status controller" + default y + + config UAVCAN_BEEP_CONTROLLER + bool "Include beep controller" + default y + + config UAVCAN_OUTPUTS_CONTROLLER + bool "Include servo & ESC controller" + default y + + config UAVCAN_HARDPOINT_CONTROLLER + bool "Include hardpoint controller" + default y + + config UAVCAN_SAFETY_STATE_CONTROLLER + bool "Include safety state controller" + default y + + config UAVCAN_RGB_CONTROLLER + bool "Include rgb controller" + default y + + config UAVCAN_SENSOR_ACCEL + bool "Subscribe to IMU: uavcan::equipment::ahrs::RawIMU" + default y + + config UAVCAN_SENSOR_AIRSPEED + bool "Subscribe to Airspeed: uavcan::equipment::air_data::IndicatedAirspeed | uavcan::equipment::air_data::TrueAirspeed | uavcan::equipment::air_data::StaticTemperature" + default y + + config UAVCAN_SENSOR_BARO + bool "Subscribe to Barometer: uavcan::equipment::air_data::StaticPressure | uavcan::equipment::air_data::StaticTemperature" + default y + + config UAVCAN_SENSOR_BATTERY + bool "Subscribe to Battery: uavcan::equipment::power::BatteryInfo | ardupilot::equipment::power::BatteryInfoAux" + default y + + config UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE + bool "Subscribe to Differential Pressure: uavcan::equipment::air_data::RawAirData" + default y + + config UAVCAN_SENSOR_FLOW + bool "Subscribe to Flow: com::hex::equipment::flow::Measurement" + default y + + config UAVCAN_SENSOR_GNSS + bool "Subscribe to GPS: uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2" + default y + + config UAVCAN_SENSOR_HYGROMETER + bool "Subscribe to Hygrometer: dronecan::sensors::hygrometer::Hygrometer" + default y + + config UAVCAN_SENSOR_ICE_STATUS + bool "Subscribe to Internal Combustion Engine: uavcan::equipment::ice::reciprocating::Status" + default y + + config UAVCAN_SENSOR_MAG + bool "Subscribe to Magnetometer: uavcan::equipment::ahrs::MagneticFieldStrength | uavcan::equipment::ahrs::MagneticFieldStrength2" + default y + + config UAVCAN_SENSOR_RANGEFINDER + bool "Subscribe to Rangefinder: uavcan::equipment::range_sensor::Measurement" + default y + + config UAVCAN_SENSOR_SAFETY_BUTTON + bool "Subscribe to Safety Button: ardupilot::indication::Button" + default y + +endif #DRIVERS_UAVCAN + menuconfig BOARD_UAVCAN_INTERFACES depends on DRIVERS_UAVCAN || DRIVERS_UAVCANNODE diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 023a16cf1d..3f1d424bd1 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -38,19 +38,43 @@ #include "sensor_bridge.hpp" #include +#if defined(CONFIG_UAVCAN_SENSOR_ACCEL) #include "accel.hpp" -#include "airspeed.hpp" -#include "baro.hpp" -#include "battery.hpp" -#include "differential_pressure.hpp" -#include "flow.hpp" -#include "gnss.hpp" #include "gyro.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED) +#include "airspeed.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_BARO) +#include "baro.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_BATTERY) +#include "battery.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE) +#include "differential_pressure.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_FLOW) +#include "flow.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_GNSS) +#include "gnss.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_HYGROMETER) #include "hygrometer.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_ICE_STATUS) #include "ice_status.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_MAG) #include "mag.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_RANGEFINDER) #include "rangefinder.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_SAFETY_BUTTON) #include "safety_button.hpp" +#endif /* * IUavcanSensorBridge @@ -58,6 +82,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { // airspeed +#if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED) int32_t uavcan_sub_aspd = 1; param_get(param_find("UAVCAN_SUB_ASPD"), &uavcan_sub_aspd); @@ -65,6 +90,9 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, ListScheduleOnInterval(ScheduleIntervalMs * 1000); + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _instance->_mixing_interface_esc.ScheduleNow(); _instance->_mixing_interface_servo.ScheduleNow(); +#endif return OK; } @@ -482,6 +501,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) int ret; // UAVCAN_PUB_ARM +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) int32_t uavcan_pub_arm = 0; param_get(param_find("UAVCAN_PUB_ARM"), &uavcan_pub_arm); @@ -493,43 +513,60 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) } } +#endif + +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) ret = _beep_controller.init(); if (ret < 0) { return ret; } +#endif + // Actuators +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) ret = _esc_controller.init(); if (ret < 0) { return ret; } +#endif + +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) ret = _hardpoint_controller.init(); if (ret < 0) { return ret; } +#endif + +#if defined(CONFIG_UAVCAN_SAFETY_CONTROLLER) ret = _safety_state_controller.init(); if (ret < 0) { return ret; } +#endif + ret = _log_message_controller.init(); if (ret < 0) { return ret; } +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) ret = _rgbled_controller.init(); if (ret < 0) { return ret; } +#endif + /* Start node info retriever to fetch node info from new nodes */ ret = _node_info_retriever.start(); @@ -552,6 +589,8 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) PX4_DEBUG("sensor bridge '%s' init ok", br->get_name()); } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) + // Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE || UavcanEscController::max_output_value() > (int)UINT16_MAX) { @@ -560,6 +599,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) } _mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); +#endif /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset)); @@ -921,15 +961,20 @@ UavcanNode::Run() pthread_mutex_unlock(&_node_mutex); if (_task_should_exit.load()) { + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _mixing_interface_esc.mixingOutput().unregister(); _mixing_interface_esc.ScheduleClear(); + _mixing_interface_servo.mixingOutput().unregister(); _mixing_interface_servo.ScheduleClear(); +#endif ScheduleClear(); _instance = nullptr; } } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { @@ -974,6 +1019,7 @@ void UavcanMixingInterfaceServo::Run() _mixing_output.updateSubscriptions(false); pthread_mutex_unlock(&_node_mutex); } +#endif void UavcanNode::print_info() @@ -1016,10 +1062,13 @@ UavcanNode::print_info() printf("\n"); +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) printf("ESC outputs:\n"); _mixing_interface_esc.mixingOutput().printStatus(); + printf("Servo outputs:\n"); _mixing_interface_servo.mixingOutput().printStatus(); +#endif printf("\n"); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 7fcc86e968..54119259ef 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -46,15 +46,36 @@ #include #include +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) #include "actuators/esc.hpp" -#include "actuators/hardpoint.hpp" #include "actuators/servo.hpp" +#endif + +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) +#include "actuators/hardpoint.hpp" +#endif + + #include "allocator.hpp" + +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) #include "arming_status.hpp" +#endif + +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) #include "beep.hpp" +#endif + #include "logmessage.hpp" + +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) #include "rgbled.hpp" +#endif + +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) #include "safety_state.hpp" +#endif + #include "sensors/sensor_bridge.hpp" #include "uavcan_driver.hpp" #include "uavcan_servers.hpp" @@ -91,6 +112,8 @@ class UavcanNode; * a fixed rate or upon bus updates). * All work items are expected to run on the same work queue. */ + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) class UavcanMixingInterfaceESC : public OutputModuleInterface { public: @@ -143,6 +166,7 @@ private: UavcanServoController &_servo_controller; MixingOutput _mixing_output{"UAVCAN_SV", UavcanServoController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; }; +#endif /** * A UAVCAN node. @@ -225,16 +249,30 @@ private: uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) UavcanArmingStatus _arming_status_controller; +#endif +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) UavcanBeepController _beep_controller; +#endif +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) UavcanEscController _esc_controller; - UavcanServoController _servo_controller; UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller}; + + UavcanServoController _servo_controller; UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller}; +#endif +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) UavcanHardpointController _hardpoint_controller; +#endif +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) UavcanSafetyState _safety_state_controller; - UavcanLogMessage _log_message_controller; +#endif +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) UavcanRGBController _rgbled_controller; +#endif + + UavcanLogMessage _log_message_controller; uavcan::GlobalTimeSyncMaster _time_sync_master; uavcan::GlobalTimeSyncSlave _time_sync_slave;