Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add AP_Servo_Telem #28651

Merged
merged 12 commits into from
Dec 2, 2024
Merged

Add AP_Servo_Telem #28651

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
2157993
AP_Logger: add new unit `t` for torque in N.m
IamPete1 Nov 16, 2024
65b8963
AP_DroneCAN: send incomming servo telem data to new `AP_Servo_Telem` lib
IamPete1 Nov 16, 2024
365f9c8
AP_PiccoloCAN: send incomming servo telem data to new `AP_Servo_Telem…
IamPete1 Nov 16, 2024
57c0d3f
AP_Volz_Protocol: send incomming servo telem data to new `AP_Servo_Te…
IamPete1 Nov 16, 2024
c6ffd4b
AP_Logger: remove `Write_ServoStatus` and `CSRV` definition
IamPete1 Nov 16, 2024
ac4d5ac
add `AP_Servo_Telem`
IamPete1 Nov 16, 2024
44c25e9
AP_Vechicle: add `AP_Servo_Telem` and call at 50Hz
IamPete1 Nov 16, 2024
dfe52d7
Tools: ardupilotwaf: add `AP_Servo_Telem` to common vehicle libraries
IamPete1 Nov 16, 2024
09222cc
Tools: scripts: build options and extract features: add `AP_Servo_Telem`
IamPete1 Nov 16, 2024
648688b
AP_Periph: added simulation of DroneCAN servo status
tridge Nov 18, 2024
e2ab42d
AP_DroneCAN: actuator status is measured, not commanded
tridge Nov 18, 2024
e8c5252
AP_ServoTelem: added active mask
tridge Nov 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions Tools/AP_Periph/AP_Periph.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -568,6 +568,15 @@ class AP_Periph_FW {
uint16_t pool_peak_percent();
void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue);

#if AP_SIM_ENABLED
// update simulation of servos
void sim_update_actuator(uint8_t actuator_id);
struct {
uint32_t mask;
uint32_t last_send_ms;
} sim_actuator;
#endif

struct dronecan_protocol_t {
CanardInstance canard;
uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)];
Expand Down
51 changes: 51 additions & 0 deletions Tools/AP_Periph/rc_out.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,9 @@
#include <AP_HAL/AP_HAL.h>
#ifdef HAL_PERIPH_ENABLE_RC_OUT
#include "AP_Periph.h"
#if AP_SIM_ENABLED
#include <dronecan_msgs.h>
#endif

// magic value from UAVCAN driver packet
// dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan
Expand Down Expand Up @@ -112,6 +115,9 @@ void AP_Periph_FW::rcout_srv_unitless(uint8_t actuator_id, const float command_v
SRV_Channels::set_output_norm(function, command_value);

rcout_has_new_data_to_update = true;
#if AP_SIM_ENABLED
sim_update_actuator(actuator_id);
#endif
#endif
}

Expand All @@ -122,6 +128,9 @@ void AP_Periph_FW::rcout_srv_PWM(uint8_t actuator_id, const float command_value)
SRV_Channels::set_output_pwm(function, uint16_t(command_value+0.5));

rcout_has_new_data_to_update = true;
#if AP_SIM_ENABLED
sim_update_actuator(actuator_id);
#endif
#endif
}

Expand Down Expand Up @@ -172,4 +181,46 @@ void AP_Periph_FW::rcout_update()
#endif
}

#if AP_SIM_ENABLED
/*
update simulation of servos, sending actuator status
*/
void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id)
{
sim_actuator.mask |= 1U << actuator_id;

// send status at 10Hz
const uint32_t period_ms = 100;
const uint32_t now_ms = AP_HAL::millis();

if (now_ms - sim_actuator.last_send_ms < period_ms) {
return;
}
sim_actuator.last_send_ms = now_ms;

for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if ((sim_actuator.mask & (1U<<i)) == 0) {
continue;
}
const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1);
uavcan_equipment_actuator_Status pkt {};
pkt.actuator_id = i;
// assume 45 degree angle for simulation
pkt.position = radians(SRV_Channels::get_output_norm(function) * 45);
pkt.force = 0;
pkt.speed = 0;
pkt.power_rating_pct = UAVCAN_EQUIPMENT_ACTUATOR_STATUS_POWER_RATING_PCT_UNKNOWN;

uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE];
uint16_t total_size = uavcan_equipment_actuator_Status_encode(&pkt, buffer, !canfdout());

canard_broadcast(UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE,
UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
}
#endif // AP_SIM_ENABLED

#endif // HAL_PERIPH_ENABLE_RC_OUT
1 change: 1 addition & 0 deletions Tools/ardupilotwaf/ardupilotwaf.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@
'AP_EFI',
'AP_Hott_Telem',
'AP_ESC_Telem',
'AP_Servo_Telem',
'AP_Stats',
'AP_GyroFFT',
'AP_RCTelemetry',
Expand Down
1 change: 1 addition & 0 deletions Tools/scripts/build_options.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -404,6 +404,7 @@ def config_option(self):
Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'Enable SDP3X AIRSPEED', 0, 'AIRSPEED'),
Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'Enable DroneCAN AIRSPEED', 0, 'AIRSPEED,DroneCAN'), # NOQA: E501

Feature('Actuators', 'Servo telem', 'AP_SERVO_TELEM_ENABLED', 'Enable servo telemetry library', 0, None),
Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None),
Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, "DroneCAN,Volz"), # noqa: E501
Feature('Actuators', 'RobotisServo', 'AP_ROBOTISSERVO_ENABLED', 'Enable RobotisServo protocol', 0, None),
Expand Down
1 change: 1 addition & 0 deletions Tools/scripts/extract_features.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -190,6 +190,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P<type>.*)::_process_byte\b',),
('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P<type>.*)::process_pulse\b',),

('AP_SERVO_TELEM_ENABLED', r'AP_Servo_Telem::update\b',),
('AP_VOLZ_ENABLED', r'AP_Volz_Protocol::init\b',),
('AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', r'AP_DroneCAN::handle_actuator_status_Volz\b',),
('AP_ROBOTISSERVO_ENABLED', r'AP_RobotisServo::init\b',),
Expand Down
104 changes: 63 additions & 41 deletions libraries/AP_DroneCAN/AP_DroneCAN.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@
#include <AP_OpenDroneID/AP_OpenDroneID.h>
#include <AP_Mount/AP_Mount_Xacti.h>
#include <string.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>

#if AP_DRONECAN_SERIAL_ENABLED
#include "AP_DroneCAN_serial.h"
Expand Down Expand Up @@ -1379,62 +1380,83 @@ void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const
/*
handle actuator status message
*/
#if AP_SERVO_TELEM_ENABLED
void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg)
{
#if HAL_LOGGING_ENABLED
// log as CSRV message
AP::logger().Write_ServoStatus(AP_HAL::micros64(),
msg.actuator_id,
msg.position,
msg.force,
msg.speed,
msg.power_rating_pct,
0, 0, 0, 0, 0, 0);
#endif
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem == nullptr) {
return;
}

const AP_Servo_Telem::TelemetryData telem_data {
.measured_position = ToDeg(msg.position),
.force = msg.force,
.speed = msg.speed,
.duty_cycle = msg.power_rating_pct,
.valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::FORCE |
AP_Servo_Telem::TelemetryData::Types::SPEED |
AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE
};

servo_telem->update_telem_data(msg.actuator_id, telem_data);
}
#endif

#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
/*
handle himark ServoInfo message
*/
void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg)
{
#if HAL_LOGGING_ENABLED
// log as CSRV message
AP::logger().Write_ServoStatus(AP_HAL::micros64(),
msg.servo_id,
msg.pos_sensor*0.01,
0,
0,
0,
msg.pos_cmd*0.01,
msg.voltage*0.01,
msg.current*0.01,
msg.motor_temp*0.2-40,
msg.pcb_temp*0.2-40,
msg.error_status);
#endif
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem == nullptr) {
return;
}

const AP_Servo_Telem::TelemetryData telem_data {
.command_position = msg.pos_cmd * 0.01,
.measured_position = msg.pos_sensor * 0.01,
.voltage = msg.voltage * 0.01,
.current = msg.current * 0.01,
.motor_temperature_cdeg = int16_t(((msg.motor_temp * 0.2) - 40) * 100),
.pcb_temperature_cdeg = int16_t(((msg.pcb_temp * 0.2) - 40) * 100),
.status_flags = msg.error_status,
.valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
AP_Servo_Telem::TelemetryData::Types::CURRENT |
AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
AP_Servo_Telem::TelemetryData::Types::PCB_TEMP |
AP_Servo_Telem::TelemetryData::Types::STATUS
};

servo_telem->update_telem_data(msg.servo_id, telem_data);
}
#endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT

#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg)
{
#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming(
"CVOL",
"TimeUS,Id,Pos,Cur,V,Pow,T",
"s#dAv%O",
"F-00000",
"QBfffBh",
AP_HAL::micros64(),
msg.actuator_id,
ToDeg(msg.actual_position),
msg.current * 0.025f,
msg.voltage * 0.2f,
uint8_t(msg.motor_pwm * (100.0/255.0)),
int16_t(msg.motor_temperature) - 50);
#endif
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem == nullptr) {
return;
}

const AP_Servo_Telem::TelemetryData telem_data {
.measured_position = ToDeg(msg.actual_position),
.voltage = msg.voltage * 0.2,
.current = msg.current * 0.025,
.duty_cycle = msg.motor_pwm * (100.0/255.0),
.motor_temperature_cdeg = (int16_t(msg.motor_temperature) - 50)) * 100,
.valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
AP_Servo_Telem::TelemetryData::Types::CURRENT |
AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE |
AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP
};

servo_telem->update_telem_data(msg.actuator_id, telem_data);
}
#endif

Expand Down
17 changes: 14 additions & 3 deletions libraries/AP_DroneCAN/AP_DroneCAN.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include <dronecan_msgs.h>
#include <AP_SerialManager/AP_SerialManager_config.h>
#include <AP_Relay/AP_Relay_config.h>
#include <AP_Servo_Telem/AP_Servo_Telem_config.h>

#ifndef DRONECAN_SRV_NUMBER
#define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS
Expand Down Expand Up @@ -329,8 +330,10 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
Canard::ObjCallback<AP_DroneCAN, ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report};
Canard::Subscriber<ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_listener{traffic_report_cb, _driver_index};

#if AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_actuator_Status> actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status};
Canard::Subscriber<uavcan_equipment_actuator_Status> actuator_status_listener{actuator_status_cb, _driver_index};
#endif

Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
Expand All @@ -343,7 +346,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};

#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo};
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

himark servo was just broken, there was a function but the callback was never registered.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this must have been a regression, it worked when it went in

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In the change of DroneCAN library I think, I fixed the same thing for the volz message, #27924 we should probably check all the rest....

Copy link
Member Author

@IamPete1 IamPete1 Nov 23, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have checked the rest, we are OK no others missing.

Canard::Subscriber<com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb_listener{himark_servo_ServoInfo_cb, _driver_index};
#endif
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz};
Canard::Subscriber<com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index};
#endif
Expand Down Expand Up @@ -401,15 +408,19 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT

#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
#endif

// incoming button handling
void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg);
#if AP_SERVO_TELEM_ENABLED
void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
#endif
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED
void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
#endif
void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
#if AP_EXTENDED_ESC_TELEM_ENABLED
void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);
Expand Down
2 changes: 0 additions & 2 deletions libraries/AP_Logger/AP_Logger.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -262,8 +262,6 @@ class AP_Logger
void Write_Radio(const mavlink_radio_t &packet);
void Write_Message(const char *message);
void Write_MessageF(const char *fmt, ...);
void Write_ServoStatus(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);
void Write_Compass();
void Write_Mode(uint8_t mode, const ModeReason reason);

Expand Down
25 changes: 0 additions & 25 deletions libraries/AP_Logger/LogFile.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -452,31 +452,6 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason)
return WriteCriticalBlock(&pkt, sizeof(pkt));
}

/*
write servo status from CAN servo
*/
void AP_Logger::Write_ServoStatus(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)
{
const struct log_CSRV pkt {
LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
time_us : time_us,
id : id,
position : position,
force : force,
speed : speed,
power_pct : power_pct,
pos_cmd : pos_cmd,
voltage : voltage,
current : current,
mot_temp : mot_temp,
pcb_temp : pcb_temp,
error : error,
};
WriteBlock(&pkt, sizeof(pkt));
}


// Write a Yaw PID packet
void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info)
{
Expand Down
Loading
Loading