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

AP_Volz_Protocol: send incomming servo telem data to new `AP_Servo_Telem` lib

This commit is contained in:
Iampete1 2024-11-16 11:12:45 +00:00 committed by Andrew Tridgell
parent 71137dac31
commit 9be1a751da
2 changed files with 31 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
View File

@ -14,7 +14,7 @@
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Servo_Telem/AP_Servo_Telem.h>
// Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default) // Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default)
#define PWM_POSITION_MIN 1000 #define PWM_POSITION_MIN 1000
@ -77,7 +77,7 @@ void AP_Volz_Protocol::init(void)
} }
} }
#if HAL_LOGGING_ENABLED #if AP_SERVO_TELEM_ENABLED
// Request telem data, cycling through each servo and telem item // Request telem data, cycling through each servo and telem item
void AP_Volz_Protocol::request_telem() void AP_Volz_Protocol::request_telem()
{ {
@ -144,7 +144,7 @@ void AP_Volz_Protocol::loop()
hal.scheduler->delay_microseconds(100); hal.scheduler->delay_microseconds(100);
} }
#if HAL_LOGGING_ENABLED #if AP_SERVO_TELEM_ENABLED
// Send a command for each servo, then one telem request // Send a command for each servo, then one telem request
const uint8_t num_servos = __builtin_popcount(bitmask.get()); const uint8_t num_servos = __builtin_popcount(bitmask.get());
if (sent_count < num_servos) { if (sent_count < num_servos) {
@ -162,7 +162,7 @@ void AP_Volz_Protocol::loop()
read_telem(); read_telem();
} }
#else // No logging, send only #else // No telem, send only
send_position_cmd(); send_position_cmd();
#endif #endif
} }
@ -208,7 +208,7 @@ void AP_Volz_Protocol::send_position_cmd()
send_command(cmd); send_command(cmd);
#if HAL_LOGGING_ENABLED #if AP_SERVO_TELEM_ENABLED
{ {
// Update the commanded angle // Update the commanded angle
WITH_SEMAPHORE(telem.sem); WITH_SEMAPHORE(telem.sem);
@ -221,7 +221,7 @@ void AP_Volz_Protocol::send_position_cmd()
} }
} }
#if HAL_LOGGING_ENABLED #if AP_SERVO_TELEM_ENABLED
void AP_Volz_Protocol::process_response(const CMD &cmd) void AP_Volz_Protocol::process_response(const CMD &cmd)
{ {
// Convert to 0 indexed // Convert to 0 indexed
@ -348,7 +348,7 @@ void AP_Volz_Protocol::read_telem()
// Used up all attempts without running out of data. // Used up all attempts without running out of data.
// Really should not end up here // Really should not end up here
} }
#endif // HAL_LOGGING_ENABLED #endif // AP_SERVO_TELEM_ENABLED
// Called each time the servo outputs are sent // Called each time the servo outputs are sent
void AP_Volz_Protocol::update() void AP_Volz_Protocol::update()
@ -376,11 +376,11 @@ void AP_Volz_Protocol::update()
servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm; servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm;
} }
#if HAL_LOGGING_ENABLED #if AP_SERVO_TELEM_ENABLED
// take semaphore and log all channels at 5 Hz // Report telem data
const uint32_t now_ms = AP_HAL::millis(); AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if ((now_ms - telem.last_log_ms) > 200) { if (servo_telem != nullptr) {
telem.last_log_ms = now_ms; const uint32_t now_ms = AP_HAL::millis();
WITH_SEMAPHORE(telem.sem); WITH_SEMAPHORE(telem.sem);
for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) { for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) {
@ -389,37 +389,25 @@ void AP_Volz_Protocol::update()
continue; continue;
} }
// @LoggerMessage: VOLZ const AP_Servo_Telem::TelemetryData telem_data {
// @Description: Volz servo data .command_position = telem.data[i].desired_angle,
// @Field: TimeUS: Time since system startup .measured_position = telem.data[i].angle,
// @Field: I: Instance .voltage = telem.data[i].primary_voltage,
// @Field: Dang: desired angle .current = telem.data[i].primary_current,
// @Field: ang: reported angle .motor_temperature_cdeg = int16_t(telem.data[i].motor_temp_deg * 100),
// @Field: pc: primary supply current .pcb_temperature_cdeg = int16_t(telem.data[i].pcb_temp_deg * 100),
// @Field: sc: secondary supply current .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
// @Field: pv: primary supply voltage AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
// @Field: sv: secondary supply voltage AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
// @Field: mt: motor temperature AP_Servo_Telem::TelemetryData::Types::CURRENT |
// @Field: pt: pcb temperature AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
AP::logger().WriteStreaming("VOLZ", AP_Servo_Telem::TelemetryData::Types::PCB_TEMP
"TimeUS,I,Dang,ang,pc,sc,pv,sv,mt,pt", };
"s#ddAAvvOO",
"F000000000", servo_telem->update_telem_data(i, telem_data);
"QBffffffhh",
AP_HAL::micros64(),
i + 1, // convert to 1 indexed to match actuator IDs and SERVOx numbering
telem.data[i].desired_angle,
telem.data[i].angle,
telem.data[i].primary_current,
telem.data[i].secondary_current,
telem.data[i].primary_voltage,
telem.data[i].secondary_voltage,
telem.data[i].motor_temp_deg,
telem.data[i].pcb_temp_deg
);
} }
} }
#endif // HAL_LOGGING_ENABLED #endif // AP_SERVO_TELEM_ENABLED
} }
// Return the crc for a given command packet // Return the crc for a given command packet

6
libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
View File

@ -44,8 +44,7 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <SRV_Channel/SRV_Channel_config.h> #include <SRV_Channel/SRV_Channel_config.h>
#include <AP_Logger/AP_Logger_config.h> #include <AP_Servo_Telem/AP_Servo_Telem_config.h>
class AP_Volz_Protocol { class AP_Volz_Protocol {
public: public:
@ -110,7 +109,7 @@ private:
AP_Int16 range; AP_Int16 range;
bool initialised; bool initialised;
#if HAL_LOGGING_ENABLED #if AP_SERVO_TELEM_ENABLED
// Request telem data, cycling through each servo and telem item // Request telem data, cycling through each servo and telem item
void request_telem(); void request_telem();
@ -143,7 +142,6 @@ private:
int16_t motor_temp_deg; int16_t motor_temp_deg;
int16_t pcb_temp_deg; int16_t pcb_temp_deg;
} data[NUM_SERVO_CHANNELS]; } data[NUM_SERVO_CHANNELS];
uint32_t last_log_ms;
} telem; } telem;
#endif #endif