按推荐建议,继续执行

结论:已将 LinuxCNC 5axiskins 及 switchkins 相关源码纳入 vendored manifest、native probe 和文档验证链,完整 native 验证通过。
This commit is contained in:
2026-06-07 19:08:40 +08:00
parent 55f6659993
commit 1b2f95b3d1
15 changed files with 1129 additions and 10 deletions

View File

@@ -0,0 +1,108 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
#include "emc/motion/emcmotcfg.h"
int rtapi_app_main(void);
void rtapi_app_exit(void);
namespace {
int near(double actual, double expected)
{
return std::fabs(actual - expected) < 1e-9;
}
int near_pose(const EmcPose &actual, const EmcPose &expected)
{
return near(actual.tran.x, expected.tran.x) &&
near(actual.tran.y, expected.tran.y) &&
near(actual.tran.z, expected.tran.z) &&
near(actual.b, expected.b) &&
near(actual.c, expected.c) &&
near(actual.w, expected.w);
}
int near_joints(const double *actual, const double *expected)
{
for (int index = 0; index < 6; ++index) {
if (!near(actual[index], expected[index])) {
return 0;
}
}
return 1;
}
void print_pose(const char *prefix, const EmcPose &pose)
{
std::cout << prefix << "_xyz="
<< pose.tran.x << ","
<< pose.tran.y << ","
<< pose.tran.z << "\n";
std::cout << prefix << "_bcw="
<< pose.b << ","
<< pose.c << ","
<< pose.w << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzbcw="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << ","
<< joints[5] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "fiveaxis_init=" << init_rc << "\n";
std::cout << "fiveaxis_type=" << kinematicsType() << "\n";
std::cout << "fiveaxis_switchable=" << kinematicsSwitchable() << "\n";
double joints[EMCMOT_MAX_JOINTS]{};
joints[0] = 10.0;
joints[1] = 20.0;
joints[2] = 30.0;
joints[3] = 45.0;
joints[4] = 30.0;
joints[5] = 5.0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
EmcPose forward_pose{};
const int forward_rc = kinematicsForward(joints, &forward_pose, &fflags, &iflags);
std::cout << "fiveaxis_forward=" << forward_rc << "\n";
print_pose("fiveaxis_forward", forward_pose);
double inverse_joints[EMCMOT_MAX_JOINTS]{};
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
std::cout << "fiveaxis_inverse=" << inverse_rc << "\n";
print_joints("fiveaxis_inverse", inverse_joints);
std::cout << "fiveaxis_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
const int switch_rc = kinematicsSwitch(1);
std::cout << "fiveaxis_switch_identity=" << switch_rc << "\n";
EmcPose identity_pose{};
const int identity_forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags);
std::cout << "fiveaxis_identity_forward=" << identity_forward_rc << "\n";
print_pose("fiveaxis_identity", identity_pose);
EmcPose expected_identity{};
expected_identity.tran.x = joints[0];
expected_identity.tran.y = joints[1];
expected_identity.tran.z = joints[2];
expected_identity.b = joints[3];
expected_identity.c = joints[4];
expected_identity.w = joints[5];
std::cout << "fiveaxis_identity_near=" << near_pose(identity_pose, expected_identity) << "\n";
rtapi_app_exit();
return 0;
}

View File

@@ -1,7 +1,11 @@
#include "linuxcnc_hal_adapter.hh"
#include <cstdarg>
#include <cstdio>
#include <new>
#include <string>
#include <unordered_map>
#include <vector>
namespace {
@@ -29,6 +33,12 @@ std::unordered_map<std::string, HalEntry> &params()
return values;
}
std::vector<void *> &hal_allocations()
{
static std::vector<void *> values;
return values;
}
std::unordered_map<std::string, HalEntry> &values_for(standalone::HalValueKind kind)
{
switch (kind) {
@@ -96,6 +106,150 @@ int hal_exit(int)
return 0;
}
void *hal_malloc(long int size)
{
if (size <= 0) {
return nullptr;
}
void *ptr = ::operator new(static_cast<std::size_t>(size), std::nothrow);
if (ptr) {
hal_allocations().push_back(ptr);
}
return ptr;
}
int hal_pin_bit_new(const char *, hal_pin_dir_t, hal_bit_t **data_ptr_addr, int)
{
if (!data_ptr_addr) {
return -1;
}
*data_ptr_addr = new hal_bit_t{};
return 0;
}
int hal_pin_float_new(const char *, hal_pin_dir_t, hal_float_t **data_ptr_addr, int)
{
if (!data_ptr_addr) {
return -1;
}
*data_ptr_addr = new hal_float_t{};
return 0;
}
int hal_pin_u32_new(const char *, hal_pin_dir_t, hal_u32_t **data_ptr_addr, int)
{
if (!data_ptr_addr) {
return -1;
}
*data_ptr_addr = new hal_u32_t{};
return 0;
}
int hal_pin_s32_new(const char *, hal_pin_dir_t, hal_s32_t **data_ptr_addr, int)
{
if (!data_ptr_addr) {
return -1;
}
*data_ptr_addr = new hal_s32_t{};
return 0;
}
int hal_pin_u64_new(const char *, hal_pin_dir_t, hal_u64_t **data_ptr_addr, int)
{
if (!data_ptr_addr) {
return -1;
}
*data_ptr_addr = new hal_u64_t{};
return 0;
}
int hal_pin_s64_new(const char *, hal_pin_dir_t, hal_s64_t **data_ptr_addr, int)
{
if (!data_ptr_addr) {
return -1;
}
*data_ptr_addr = new hal_s64_t{};
return 0;
}
template <typename T, int (*Create)(const char *, hal_pin_dir_t, T **, int)>
int hal_pin_newf_impl(hal_pin_dir_t dir, T **data_ptr_addr, int comp_id, const char *fmt,
va_list ap)
{
if (!fmt) {
return -1;
}
char name[HAL_NAME_LEN * 2]{};
vsnprintf(name, sizeof(name), fmt, ap);
return Create(name, dir, data_ptr_addr, comp_id);
}
int hal_pin_bit_newf(hal_pin_dir_t dir, hal_bit_t **data_ptr_addr, int comp_id,
const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
const int rc = hal_pin_newf_impl<hal_bit_t, hal_pin_bit_new>(
dir, data_ptr_addr, comp_id, fmt, ap);
va_end(ap);
return rc;
}
int hal_pin_float_newf(hal_pin_dir_t dir, hal_float_t **data_ptr_addr, int comp_id,
const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
const int rc = hal_pin_newf_impl<hal_float_t, hal_pin_float_new>(
dir, data_ptr_addr, comp_id, fmt, ap);
va_end(ap);
return rc;
}
int hal_pin_u32_newf(hal_pin_dir_t dir, hal_u32_t **data_ptr_addr, int comp_id,
const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
const int rc = hal_pin_newf_impl<hal_u32_t, hal_pin_u32_new>(
dir, data_ptr_addr, comp_id, fmt, ap);
va_end(ap);
return rc;
}
int hal_pin_s32_newf(hal_pin_dir_t dir, hal_s32_t **data_ptr_addr, int comp_id,
const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
const int rc = hal_pin_newf_impl<hal_s32_t, hal_pin_s32_new>(
dir, data_ptr_addr, comp_id, fmt, ap);
va_end(ap);
return rc;
}
int hal_pin_u64_newf(hal_pin_dir_t dir, hal_u64_t **data_ptr_addr, int comp_id,
const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
const int rc = hal_pin_newf_impl<hal_u64_t, hal_pin_u64_new>(
dir, data_ptr_addr, comp_id, fmt, ap);
va_end(ap);
return rc;
}
int hal_pin_s64_newf(hal_pin_dir_t dir, hal_s64_t **data_ptr_addr, int comp_id,
const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
const int rc = hal_pin_newf_impl<hal_s64_t, hal_pin_s64_new>(
dir, data_ptr_addr, comp_id, fmt, ap);
va_end(ap);
return rc;
}
int hal_get_pin_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr,
bool *connected)
{

View File

@@ -2,6 +2,8 @@
#include <stdbool.h>
#include "rtapi.h"
#define HAL_NAME_LEN 64
typedef enum {
@@ -14,6 +16,13 @@ typedef enum {
HAL_U64
} hal_type_t;
typedef enum {
HAL_DIR_UNSPECIFIED = -1,
HAL_IN = 16,
HAL_OUT = 32,
HAL_IO = (HAL_IN | HAL_OUT),
} hal_pin_dir_t;
typedef union {
bool b;
double f;
@@ -33,6 +42,19 @@ typedef unsigned long long hal_u64_t;
int hal_init(const char *);
int hal_ready(int);
int hal_exit(int);
void *hal_malloc(long int);
int hal_pin_bit_new(const char *, hal_pin_dir_t, hal_bit_t **, int);
int hal_pin_float_new(const char *, hal_pin_dir_t, hal_float_t **, int);
int hal_pin_u32_new(const char *, hal_pin_dir_t, hal_u32_t **, int);
int hal_pin_s32_new(const char *, hal_pin_dir_t, hal_s32_t **, int);
int hal_pin_u64_new(const char *, hal_pin_dir_t, hal_u64_t **, int);
int hal_pin_s64_new(const char *, hal_pin_dir_t, hal_s64_t **, int);
int hal_pin_bit_newf(hal_pin_dir_t, hal_bit_t **, int, const char *, ...);
int hal_pin_float_newf(hal_pin_dir_t, hal_float_t **, int, const char *, ...);
int hal_pin_u32_newf(hal_pin_dir_t, hal_u32_t **, int, const char *, ...);
int hal_pin_s32_newf(hal_pin_dir_t, hal_s32_t **, int, const char *, ...);
int hal_pin_u64_newf(hal_pin_dir_t, hal_u64_t **, int, const char *, ...);
int hal_pin_s64_newf(hal_pin_dir_t, hal_s64_t **, int, const char *, ...);
int hal_get_pin_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
int hal_get_signal_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
int hal_get_param_value_by_name(const char *, hal_type_t *, hal_data_u **);