mirror of https://github.com/ArduPilot/ardupilot
87 lines
2.8 KiB
C++
87 lines
2.8 KiB
C++
#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;
|
|
|
|
uint32_t active_mask;
|
|
};
|
|
#endif // AP_SERVO_TELEM_ENABLED
|