35#include "uavcan.equipment.esc.Status.h"
36#include "uavcan.equipment.esc.RawCommand.h"
37#include "uavcan.equipment.actuator.Status.h"
38#include "uavcan.equipment.actuator.ArrayCommand.h"
39#include "uavcan.equipment.device.Temperature.h"
42#ifndef UAVCAN_ACTUATORS_USE_CURRENT
43#define UAVCAN_ACTUATORS_USE_CURRENT TRUE
46#define UNITLESS UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS
47#define POSITION UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_POSITION
48#define FORCE UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_FORCE
49#define SPEED UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_SPEED
50#define PWM UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_PWM
67#ifdef SERVOS_UAVCAN1_NB
75#ifdef SERVOS_UAVCAN2_NB
84#ifdef SERVOS_UAVCAN1CMD_NB
86#ifndef ACTUATOR_UAVCAN1_CMD_TYPES
87#define ACTUATOR_UAVCAN1_CMD_TYPES {[0 ... SERVOS_UAVCAN1CMD_NB - 1] = UNITLESS}
90#ifndef ACTUATOR_UAVCAN1_CMD_FACTORS
91#define ACTUATOR_UAVCAN1_CMD_FACTORS {[0 ... SERVOS_UAVCAN1CMD_NB - 1] = (float)MAX_PPRZ}
94static const uint8_t uavcan1_cmd_types[SERVOS_UAVCAN1CMD_NB] = ACTUATOR_UAVCAN1_CMD_TYPES;
95static const float uavcan1_cmd_factors[SERVOS_UAVCAN1CMD_NB] = ACTUATOR_UAVCAN1_CMD_FACTORS;
106#ifdef SERVOS_UAVCAN2CMD_NB
107#ifndef ACTUATOR_UAVCAN2_CMD_TYPES
108#define ACTUATOR_UAVCAN2_CMD_TYPES {[0 ... SERVOS_UAVCAN2CMD_NB - 1] = UNITLESS}
111#ifndef ACTUATOR_UAVCAN2_CMD_FACTORS
112#define ACTUATOR_UAVCAN2_CMD_FACTORS {[0 ... SERVOS_UAVCAN2CMD_NB - 1] = (float)MAX_PPRZ}
115static const uint8_t uavcan2_cmd_types[SERVOS_UAVCAN2CMD_NB] = ACTUATOR_UAVCAN2_CMD_TYPES;
116static const float uavcan2_cmd_factors[SERVOS_UAVCAN2CMD_NB] = ACTUATOR_UAVCAN2_CMD_FACTORS;
127#if SERVOS_UAVCAN1CMD_NB > SERVOS_UAVCAN1_NB
128#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1CMD_NB
130#elif defined(SERVOS_UAVCAN1_NB)
131#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1_NB
135#if SERVOS_UAVCAN2CMD_NB > SERVOS_UAVCAN2_NB
136#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2CMD_NB
138#elif defined(SERVOS_UAVCAN2_NB)
139#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2_NB
144#define UAVCAN_CMD_UNUSED (MIN_PPRZ-1)
152#if PERIODIC_TELEMETRY
158#ifdef UAVCAN_ACTUATORS_TELEM_IDX
160 static const uint8_t telem_idx[] = UAVCAN_ACTUATORS_TELEM_IDX;
167 if (idx_cur >= (
sizeof(telem_idx) /
sizeof(telem_idx[0]))) {
177#ifdef UAVCAN1_TELEM_NB
178 if (
idx < UAVCAN1_TELEM_NB) {
179 return uavcan1_telem[
idx].
set ? &uavcan1_telem[
idx] : NULL;
181 offset += UAVCAN1_TELEM_NB;
183#ifdef UAVCAN2_TELEM_NB
200#ifdef UAVCAN1_TELEM_NB
202 if(uavcan1_telem[i].
set) {
205 return &uavcan1_telem[i];
210 offset += UAVCAN1_TELEM_NB;
212#ifdef UAVCAN2_TELEM_NB
214 if(uavcan2_telem[i].
set) {
217 return &uavcan2_telem[i];
222 offset += UAVCAN2_TELEM_NB;
255#ifdef UAVCAN1_TELEM_NB
256 if (iface == &uavcan1 &&
idx < UAVCAN1_TELEM_NB) {
257 telem = &uavcan1_telem[
idx];
260#ifdef UAVCAN2_TELEM_NB
261 if (iface == &uavcan2 &&
idx < UAVCAN2_TELEM_NB) {
262 telem = &uavcan2_telem[
idx];
272#ifdef UAVCAN1_TELEM_NB
273 if(iface == &uavcan1) {
274#ifdef SERVOS_UAVCAN1_NB
276 actuator_idx = get_servo_idx_UAVCAN1(
idx);
278#ifdef SERVOS_UAVCAN1CMD_NB
280 if(
idx < SERVOS_UAVCAN1CMD_NB) {
281 uint8_t cmd_idx = get_servo_idx_UAVCAN1CMD(
idx);
283 actuator_idx = cmd_idx;
289#ifdef UAVCAN2_TELEM_NB
290 if(iface == &uavcan2) {
291#ifdef SERVOS_UAVCAN2_NB
293 actuator_idx = get_servo_idx_UAVCAN2(
idx);
295#ifdef SERVOS_UAVCAN2CMD_NB
297 if(
idx < SERVOS_UAVCAN2CMD_NB) {
298 uint8_t cmd_idx = get_servo_idx_UAVCAN2CMD(
idx);
300 actuator_idx = cmd_idx;
316 struct uavcan_equipment_esc_Status msg;
317 if(uavcan_equipment_esc_Status_decode(
transfer, &msg)) {
331 telem->
energy = msg.error_count;
335 telem->
rpm = msg.rpm;
345 feedback.
rpm = telem->
rpm;
351#if UAVCAN_ACTUATORS_USE_CURRENT
354#ifdef UAVCAN1_TELEM_NB
355 for (
uint8_t i = 0; i < UAVCAN1_TELEM_NB; ++i) {
359#ifdef UAVCAN2_TELEM_NB
360 for (
uint8_t i = 0; i < UAVCAN2_TELEM_NB; ++i) {
373 struct uavcan_equipment_actuator_Status msg;
374 if(uavcan_equipment_actuator_Status_decode(
transfer, &msg)) {
393 feedback.
force = msg.force;
394 feedback.
set.
force = !isnan(msg.force);
395 feedback.
speed = msg.speed;
396 feedback.
set.
speed = !isnan(msg.speed);
397 if (msg.power_rating_pct <= 100) {
398 feedback.
power_pct = msg.power_rating_pct;
412 struct uavcan_equipment_device_Temperature msg;
413 if(uavcan_equipment_device_Temperature_decode(
transfer, &msg)) {
447#if PERIODIC_TELEMETRY
452#ifdef SERVOS_UAVCAN1CMD_NB
453 for(
uint8_t i = 0; i < SERVOS_UAVCAN1CMD_NB; i++)
456#ifdef SERVOS_UAVCAN2CMD_NB
457 for(
uint8_t i = 0; i < SERVOS_UAVCAN2CMD_NB; i++)
474 struct uavcan_equipment_esc_RawCommand msg = {0};
476 for (
uint8_t i = 0; i < nb; i++) {
477 msg.cmd.data[i] = values[i];
481 uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
482 uint32_t total_size = uavcan_equipment_esc_RawCommand_encode(&msg, buffer);
485 uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID,
486 CANARD_TRANSFER_PRIORITY_HIGH, buffer, total_size);
495 const uint8_t *cmd_types = NULL;
496 const float *cmd_factors = NULL;
498#ifdef SERVOS_UAVCAN1CMD_NB
499 if(iface == &uavcan1) {
500 cmd_types = uavcan1_cmd_types;
501 cmd_factors = uavcan1_cmd_factors;
504#ifdef SERVOS_UAVCAN2CMD_NB
505 if(iface == &uavcan2) {
506 cmd_types = uavcan2_cmd_types;
507 cmd_factors = uavcan2_cmd_factors;
511 if (cmd_types == NULL || cmd_factors == NULL) {
515 struct uavcan_equipment_actuator_ArrayCommand msg = {0};
516 msg.commands.len = 0;
519 for (
uint8_t i = 0; i < nb; i++) {
524 msg.commands.data[msg.commands.len].actuator_id = i;
525 msg.commands.data[msg.commands.len].command_type = cmd_types[i];
526 msg.commands.data[msg.commands.len].command_value = (float)values[i] / cmd_factors[i];
531 if (msg.commands.len == 0) {
536 uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE];
537 uint32_t total_size = uavcan_equipment_actuator_ArrayCommand_encode(&msg, buffer);
540 uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID,
541 CANARD_TRANSFER_PRIORITY_HIGH, buffer, total_size);
Main include for ABI (AirBorneInterface).
#define ACT_FEEDBACK_UAVCAN_ID
void actuators_uavcan1_set(uint8_t idx, int16_t value)
int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB]
Stub file needed per uavcan interface because of generator.
int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB]
Stub file needed per uavcan interface because of generator.
void actuators_uavcan1cmd_set(uint8_t idx, int16_t value)
void actuators_uavcan2_set(uint8_t idx, int16_t value)
int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB]
Stub file needed per interface because of generator.
void actuators_uavcan2cmd_set(uint8_t idx, int16_t value)
int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB]
Stub file needed per uavcan interface because of generator.
static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_device *dev)
void actuators_uavcan_init(struct uavcan_iface_t *iface)
Initialize an uavcan interface.
static void actuators_uavcan_device_temperature_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Whevener an DEVICE_TEMPERATURE message from the EQUIPMENT group is received.
static uavcan_event actuator_status_ev
static struct actuators_uavcan_telem_t * actuators_uavcan_next_telem(void)
void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
Commit actuator values to the uavcan interface (EQUIPMENT_ESC_RAWCOMMAND).
static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Whevener an ESC_STATUS message from the EQUIPMENT group is received.
static uavcan_event device_temperature_ev
static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Whevener an ACTUATOR_STATUS message from the EQUIPMENT group is received.
static bool actuators_uavcan_initialized
void actuators_uavcan_cmd_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
Commit actuator values to the uavcan interface (EQUIPMENT_ACTUATOR_ARRAYCOMMAND).
static struct actuators_uavcan_telem_t * get_actuator_telem(struct uavcan_iface_t *iface, uint8_t idx)
#define UAVCAN_CMD_UNUSED
static uint8_t get_actuator_idx(struct uavcan_iface_t *iface, uint8_t idx)
static uavcan_event esc_status_ev
static struct uart_periph * dev
struct Electrical electrical
Interface for electrical status: supply voltage, current, battery status, etc.
Hardware independent API for actuators (servos, motor controllers).
bool position
Position is set.
struct act_feedback_t::act_feedback_set_t set
Bitset registering what is set as feedback.
uint8_t power_pct
Power rating in percent.
uint8_t idx
General index of the actuators (generated in airframe.h).
float speed
In meter per second or radian per second.
float force
In Newton or Newton metre.
float position
In radians.
bool power
Power rating is set.
double rand_uniform(void)
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void *payload, uint16_t payload_len)
Legacy function Broadcast an uavcan message to a specific interface.
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
Bind to a receiving message from uavcan.
struct uavcan_event_t uavcan_event
static const float offset[]
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
int transfer(const Mat *from, const image_t *to)