mirror of https://github.com/ArduPilot/ardupilot
add `AP_Servo_Telem`
This commit is contained in:
Iampete1
Andrew Tridgell
parent
d0bbc02995
commit
e003cc511d
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue