按推荐建议,继续执行
结论:已将 LinuxCNC 5axiskins 及 switchkins 相关源码纳入 vendored manifest、native probe 和文档验证链,完整 native 验证通过。
This commit is contained in:
@@ -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;
|
||||
}
|
||||
@@ -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> ¶ms()
|
||||
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)
|
||||
{
|
||||
|
||||
@@ -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 **);
|
||||
|
||||
Reference in New Issue
Block a user