Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add `AP_Servo_Telem`

This commit is contained in:
Iampete1 2024-11-16 11:14:01 +00:00 committed by Andrew Tridgell
parent d0bbc02995
commit e003cc511d
4 changed files with 299 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
libraries/AP_Servo_Telem/AP_Servo_Telem.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "AP_Servo_Telem.h"
#if AP_SERVO_TELEM_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
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

84
libraries/AP_Servo_Telem/AP_Servo_Telem.h Normal file
View File

@ -0,0 +1,84 @@
#pragma once
#include "AP_Servo_Telem_config.h"
#if AP_SERVO_TELEM_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <SRV_Channel/SRV_Channel_config.h>
#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

8
libraries/AP_Servo_Telem/AP_Servo_Telem_config.h Normal file
View File

@ -0,0 +1,8 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <SRV_Channel/SRV_Channel_config.h>
#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

47
libraries/AP_Servo_Telem/LogStructure.h Normal file
View File

@ -0,0 +1,47 @@
#pragma once
#include "AP_Servo_Telem_config.h"
#include <AP_Logger/LogStructure.h>
#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