diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index dbdaf79b7f..0847b45112 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -212,6 +212,8 @@ set(msg_files SensorTemp.msg SensorUwb.msg SensorAirflow.msg + ServoReport.msg + ServoStatus.msg SystemPower.msg TakeoffStatus.msg TaskStackInfo.msg diff --git a/msg/ServoReport.msg b/msg/ServoReport.msg new file mode 100644 index 0000000000..4dbb43b0ee --- /dev/null +++ b/msg/ServoReport.msg @@ -0,0 +1,25 @@ +uint64 timestamp # time since system start (microseconds) +uint8 servo_actuator_id # actuator id of current servo (in most cases 1-8) +uint8 servo_node_id # Node ID of device on CAN bus +float32 servo_position # meter or radian +float32 servo_force # Newton or Newton metre +float32 servo_speed # meter per second or radian per second +uint8 servo_power_rating_pct # 0 - unloaded, 100 - full load + +uint8 servo_function # servo output function + + +float32 servo_temperature # in kelvin +uint8 servo_temperature_error_flags + +uint8 ERROR_FLAG_OVERHEATING = 1 +uint8 ERROR_FLAG_OVERCOOLING = 2 + +float32 servo_voltage # Volts +float32 servo_current # Amps +uint8 servo_power_error_flags + +uint8 ERROR_FLAG_OVERVOLTAGE = 1 +uint8 ERROR_FLAG_UNDERVOLTAGE = 2 +uint8 ERROR_FLAG_OVERCURRENT = 4 +uint8 ERROR_FLAG_UNDERCURRENT = 8 diff --git a/msg/ServoStatus.msg b/msg/ServoStatus.msg new file mode 100644 index 0000000000..7e06eb5197 --- /dev/null +++ b/msg/ServoStatus.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) +uint8 CONNECTED_SERVO_MAX = 8 # The number of SERVOs supported via UAVCAN + +uint8 SERVO_CONNECTION_TYPE_PPM = 0 # Traditional PPM SERVO +uint8 SERVO_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected SERVO +uint8 SERVO_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM +uint8 SERVO_CONNECTION_TYPE_I2C = 3 # I2C +uint8 SERVO_CONNECTION_TYPE_CAN = 4 # CAN-Bus +uint8 SERVO_CONNECTION_TYPE_DSHOT = 5 # DShot + +uint16 counter # incremented by the writing thread everytime new data is stored +uint16 temperature_counter # Incremented when new temperature data is stored +uint16 power_counter # Incremented when new power data is stored +uint8 servo_count # number of connected servo's + +uint8 servo_connectiontype # how SERVOs connected to the system. Currently only CAN supports servo telemtry + +uint8 servo_online_flags # Bitmask indicating which SERVO is online/offline +# servo_online_flags bit 0 : Set to 1 if SERVO0 is online +# servo_online_flags bit 1 : Set to 1 if SERVO1 is online +# servo_online_flags bit 2 : Set to 1 if SERVO2 is online +# servo_online_flags bit 3 : Set to 1 if SERVO3 is online +# servo_online_flags bit 4 : Set to 1 if SERVO4 is online +# servo_online_flags bit 5 : Set to 1 if SERVO5 is online +# servo_online_flags bit 6 : Set to 1 if SERVO6 is online +# servo_online_flags bit 7 : Set to 1 if SERVO7 is online + +ServoReport[8] servo diff --git a/src/drivers/uavcan/actuators/servo.cpp b/src/drivers/uavcan/actuators/servo.cpp index 1af9a7173c..8e42b9b217 100644 --- a/src/drivers/uavcan/actuators/servo.cpp +++ b/src/drivers/uavcan/actuators/servo.cpp @@ -39,9 +39,56 @@ using namespace time_literals; UavcanServoController::UavcanServoController(uavcan::INode &node) : _node(node), - _uavcan_pub_array_cmd(node) + _uavcan_pub_array_cmd(node), + _uavcan_sub_status(node), + _uavcan_sub_temperature(node), + _uavcan_sub_circuit_status(node) { _uavcan_pub_array_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY); + memset(_last_temperature, 0, sizeof(_last_temperature)); // Set bytes to 0 + memset(_last_temperature_error_flag, 0, sizeof(_last_temperature_error_flag)); + memset(_last_voltage, 0, sizeof(_last_voltage)); + memset(_last_current, 0, sizeof(_last_current)); + memset(_last_power_error_flag, 0, sizeof(_last_power_error_flag)); +} + +int +UavcanServoController::init() +{ + // Servo status subscription + int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanServoController::servo_status_sub_cb)); + + if (res < 0) { + PX4_ERR("Servo status sub failed %i", res); + return res; + } + + // Servo temperature subscription + res = _uavcan_sub_temperature.start(TemperatureCbBinder(this, &UavcanServoController::servo_temperature_sub_cb)); + + if (res < 0) { + PX4_ERR("Servo Temperature sub failed %i", res); + return res; + } + + // Servo circuit status subscription + res = _uavcan_sub_circuit_status.start(CircuitStatusCbBinder(this, + &UavcanServoController::servo_circuit_status_sub_cb)); + + if (res < 0) { + PX4_ERR("Servo circuit status sub failed %i", res); + return res; + } + + _servo_status_pub.advertise(); // advertise to ensure messages are logged + + return res; // return if both subscriptions are successful +} + +void +UavcanServoController::set_servo_count(uint8_t count) +{ + _servo_count = count; } void @@ -60,3 +107,82 @@ UavcanServoController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned _uavcan_pub_array_cmd.broadcast(msg); } + +void +UavcanServoController::servo_temperature_sub_cb(const + uavcan::ReceivedDataStructure + &msg) +{ + if (msg.device_id < servo_status_s::CONNECTED_SERVO_MAX) { + + _last_temperature[msg.device_id] = msg.temperature; + _last_temperature_error_flag[msg.device_id] = msg.error_flags; + _servo_temperature_counter += 1; // Keep counter to monitor callbacks + } +} + +void +UavcanServoController::servo_circuit_status_sub_cb(const + uavcan::ReceivedDataStructure + &msg) +{ + if (msg.circuit_id < servo_status_s::CONNECTED_SERVO_MAX) { + + _last_voltage[msg.circuit_id] = msg.voltage; + _last_current[msg.circuit_id] = msg.current; + _last_power_error_flag[msg.circuit_id] = msg.error_flags; + _servo_power_counter += 1; // Keep counter to monitor callbacks + } +} + +void +UavcanServoController::servo_status_sub_cb(const uavcan::ReceivedDataStructure + &msg) +{ + if (msg.actuator_id < servo_status_s::CONNECTED_SERVO_MAX) { + auto &ref = _servo_status.servo[msg.actuator_id]; + + ref.timestamp = hrt_absolute_time(); + ref.servo_node_id = msg.getSrcNodeID().get(); + ref.servo_actuator_id = msg.actuator_id; + ref.servo_position = msg.position; + ref.servo_force = msg.force; + ref.servo_speed = msg.speed; + ref.servo_power_rating_pct = msg.power_rating_pct; + + // Add servo temperature data + ref.servo_temperature = _last_temperature[msg.actuator_id]; + ref.servo_temperature_error_flags = _last_temperature_error_flag[msg.actuator_id]; + + // Add servo power data + ref.servo_voltage = _last_voltage[msg.actuator_id]; + ref.servo_current = _last_current[msg.actuator_id]; + ref.servo_power_error_flags = _last_power_error_flag[msg.actuator_id]; + + _servo_status.counter += 1; + _servo_status.temperature_counter = _servo_temperature_counter; + _servo_status.power_counter = _servo_power_counter; + _servo_status.servo_count = _servo_count; + _servo_status.servo_connectiontype = servo_status_s::SERVO_CONNECTION_TYPE_CAN; + _servo_status.servo_online_flags = check_servos_status(); + _servo_status.timestamp = hrt_absolute_time(); + _servo_status_pub.publish(_servo_status); + } +} + +uint8_t +UavcanServoController::check_servos_status() +{ + int servo_status_flags = 0; + const hrt_abstime now = hrt_absolute_time(); + + for (int index = 0; index < servo_status_s::CONNECTED_SERVO_MAX; index++) { + + if (_servo_status.servo[index].timestamp > 0 && now - _servo_status.servo[index].timestamp < 1200_ms) { + servo_status_flags |= (1 << index); + } + + } + + return servo_status_flags; +} diff --git a/src/drivers/uavcan/actuators/servo.hpp b/src/drivers/uavcan/actuators/servo.hpp index 78d6f233e1..31412f9c58 100644 --- a/src/drivers/uavcan/actuators/servo.hpp +++ b/src/drivers/uavcan/actuators/servo.hpp @@ -36,9 +36,12 @@ #include #include #include +#include +#include #include #include #include +#include #include #include @@ -48,13 +51,76 @@ public: static constexpr int MAX_ACTUATORS = 8; static constexpr unsigned MAX_RATE_HZ = 50; static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 6; ///< 0..31, inclusive, 0 - highest, 31 - lowest + static_assert(uavcan::equipment::actuator::ArrayCommand::FieldTypes::commands::MaxSize >= MAX_ACTUATORS, + "Too many actuators"); UavcanServoController(uavcan::INode &node); ~UavcanServoController() = default; + int init(); + void set_servo_count(uint8_t count); void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); + servo_status_s &servo_status() { return _servo_status; } + + private: + + /** + * Servo status message reception will be reported via this callback. + */ + void servo_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + /** + * Servo temperature message reception will be reported via this callback. + */ + void servo_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); + + /** + * Servo circuit status message reception will be reported via this callback. + */ + void servo_circuit_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + + /** + * Checks all the servo freshness based on timestamp, if an servo exceeds the timeout then is flagged offline. + */ + uint8_t check_servos_status(); + + typedef uavcan::MethodBinder&)> + StatusCbBinder; + + typedef uavcan::MethodBinder&)> + TemperatureCbBinder; + + typedef uavcan::MethodBinder&)> + CircuitStatusCbBinder; + + servo_status_s _servo_status{}; + uint8_t _servo_count{0}; + + uORB::PublicationMulti _servo_status_pub{ORB_ID(servo_status)}; + + /* + * libuavcan related things + */ + uavcan::INode &_node; uavcan::Publisher _uavcan_pub_array_cmd; + uavcan::Subscriber _uavcan_sub_status; + uavcan::Subscriber _uavcan_sub_temperature; + uavcan::Subscriber _uavcan_sub_circuit_status; + + + float _last_temperature[servo_status_s::CONNECTED_SERVO_MAX]; + uint8_t _last_temperature_error_flag[servo_status_s::CONNECTED_SERVO_MAX]; + float _last_voltage[servo_status_s::CONNECTED_SERVO_MAX]; + float _last_current[servo_status_s::CONNECTED_SERVO_MAX]; + uint8_t _last_power_error_flag[servo_status_s::CONNECTED_SERVO_MAX]; + int _servo_temperature_counter{0}; + int _servo_power_counter{0}; + }; diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index e70c4436f2..5848715248 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -528,7 +528,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) #endif - // Actuators + // ESCs #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) int32_t uavcan_enable = -1; (void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable); @@ -544,6 +544,17 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) #endif + // Servos +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) + ret = _servo_controller.init(); + + if (ret < 0) { + return ret; + } + +#endif + + #if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) ret = _hardpoint_controller.init(); @@ -1135,6 +1146,22 @@ void UavcanMixingInterfaceESC::mixerChanged() _esc_controller.set_rotor_count(rotor_count); } +void UavcanMixingInterfaceServo::mixerChanged() +{ + int servo_count = 0; + + for (unsigned i = 0; i < MAX_ACTUATORS; ++i) { + servo_count += _mixing_output.isFunctionSet(i); + + if (i < servo_status_s::CONNECTED_SERVO_MAX) { + _servo_controller.servo_status().servo[i].servo_function = (uint8_t)_mixing_output.outputFunction(i); + } + + } + + _servo_controller.set_servo_count(servo_count); +} + bool UavcanMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 56f7c445fe..474b93d3aa 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -165,6 +165,8 @@ public: bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; + void mixerChanged() override; + MixingOutput &mixingOutput() { return _mixing_output; } protected: diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5e366273d3..2214e0c746 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -126,6 +126,7 @@ void LoggedTopics::add_default_topics() add_topic("sensor_selection"); add_topic("sensors_status_imu", 200); add_optional_topic("spoilers_setpoint", 1000); + add_optional_topic("servo_status", 10); add_topic("system_power", 500); add_optional_topic("takeoff_status", 1000); add_optional_topic("tecs_status", 200);