forked from Archive/PX4-Autopilot
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
This commit is contained in:
parent
a03af59c5c
commit
3cee941f64
|
@ -4,6 +4,81 @@ menuconfig DRIVERS_UAVCAN
|
||||||
---help---
|
---help---
|
||||||
Enable support for uavcan
|
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
|
menuconfig BOARD_UAVCAN_INTERFACES
|
||||||
depends on DRIVERS_UAVCAN || DRIVERS_UAVCANNODE
|
depends on DRIVERS_UAVCAN || DRIVERS_UAVCANNODE
|
||||||
|
|
|
@ -38,19 +38,43 @@
|
||||||
#include "sensor_bridge.hpp"
|
#include "sensor_bridge.hpp"
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_ACCEL)
|
||||||
#include "accel.hpp"
|
#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"
|
#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"
|
#include "hygrometer.hpp"
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_ICE_STATUS)
|
||||||
#include "ice_status.hpp"
|
#include "ice_status.hpp"
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_MAG)
|
||||||
#include "mag.hpp"
|
#include "mag.hpp"
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_RANGEFINDER)
|
||||||
#include "rangefinder.hpp"
|
#include "rangefinder.hpp"
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_SAFETY_BUTTON)
|
||||||
#include "safety_button.hpp"
|
#include "safety_button.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* IUavcanSensorBridge
|
* IUavcanSensorBridge
|
||||||
|
@ -58,6 +82,7 @@
|
||||||
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
|
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
|
||||||
{
|
{
|
||||||
// airspeed
|
// airspeed
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED)
|
||||||
int32_t uavcan_sub_aspd = 1;
|
int32_t uavcan_sub_aspd = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_ASPD"), &uavcan_sub_aspd);
|
param_get(param_find("UAVCAN_SUB_ASPD"), &uavcan_sub_aspd);
|
||||||
|
|
||||||
|
@ -65,6 +90,9 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanAirspeedBridge(node));
|
list.add(new UavcanAirspeedBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_BARO)
|
||||||
// baro
|
// baro
|
||||||
int32_t uavcan_sub_baro = 1;
|
int32_t uavcan_sub_baro = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_BARO"), &uavcan_sub_baro);
|
param_get(param_find("UAVCAN_SUB_BARO"), &uavcan_sub_baro);
|
||||||
|
@ -73,7 +101,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanBarometerBridge(node));
|
list.add(new UavcanBarometerBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// battery
|
// battery
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_BATTERY)
|
||||||
int32_t uavcan_sub_bat = 1;
|
int32_t uavcan_sub_bat = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_BAT"), &uavcan_sub_bat);
|
param_get(param_find("UAVCAN_SUB_BAT"), &uavcan_sub_bat);
|
||||||
|
|
||||||
|
@ -81,7 +112,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanBatteryBridge(node));
|
list.add(new UavcanBatteryBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// differential pressure
|
// differential pressure
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE)
|
||||||
int32_t uavcan_sub_dpres = 1;
|
int32_t uavcan_sub_dpres = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_DPRES"), &uavcan_sub_dpres);
|
param_get(param_find("UAVCAN_SUB_DPRES"), &uavcan_sub_dpres);
|
||||||
|
|
||||||
|
@ -89,7 +123,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanDifferentialPressureBridge(node));
|
list.add(new UavcanDifferentialPressureBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// flow
|
// flow
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_FLOW)
|
||||||
int32_t uavcan_sub_flow = 1;
|
int32_t uavcan_sub_flow = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_FLOW"), &uavcan_sub_flow);
|
param_get(param_find("UAVCAN_SUB_FLOW"), &uavcan_sub_flow);
|
||||||
|
|
||||||
|
@ -97,7 +134,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanFlowBridge(node));
|
list.add(new UavcanFlowBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// GPS
|
// GPS
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_GNSS)
|
||||||
int32_t uavcan_sub_gps = 1;
|
int32_t uavcan_sub_gps = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_GPS"), &uavcan_sub_gps);
|
param_get(param_find("UAVCAN_SUB_GPS"), &uavcan_sub_gps);
|
||||||
|
|
||||||
|
@ -105,7 +145,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanGnssBridge(node));
|
list.add(new UavcanGnssBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// hygrometer
|
// hygrometer
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_HYGROMETER)
|
||||||
int32_t uavcan_sub_hygro = 1;
|
int32_t uavcan_sub_hygro = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_HYGRO"), &uavcan_sub_hygro);
|
param_get(param_find("UAVCAN_SUB_HYGRO"), &uavcan_sub_hygro);
|
||||||
|
|
||||||
|
@ -113,7 +156,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanHygrometerBridge(node));
|
list.add(new UavcanHygrometerBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// ice (internal combustion engine)
|
// ice (internal combustion engine)
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_ICE_STATUS)
|
||||||
int32_t uavcan_sub_ice = 1;
|
int32_t uavcan_sub_ice = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_ICE"), &uavcan_sub_ice);
|
param_get(param_find("UAVCAN_SUB_ICE"), &uavcan_sub_ice);
|
||||||
|
|
||||||
|
@ -121,7 +167,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanIceStatusBridge(node));
|
list.add(new UavcanIceStatusBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// IMU
|
// IMU
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_ACCEL)
|
||||||
int32_t uavcan_sub_imu = 1;
|
int32_t uavcan_sub_imu = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_IMU"), &uavcan_sub_imu);
|
param_get(param_find("UAVCAN_SUB_IMU"), &uavcan_sub_imu);
|
||||||
|
|
||||||
|
@ -130,7 +179,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanGyroBridge(node));
|
list.add(new UavcanGyroBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// magnetometer
|
// magnetometer
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_MAG)
|
||||||
int32_t uavcan_sub_mag = 1;
|
int32_t uavcan_sub_mag = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_MAG"), &uavcan_sub_mag);
|
param_get(param_find("UAVCAN_SUB_MAG"), &uavcan_sub_mag);
|
||||||
|
|
||||||
|
@ -138,7 +190,10 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanMagnetometerBridge(node));
|
list.add(new UavcanMagnetometerBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// range finder
|
// range finder
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_RANGEFINDER)
|
||||||
int32_t uavcan_sub_rng = 1;
|
int32_t uavcan_sub_rng = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_RNG"), &uavcan_sub_rng);
|
param_get(param_find("UAVCAN_SUB_RNG"), &uavcan_sub_rng);
|
||||||
|
|
||||||
|
@ -146,13 +201,19 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||||
list.add(new UavcanRangefinderBridge(node));
|
list.add(new UavcanRangefinderBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// safety button
|
// safety button
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_SENSOR_SAFETY_BUTTON)
|
||||||
int32_t uavcan_sub_button = 1;
|
int32_t uavcan_sub_button = 1;
|
||||||
param_get(param_find("UAVCAN_SUB_BTN"), &uavcan_sub_button);
|
param_get(param_find("UAVCAN_SUB_BTN"), &uavcan_sub_button);
|
||||||
|
|
||||||
if (uavcan_sub_button != 0) {
|
if (uavcan_sub_button != 0) {
|
||||||
list.add(new UavcanSafetyButtonBridge(node));
|
list.add(new UavcanSafetyButtonBridge(node));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -80,14 +80,26 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
|
||||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
|
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
|
||||||
ModuleParams(nullptr),
|
ModuleParams(nullptr),
|
||||||
_node(can_driver, system_clock, _pool_allocator),
|
_node(can_driver, system_clock, _pool_allocator),
|
||||||
|
#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER)
|
||||||
_arming_status_controller(_node),
|
_arming_status_controller(_node),
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER)
|
||||||
_beep_controller(_node),
|
_beep_controller(_node),
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
_esc_controller(_node),
|
_esc_controller(_node),
|
||||||
_servo_controller(_node),
|
_servo_controller(_node),
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER)
|
||||||
_hardpoint_controller(_node),
|
_hardpoint_controller(_node),
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER)
|
||||||
_safety_state_controller(_node),
|
_safety_state_controller(_node),
|
||||||
_log_message_controller(_node),
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
|
||||||
_rgbled_controller(_node),
|
_rgbled_controller(_node),
|
||||||
|
#endif
|
||||||
|
_log_message_controller(_node),
|
||||||
_time_sync_master(_node),
|
_time_sync_master(_node),
|
||||||
_time_sync_slave(_node),
|
_time_sync_slave(_node),
|
||||||
_node_status_monitor(_node),
|
_node_status_monitor(_node),
|
||||||
|
@ -103,8 +115,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
|
||||||
std::abort();
|
std::abort();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
_mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
|
_mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
|
||||||
_mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ);
|
_mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
UavcanNode::~UavcanNode()
|
UavcanNode::~UavcanNode()
|
||||||
|
@ -395,8 +409,10 @@ UavcanNode::get_param(int remote_node_id, const char *name)
|
||||||
void
|
void
|
||||||
UavcanNode::update_params()
|
UavcanNode::update_params()
|
||||||
{
|
{
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
_mixing_interface_esc.updateParams();
|
_mixing_interface_esc.updateParams();
|
||||||
_mixing_interface_servo.updateParams();
|
_mixing_interface_servo.updateParams();
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -429,8 +445,11 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||||
}
|
}
|
||||||
|
|
||||||
_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
|
_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
_instance->_mixing_interface_esc.ScheduleNow();
|
_instance->_mixing_interface_esc.ScheduleNow();
|
||||||
_instance->_mixing_interface_servo.ScheduleNow();
|
_instance->_mixing_interface_servo.ScheduleNow();
|
||||||
|
#endif
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
@ -482,6 +501,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
// UAVCAN_PUB_ARM
|
// UAVCAN_PUB_ARM
|
||||||
|
#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER)
|
||||||
int32_t uavcan_pub_arm = 0;
|
int32_t uavcan_pub_arm = 0;
|
||||||
param_get(param_find("UAVCAN_PUB_ARM"), &uavcan_pub_arm);
|
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();
|
ret = _beep_controller.init();
|
||||||
|
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// Actuators
|
// Actuators
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
ret = _esc_controller.init();
|
ret = _esc_controller.init();
|
||||||
|
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER)
|
||||||
ret = _hardpoint_controller.init();
|
ret = _hardpoint_controller.init();
|
||||||
|
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_SAFETY_CONTROLLER)
|
||||||
ret = _safety_state_controller.init();
|
ret = _safety_state_controller.init();
|
||||||
|
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
ret = _log_message_controller.init();
|
ret = _log_message_controller.init();
|
||||||
|
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
|
||||||
ret = _rgbled_controller.init();
|
ret = _rgbled_controller.init();
|
||||||
|
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
/* Start node info retriever to fetch node info from new nodes */
|
/* Start node info retriever to fetch node info from new nodes */
|
||||||
ret = _node_info_retriever.start();
|
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());
|
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
|
// Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions
|
||||||
if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE
|
if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE
|
||||||
|| UavcanEscController::max_output_value() > (int)UINT16_MAX) {
|
|| 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);
|
_mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
|
||||||
|
#endif
|
||||||
|
|
||||||
/* Set up shared service clients */
|
/* Set up shared service clients */
|
||||||
_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
|
_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
|
||||||
|
@ -921,15 +961,20 @@ UavcanNode::Run()
|
||||||
pthread_mutex_unlock(&_node_mutex);
|
pthread_mutex_unlock(&_node_mutex);
|
||||||
|
|
||||||
if (_task_should_exit.load()) {
|
if (_task_should_exit.load()) {
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
_mixing_interface_esc.mixingOutput().unregister();
|
_mixing_interface_esc.mixingOutput().unregister();
|
||||||
_mixing_interface_esc.ScheduleClear();
|
_mixing_interface_esc.ScheduleClear();
|
||||||
|
|
||||||
_mixing_interface_servo.mixingOutput().unregister();
|
_mixing_interface_servo.mixingOutput().unregister();
|
||||||
_mixing_interface_servo.ScheduleClear();
|
_mixing_interface_servo.ScheduleClear();
|
||||||
|
#endif
|
||||||
ScheduleClear();
|
ScheduleClear();
|
||||||
_instance = nullptr;
|
_instance = nullptr;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||||
unsigned num_control_groups_updated)
|
unsigned num_control_groups_updated)
|
||||||
{
|
{
|
||||||
|
@ -974,6 +1019,7 @@ void UavcanMixingInterfaceServo::Run()
|
||||||
_mixing_output.updateSubscriptions(false);
|
_mixing_output.updateSubscriptions(false);
|
||||||
pthread_mutex_unlock(&_node_mutex);
|
pthread_mutex_unlock(&_node_mutex);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
void
|
void
|
||||||
UavcanNode::print_info()
|
UavcanNode::print_info()
|
||||||
|
@ -1016,10 +1062,13 @@ UavcanNode::print_info()
|
||||||
|
|
||||||
printf("\n");
|
printf("\n");
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
printf("ESC outputs:\n");
|
printf("ESC outputs:\n");
|
||||||
_mixing_interface_esc.mixingOutput().printStatus();
|
_mixing_interface_esc.mixingOutput().printStatus();
|
||||||
|
|
||||||
printf("Servo outputs:\n");
|
printf("Servo outputs:\n");
|
||||||
_mixing_interface_servo.mixingOutput().printStatus();
|
_mixing_interface_servo.mixingOutput().printStatus();
|
||||||
|
#endif
|
||||||
|
|
||||||
printf("\n");
|
printf("\n");
|
||||||
|
|
||||||
|
|
|
@ -46,15 +46,36 @@
|
||||||
#include <px4_platform_common/atomic.h>
|
#include <px4_platform_common/atomic.h>
|
||||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
#include "actuators/esc.hpp"
|
#include "actuators/esc.hpp"
|
||||||
#include "actuators/hardpoint.hpp"
|
|
||||||
#include "actuators/servo.hpp"
|
#include "actuators/servo.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER)
|
||||||
|
#include "actuators/hardpoint.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#include "allocator.hpp"
|
#include "allocator.hpp"
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER)
|
||||||
#include "arming_status.hpp"
|
#include "arming_status.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER)
|
||||||
#include "beep.hpp"
|
#include "beep.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
#include "logmessage.hpp"
|
#include "logmessage.hpp"
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
|
||||||
#include "rgbled.hpp"
|
#include "rgbled.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER)
|
||||||
#include "safety_state.hpp"
|
#include "safety_state.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
#include "sensors/sensor_bridge.hpp"
|
#include "sensors/sensor_bridge.hpp"
|
||||||
#include "uavcan_driver.hpp"
|
#include "uavcan_driver.hpp"
|
||||||
#include "uavcan_servers.hpp"
|
#include "uavcan_servers.hpp"
|
||||||
|
@ -91,6 +112,8 @@ class UavcanNode;
|
||||||
* a fixed rate or upon bus updates).
|
* a fixed rate or upon bus updates).
|
||||||
* All work items are expected to run on the same work queue.
|
* All work items are expected to run on the same work queue.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
class UavcanMixingInterfaceESC : public OutputModuleInterface
|
class UavcanMixingInterfaceESC : public OutputModuleInterface
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -143,6 +166,7 @@ private:
|
||||||
UavcanServoController &_servo_controller;
|
UavcanServoController &_servo_controller;
|
||||||
MixingOutput _mixing_output{"UAVCAN_SV", UavcanServoController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
MixingOutput _mixing_output{"UAVCAN_SV", UavcanServoController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||||
};
|
};
|
||||||
|
#endif
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A UAVCAN node.
|
* A UAVCAN node.
|
||||||
|
@ -225,16 +249,30 @@ private:
|
||||||
uavcan::Node<> _node; ///< library instance
|
uavcan::Node<> _node; ///< library instance
|
||||||
pthread_mutex_t _node_mutex;
|
pthread_mutex_t _node_mutex;
|
||||||
|
|
||||||
|
#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER)
|
||||||
UavcanArmingStatus _arming_status_controller;
|
UavcanArmingStatus _arming_status_controller;
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER)
|
||||||
UavcanBeepController _beep_controller;
|
UavcanBeepController _beep_controller;
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||||
UavcanEscController _esc_controller;
|
UavcanEscController _esc_controller;
|
||||||
UavcanServoController _servo_controller;
|
|
||||||
UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller};
|
UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller};
|
||||||
|
|
||||||
|
UavcanServoController _servo_controller;
|
||||||
UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller};
|
UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller};
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER)
|
||||||
UavcanHardpointController _hardpoint_controller;
|
UavcanHardpointController _hardpoint_controller;
|
||||||
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER)
|
||||||
UavcanSafetyState _safety_state_controller;
|
UavcanSafetyState _safety_state_controller;
|
||||||
UavcanLogMessage _log_message_controller;
|
#endif
|
||||||
|
#if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
|
||||||
UavcanRGBController _rgbled_controller;
|
UavcanRGBController _rgbled_controller;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
UavcanLogMessage _log_message_controller;
|
||||||
|
|
||||||
uavcan::GlobalTimeSyncMaster _time_sync_master;
|
uavcan::GlobalTimeSyncMaster _time_sync_master;
|
||||||
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
||||||
|
|
Loading…
Reference in New Issue