From e003cc511dc16f594f90ec774f766df548115674 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:14:01 +0000 Subject: [PATCH] add `AP_Servo_Telem` --- libraries/AP_Servo_Telem/AP_Servo_Telem.cpp | 160 ++++++++++++++++++ libraries/AP_Servo_Telem/AP_Servo_Telem.h | 84 +++++++++ .../AP_Servo_Telem/AP_Servo_Telem_config.h | 8 + libraries/AP_Servo_Telem/LogStructure.h | 47 +++++ 4 files changed, 299 insertions(+) create mode 100644 libraries/AP_Servo_Telem/AP_Servo_Telem.cpp create mode 100644 libraries/AP_Servo_Telem/AP_Servo_Telem.h create mode 100644 libraries/AP_Servo_Telem/AP_Servo_Telem_config.h create mode 100644 libraries/AP_Servo_Telem/LogStructure.h diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp new file mode 100644 index 0000000000..923b5bd261 --- /dev/null +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp @@ -0,0 +1,160 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_Servo_Telem.h" + +#if AP_SERVO_TELEM_ENABLED + +#include +#include +#include + +AP_Servo_Telem *AP_Servo_Telem::_singleton; + +AP_Servo_Telem::AP_Servo_Telem() +{ + if (_singleton) { + AP_HAL::panic("Too many AP_Servo_Telem instances"); + } + _singleton = this; +} + +// return true if the data is stale +bool AP_Servo_Telem::TelemetryData::stale(uint32_t now_ms) const volatile +{ + return (last_update_ms == 0) || ((now_ms - last_update_ms) > 5000); +} + +// return true if the requested types of data are available +bool AP_Servo_Telem::TelemetryData::present(const uint16_t type_mask) const volatile +{ + return (valid_types & type_mask) != 0; +} + +// return true if the requested types of data are available and not stale +bool AP_Servo_Telem::TelemetryData::valid(const uint16_t type_mask) const volatile +{ + return present(type_mask) && !stale(AP_HAL::millis()); +} + +// record an update to the telemetry data together with timestamp +// callback to update the data in the frontend, should be called by the driver when new data is available +void AP_Servo_Telem::update_telem_data(const uint8_t servo_index, const TelemetryData& new_data) +{ + // telemetry data are not protected by a semaphore even though updated from different threads + // each element is a primitive type and the timestamp is only updated at the end, thus a caller + // can only get slightly more up-to-date information that perhaps they were expecting or might + // read data that has just gone stale - both of these are safe and avoid the overhead of locking + + if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.valid_types == 0)) { + return; + } + + volatile TelemetryData &telemdata = _telem_data[servo_index]; + + if (new_data.present(TelemetryData::Types::COMMANDED_POSITION)) { + telemdata.command_position = new_data.command_position; + } + if (new_data.present(TelemetryData::Types::MEASURED_POSITION)) { + telemdata.measured_position = new_data.measured_position; + } + if (new_data.present(TelemetryData::Types::FORCE)) { + telemdata.force = new_data.force; + } + if (new_data.present(TelemetryData::Types::SPEED)) { + telemdata.speed = new_data.speed; + } + if (new_data.present(TelemetryData::Types::VOLTAGE)) { + telemdata.voltage = new_data.voltage; + } + if (new_data.present(TelemetryData::Types::CURRENT)) { + telemdata.current = new_data.current; + } + if (new_data.present(TelemetryData::Types::DUTY_CYCLE)) { + telemdata.duty_cycle = new_data.duty_cycle; + } + if (new_data.present(TelemetryData::Types::MOTOR_TEMP)) { + telemdata.motor_temperature_cdeg = new_data.motor_temperature_cdeg; + } + if (new_data.present(TelemetryData::Types::PCB_TEMP)) { + telemdata.pcb_temperature_cdeg = new_data.pcb_temperature_cdeg; + } + if (new_data.present(TelemetryData::Types::PCB_TEMP)) { + telemdata.status_flags = new_data.status_flags; + } + + telemdata.valid_types |= new_data.valid_types; + telemdata.last_update_ms = AP_HAL::millis(); +} + +void AP_Servo_Telem::update() +{ +#if HAL_LOGGING_ENABLED + write_log(); +#endif +} + +#if HAL_LOGGING_ENABLED +void AP_Servo_Telem::write_log() +{ + AP_Logger *logger = AP_Logger::get_singleton(); + + // Check logging is available and enabled + if ((logger == nullptr) || !logger->logging_enabled()) { + return; + } + + const uint64_t now_us = AP_HAL::micros64(); + + for (uint8_t i = 0; i < ARRAY_SIZE(_telem_data); i++) { + const volatile TelemetryData &telemdata = _telem_data[i]; + + if (telemdata.last_update_ms == _last_telem_log_ms[i]) { + // No new data since last log call, skip + continue; + } + + // Update last log timestamp + _last_telem_log_ms[i] = telemdata.last_update_ms; + + // Log, use nan for float values which are not available + const struct log_CSRV pkt { + LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG), + time_us : now_us, + id : i, + position : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position : AP::logger().quiet_nanf(), + force : telemdata.present(TelemetryData::Types::FORCE) ? telemdata.force : AP::logger().quiet_nanf(), + speed : telemdata.present(TelemetryData::Types::SPEED) ? telemdata.speed : AP::logger().quiet_nanf(), + power_pct : telemdata.duty_cycle, + pos_cmd : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.command_position : AP::logger().quiet_nanf(), + voltage : telemdata.present(TelemetryData::Types::VOLTAGE) ? telemdata.voltage : AP::logger().quiet_nanf(), + current : telemdata.present(TelemetryData::Types::CURRENT) ? telemdata.current : AP::logger().quiet_nanf(), + mot_temp : telemdata.present(TelemetryData::Types::MOTOR_TEMP) ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), + pcb_temp : telemdata.present(TelemetryData::Types::PCB_TEMP) ? telemdata.pcb_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), + error : telemdata.status_flags, + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + } + +} +#endif // HAL_LOGGING_ENABLED + +// Get the AP_Servo_Telem singleton +AP_Servo_Telem *AP_Servo_Telem::get_singleton() +{ + return AP_Servo_Telem::_singleton; +} + +#endif // AP_SERVO_TELEM_ENABLED diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.h b/libraries/AP_Servo_Telem/AP_Servo_Telem.h new file mode 100644 index 0000000000..bff0aece82 --- /dev/null +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.h @@ -0,0 +1,84 @@ +#pragma once + +#include "AP_Servo_Telem_config.h" + +#if AP_SERVO_TELEM_ENABLED + +#include +#include + + +#ifndef SERVO_TELEM_MAX_SERVOS + #define SERVO_TELEM_MAX_SERVOS NUM_SERVO_CHANNELS +#endif +static_assert(SERVO_TELEM_MAX_SERVOS > 0, "Cannot have 0 Servo telem instances"); + +class AP_Servo_Telem { +public: + AP_Servo_Telem(); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Servo_Telem); + + static AP_Servo_Telem *get_singleton(); + + struct TelemetryData { + // Telemetry values + float command_position; // Commanded servo position in degrees + float measured_position; // Measured Servo position in degrees + float force; // Force in Newton meters + float speed; // Speed degrees per second + float voltage; // Voltage in volts + float current; // Current draw in Ampere + uint8_t duty_cycle; // duty cycle 0% to 100% + int16_t motor_temperature_cdeg; // centi-degrees C, negative values allowed + int16_t pcb_temperature_cdeg; // centi-degrees C, negative values allowed + uint8_t status_flags; // Type specific status flags + + // last update time in milliseconds, determines data is stale + uint32_t last_update_ms; + + // telemetry types present + enum Types { + COMMANDED_POSITION = 1 << 0, + MEASURED_POSITION = 1 << 1, + FORCE = 1 << 2, + SPEED = 1 << 3, + VOLTAGE = 1 << 4, + CURRENT = 1 << 5, + DUTY_CYCLE = 1 << 6, + MOTOR_TEMP = 1 << 7, + PCB_TEMP = 1 << 8, + STATUS = 1 << 9, + }; + uint16_t valid_types; + + // return true if the data is stale + bool stale(uint32_t now_ms) const volatile; + + // return true if the requested types of data are available + bool present(const uint16_t type_mask) const volatile; + + // return true if the requested type of data is available and not stale + bool valid(const uint16_t type_mask) const volatile; + }; + + // update at 10Hz to log telemetry + void update(); + + // record an update to the telemetry data together with timestamp + // callback to update the data in the frontend, should be called by the driver when new data is available + void update_telem_data(const uint8_t servo_index, const TelemetryData& new_data); + +private: + + // Log telem of each servo + void write_log(); + + volatile TelemetryData _telem_data[SERVO_TELEM_MAX_SERVOS]; + + uint32_t _last_telem_log_ms[SERVO_TELEM_MAX_SERVOS]; + + static AP_Servo_Telem *_singleton; +}; +#endif // AP_SERVO_TELEM_ENABLED diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem_config.h b/libraries/AP_Servo_Telem/AP_Servo_Telem_config.h new file mode 100644 index 0000000000..97836c5366 --- /dev/null +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem_config.h @@ -0,0 +1,8 @@ +#pragma once + +#include +#include + +#ifndef AP_SERVO_TELEM_ENABLED +#define AP_SERVO_TELEM_ENABLED ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS)) && !defined(HAL_BUILD_AP_PERIPH)) +#endif diff --git a/libraries/AP_Servo_Telem/LogStructure.h b/libraries/AP_Servo_Telem/LogStructure.h new file mode 100644 index 0000000000..d8dcb9771a --- /dev/null +++ b/libraries/AP_Servo_Telem/LogStructure.h @@ -0,0 +1,47 @@ +#pragma once + +#include "AP_Servo_Telem_config.h" +#include + +#define LOG_IDS_FROM_SERVO_TELEM \ + LOG_CSRV_MSG + +// @LoggerMessage: CSRV +// @Description: Servo feedback data +// @Field: TimeUS: Time since system startup +// @Field: Id: Servo number this data relates to +// @Field: Pos: Current servo position +// @Field: Force: Force being applied +// @Field: Speed: Current servo movement speed +// @Field: Pow: Amount of rated power being applied +// @Field: PosCmd: commanded servo position +// @Field: V: Voltage +// @Field: A: Current +// @Field: MotT: motor temperature +// @Field: PCBT: PCB temperature +// @Field: Err: error flags + +struct PACKED log_CSRV { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t id; + float position; + float force; + float speed; + uint8_t power_pct; + float pos_cmd; + float voltage; + float current; + float mot_temp; + float pcb_temp; + uint8_t error; +}; + + +#if !AP_SERVO_TELEM_ENABLED +#define LOG_STRUCTURE_FROM_SERVO_TELEM +#else +#define LOG_STRUCTURE_FROM_SERVO_TELEM \ + { LOG_CSRV_MSG, sizeof(log_CSRV), \ + "CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#dtk%dvAOO-", "F-000000000-", true }, +#endif