Kinematics issue with master branch (2.8 pre1 2959)

/********************************************************************
* Description: XZABkins.c
*   Derived from trivkins for 3 axis Cartesian machine
*   Established for 4-axis lathe (XZAB) where A rotates around X in Z-Yplane and B rotates around Y in Z-Xplane
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* License: GPL Version 2
*    
* Copyright (c) 2009 All rights reserved.
*
********************************************************************/

#include "motion.h"
#include "posemath.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "rtapi_math.h"
#include "rtapi_string.h"

struct haldata { 
    hal_float_t *Tool_offset;
    hal_s32_t *Tool_number;
    hal_float_t *J0;
    hal_float_t *J1;
    hal_float_t *J3;
    hal_float_t *Px;
    hal_float_t *Pz;
    hal_float_t *Pb;
    hal_float_t *FwdRad;
    hal_float_t *InvRad;

} *haldata;

hal_float_t Last_Tool_offset = 0.00;
hal_s32_t Last_Tool_number = 0;


int kinematicsForward(const double *joints,
		      EmcPose * pos,
		      const KINEMATICS_FORWARD_FLAGS * fflags,
		      KINEMATICS_INVERSE_FLAGS * iflags)
{
    double b_rad;
    double dT = *(haldata->Tool_offset);
    long lTN = *(haldata->Tool_number);
    double b_adj = 0.00;
    
    
    if (lTN == 100 && dT != 0.00) {
	b_adj = 90.00;
    }
    b_rad = (joints[3] + b_adj)*TO_RAD; /* B-axis offset */
    
    pos->tran.x = joints[0] - (dT * sin(b_rad));
    pos->tran.z = joints[1] - (dT * cos(b_rad)) + dT; 
    pos->a = joints[2];
    pos->b = joints[3] + b_adj ;

    *(haldata->Px) = pos->tran.x;
    *(haldata->Pz) = pos->tran.z;
    *(haldata->Pb) = pos->b;
    *(haldata->InvRad) = b_rad;

    return 0;
}

int kinematicsInverse(const EmcPose * pos,
		      double *joints,
		      const KINEMATICS_INVERSE_FLAGS * iflags,
		      KINEMATICS_FORWARD_FLAGS * fflags)
{
    double b_rad;
    double dT = *(haldata->Tool_offset);
    long lTN = *(haldata->Tool_number);
    double b_adj = 0.00;

    if (lTN == 100 && dT != 0.00) {
	b_adj = 90.00;
    }
    b_rad = (pos->b)*TO_RAD; /* B-axis offset */
    
    joints[0] = pos->tran.x + (dT * sin(b_rad));
    joints[1] = (pos->tran.z - dT) +  (dT * cos(b_rad)) ;
    joints[2] = pos->a;
    joints[3] = pos->b - b_adj;

    *(haldata->J0) = joints[0];
    *(haldata->J1) = joints[1];
    *(haldata->J3) = joints[3];
    *(haldata->FwdRad) = b_rad;

    return 0;
}

/* implemented for these kinematics as giving joints preference */
int kinematicsHome(EmcPose * world,
		   double *joint,
		   KINEMATICS_FORWARD_FLAGS * fflags,
		   KINEMATICS_INVERSE_FLAGS * iflags)
{
    *fflags = 0;
    *iflags = 0;

    return kinematicsForward(joint, world, fflags, iflags);
}

static KINEMATICS_TYPE ktype = -1;

KINEMATICS_TYPE kinematicsType()
{
    return ktype;
}

static char *kinstype = "b"; // use KINEMATICS_BOTH
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");

EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");

int comp_id;
int rtapi_app_main(void) {
    int res = 0;
    comp_id = hal_init("XZABkins");
    if(comp_id < 0) return comp_id;

    haldata = hal_malloc(sizeof(struct haldata));

    switch (*kinstype) {
      case 'b': case 'B': ktype = KINEMATICS_BOTH;         break;
      case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
      case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
      case '1': default:  ktype = KINEMATICS_IDENTITY;
    }

    if((res = hal_pin_float_new("XZABkins.Tool-offset", HAL_IN, &(haldata->Tool_offset),
      comp_id)) < 0) goto error;
    if((res = hal_pin_s32_new("XZABkins.Tool-number", HAL_IN, &(haldata->Tool_number),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.Joint0", HAL_OUT, &(haldata->J0),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.Joint1", HAL_OUT, &(haldata->J1),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.Joint3", HAL_OUT, &(haldata->J3),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.PosX", HAL_OUT, &(haldata->Px),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.PosZ", HAL_OUT, &(haldata->Pz),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.PosB", HAL_OUT, &(haldata->Pb),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.FwdRad", HAL_OUT, &(haldata->FwdRad),
      comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("XZABkins.InvRad", HAL_OUT, &(haldata->InvRad),
      comp_id)) < 0) goto error;

    hal_ready(comp_id);
    return 0;

error:
    hal_exit(comp_id);
    return res;
}

void rtapi_app_exit(void) { hal_exit(comp_id); }