/***************************************************************
 * PrForceSensorATI.cpp
 *
 * This is an implementation of the ATI Force Sensor class.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 12/12/97: K.C. Chang: created.
 * 02/22/03: Weyn Ong  : Version for QNX 6.  I/O permissions must be enabled
 *                       for port access.  Check PrATIDefn.h to verify that
 *                       PR_ATI_ADDR_SPACE_SIZE is the correct value.
 */

//------------------------------------------------------------------
// Source Code for ATI ISA FT adapter
//
// 	This file contains sample 'C' Source code routines for interfacing the
//	ATI ISA/FT card.  It is intended for use by non-Windows applications
//	in interfacing the card.
//
//	Windows applications are expected to use the AITFT.DLL Dynamic Link
//	Library (DLL) supplied by ATI and do not require driver source code.  
//
//  This code should compile without errors using Borland C++ V4.51 or later.
//
//  Auther: Jerry Dahl, ATI Industrial Automation
//
//------------------------------------------------------------------

#include "PrATIDefn.h"
#include "PrForceSensorATI.h"
#include <stdio.h>
#include <sys/mman.h>
#include <sys/neutrino.h>

//-------------------------------------------------
// sensor_data command structure
//-------------------------------------------------
class PrForceSensorATIData	
{
 public:
    unsigned short cmd_rc;	  // 0x0100
    unsigned short mode;
    int matrix[6][8];    // 6 x 8 32-bit words
    short sbias[8];      // hardware bias array
    short Mm[8];
    short Bm[8];
    short Mb[8];
    short Bb[8];
    unsigned short number_of_gages;
    unsigned short counts_per_force;
    unsigned short counts_per_torque;
    unsigned short max_force;
    unsigned short max_torque;
    unsigned short maxtemperature;
    unsigned short min_temperature;
    unsigned short units;
    unsigned short serial_number;
};


PrForceSensorATI::PrForceSensorATI()
: PrForceSensorIntf(PR_ATI_CHANNEL_COUNT,PR_ATI_BIT_COUNT)
{
    sensorData_ = new PrForceSensorATIData;
    ReadSensorInfo();
    forcePerCount_ = 1.0/(sensorData_->counts_per_force/10.0);
    torquePerCount_ = 1.0/(sensorData_->counts_per_torque/10.0);

    // Enable permissions for I/O access
    ThreadCtl( _NTO_TCTL_IO, 0 );
    mmap_device_io( PR_ATI_ADDR_SPACE_SIZE, PR_ATI_BASE_ADDR );

    flag(zero());
}

PrForceSensorATI::~PrForceSensorATI()
{
    delete sensorData_;
}

Float PrForceSensorATI::forceMax()
{
    return (Float)sensorData_->max_force;
}

Float PrForceSensorATI::torqueMax()
{
    return (Float)sensorData_->max_torque;
}

//------------------------------------------------------------------
// (C) Copyright 1997,  by ATI Industrial Automation
//
// function:  send_command
//
// Uses global registers:
//      PR_ATI_ADDR_REG	PC I/O address of F/T card's address register
//	PR_ATI_DATA_REG	PC I/O address of F/T card's data register
//
// Calling parameters:
//	unsigned short *bptr = pointer to command structure
//	int numwords	     = length of command
//
// Return Value: 	PR_DRIVER_OK 	if successful
//			PR_DRIVER_ERROR 	if abort
//
//------------------------------------------------------------------
int PrForceSensorATI::SendCommand(unsigned short *bptr, int numwords)
{
    //---------------------------------------
    // write command block to DPRAM at offset 0x0000
    //---------------------------------------
    unsigned int command = bptr[0]; // save command word
    // set address register to command_block
    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_COMMAND_BLOCK);
    for(int i=0;i<numwords;i++)  // write command block to dual port RAM
      {
	  hwIO().output2B(PR_ATI_DATA_REG,bptr[i]) ;
      }
    
    //---------------------------------------
    // read MailBox at DPRAM 0x3FE to clear host acknowledgment flag
    //---------------------------------------
    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_ACK_ADDR);
    hwIO().input2B(PR_ATI_DATA_REG); // reset acknowledgment
    //    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_ACK_ADDR);
    //    hwIO().output2B(PR_ATI_DATA_REG,command); // set acknowledgment
    
    //---------------------------------------
    // write command word to Mailbox at DPRAM 0x3FF to set
    // DSP interrupt flag
    //---------------------------------------
    //    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_INTERRUPT_ADDR);
    //    hwIO().input2B(PR_ATI_DATA_REG);          // clear interrupt flag
    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_INTERRUPT_ADDR);
    hwIO().output2B(PR_ATI_DATA_REG,command); // set interrupt flag
    
    //---------------------------------------
    //  waits for command completion (acknowledgment flag) from DSP
    //---------------------------------------
    unsigned int retcode;
    while(1)
      {
	  unsigned int dword = hwIO().input2B(PR_ATI_STATUS_REG);
	  
	  // wait for acknowledgment flag
	  if((dword & PR_ATI_ACK_BIT)!=PR_ATI_CLEAR) // acknowledgment set
	    {
		// read mailbox location at 0x3FE to reset acknowledgment flag
		hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_ACK_ADDR);
		// reset acknowledgment flag
		retcode = hwIO().input2B(PR_ATI_DATA_REG);
		break;
	    }
      }
    if (retcode == PR_ATI_CLEAR)
      return PR_DRIVER_ERROR;
    else
      return PR_DRIVER_OK;
}

//------------------------------------------------------------------
// (C) Copyright 1997,  by ATI Industrial Automation
// function:  ReceiveCommand
// reads numwords words from ft card starting at fifo offset 0
//
// global registers:
//    PR_ATI_ADDR_REG	PC I/O address of F/T card's address register
//    PR_ATI_DATA_REG	PC I/O address of F/T card's data register
//
//
//  Calling parameters:
//	unsigned short *bptr = pointer to command structure
//	int numwords	     = length of command
//
//------------------------------------------------------------------
void PrForceSensorATI::ReceiveCommand(unsigned short *bptr, int numword)
{
    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_COMMAND_BLOCK); // reset address
    
    for(int i=0;i<numword;i++)
      {
	  bptr[i] = (unsigned short)hwIO().input2B(PR_ATI_DATA_REG);
      }
}


//------------------------------------------------------------------
// (C) Copyright 1997,  by ATI Industrial Automation
//
// function: ReadSensorInfo()
//	Sends sensor_command to F/T card
//	Returns sensor_data
//
//  global registers:
//    PR_ATI_ADDR_REG	PC I/O address of F/T card's address register
//    PR_ATI_DATA_REG	PC I/O address of F/T card's data register
//  Return Value: 	PR_DRIVER_OK 	if successful
//			PR_DRIVER_ERROR 	if abort
//
//------------------------------------------------------------------
//-----------------------------------
// read sensor info to get
// counts_per_force, counts_per_torque, units
//----------------------------------
int PrForceSensorATI::ReadSensorInfo(void)
{
    sensorData_->cmd_rc = PR_ATI_READ_COMMAND; // sensor data command word
    sensorData_->mode   = PR_ATI_READ_MODE;  // mode=read
    if(SendCommand((unsigned short *)sensorData_,2) != PR_DRIVER_OK) 
      return(PR_DRIVER_ERROR);
    ReceiveCommand((unsigned short *)sensorData_,
		   sizeof(PrForceSensorATIData)/sizeof(unsigned short));
    return (PR_DRIVER_OK);
}


//------------------------------------------------------------------
// (C) Copyright 1997,  by ATI Industrial Automation
//
// value()
//
// global registers:
//    PR_ATI_ADDR_REG	PC I/O address of F/T card's address register
//    PR_ATI_DATA_REG	PC I/O address of F/T card's data register
//    PR_INTERRUPT_MODE	0=use polling node
//			1=use interrupt mode
//
// polls PR_ATI_DATA_REG[0] for new data
//------------------------------------------------------------------
int PrForceSensorATI::values(Float *data)
{
    unsigned short sem=PR_ATI_CLEAR;
    while(sem == PR_ATI_CLEAR)
      {
	  //-----------------------------------
	  // polling mode, test for new data
	  //-----------------------------------
	  // write ram starting addr
	  hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_DATA_BLOCK); 
	  // read first word[0]
	  sem = (unsigned short)hwIO().input2B(PR_ATI_DATA_REG);
      }

    //-----------------------------------------
    // new data available.
    // first check monitoring conditions
    //-----------------------------------------
    // read 2nd word=monitor cond
    unsigned short mon = (unsigned short)hwIO().input2B(PR_ATI_DATA_REG); 
    if (mon != PR_ATI_CLEAR)
      {
	  //-----------------------------------
	  // mon_cond_status
	  // 1 = Alarm state
	  // 2 = Out of Alarm State
	  // 3 = Expired
	  //------------------------------------
	  // monitor condition
	  hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_MONITOR_ADDR); 
	  // clear monitor condition
	  hwIO().output2B(PR_ATI_DATA_REG,PR_ATI_CLEAR);  
      }
    
    //-----------------------------------------
    // get the new F/T data in counts
    // to get actual forces and torque, the application
    // should divide the counts by the
    // counts_per_force and counts_per_torque
    // conversion factors
    //-----------------------------------------
    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_FORCE_ADDR); 
    short forceCounts[6];
    int i;
    for(i=0;i<6;i++) 
      {
	  forceCounts[i] = (short)hwIO().input2B(PR_ATI_DATA_REG);
      }
    for(i=0;i<3;i++) 
      {
	  data[i] = forceCounts[i]*forcePerCount_ - bias()[i];
	  data[i+3] = forceCounts[i+3]*torquePerCount_ - bias()[i+3];
      }

    // acknowledge data
    // write ram starting addr
    hwIO().output2B(PR_ATI_ADDR_REG,PR_ATI_DATA_BLOCK); 
    // acknowledge data -> clear the semaphore
    hwIO().output2B(PR_ATI_DATA_REG,PR_ATI_CLEAR);		
    return PR_DRIVER_OK;
}

void PrForceSensorATI::display()
{
    int	i,j;
    
    printf("cmd_rc: %4x\n",sensorData_->cmd_rc);	// 0x0100
    printf("mode: %d\n",sensorData_->mode);

	printf("calibration matrix:\n");
   for(i=0; i<6; i++) 
      {
	  // 6 x 8 32-bit words
	  for(j=0; j<8; j++)
	    printf("%d ",sensorData_->matrix[i][j]); 
	  printf("\n");
      }
	printf("bias: ");
    for(i=0; i<8; i++) 
      printf("%d ",sensorData_->sbias[i]); // hardware bias array
    printf("\nMm: ");
    for(i=0; i<8; i++) 
      printf("%x ",sensorData_->Mm[i]); // hardware bias array
    printf("\nBm: ");
    for(i=0; i<8; i++) 
      printf("%x ",sensorData_->Bm[i]); // hardware bias array
    printf("\nMb: ");
    for(i=0; i<8; i++) 
      printf("%x ",sensorData_->Mb[i]); // hardware bias array
    printf("\nBb: ");
    for(i=0; i<8; i++) 
      printf("%x ",sensorData_->Bb[i]); // hardware bias array
    printf("\n");
    printf("number of gages   = %d\n",sensorData_->number_of_gages);
    printf("counts per force  = %d\n",sensorData_->counts_per_force);
    printf("counts per torque = %d\n",sensorData_->counts_per_torque);
    printf("max force         = %d\n",sensorData_->max_force);
    printf("max torque        = %d\n",sensorData_->max_torque);
    printf("max temperature   = %d\n",sensorData_->maxtemperature);
    printf("min temperature   = %d\n",sensorData_->min_temperature);
    printf("units             = %d\n",sensorData_->units);
    printf("serial number     = %d\n",sensorData_->serial_number);

}


