/********************************************************************\

  Name:         scs_210_fischerspindle.c
  Created by:   Stefan Ritt


  Contents:     Application specific (user) part of
                Midas Slow Control Bus protocol 
                for SCS-210 RS232 node connected to 
                a Sieb&Meyer SD2S power converter
                for the Mu3e 16g/s Fischer Spindle 
                helium compressor

\********************************************************************/

#include <intrins.h>
#include <stdio.h>
#include <stdlib.h>             // for atof()
#include <string.h>
#include <math.h>
#include "mscbemb.h"

extern unsigned char xdata * xdata rbuf_rp;
extern unsigned char xdata * xdata rbuf_wp;
extern char xdata rbuf[2048];

char code node_name[] = "SPDLFRxx";

/* declare number of sub-addresses to framework */
unsigned char idata _n_sub_addr = 1;

sbit CTS = P0 ^ 7;
sbit RTS = P0 ^ 6;

unsigned char xdata update_flag[2];

/*---- Define channels and configuration parameters returned to
       the CMD_GET_INFO command                                 ----*/

/* data buffer (mirrored in EEPROM) */

struct {
   unsigned char  on;
   unsigned long  rpm_demand;
   unsigned long  rpm_measured;   
   unsigned char  ack_error;
   unsigned long  status;
   
   float temp_converter;
   float temp_motor;
   float current_motor;

   unsigned char  d[18];
   unsigned char  demo;
} xdata user_data;

MSCB_INFO_VAR code vars[] = {
   1,  UNIT_BOOLEAN, 0, 0,  0,            "On",        &user_data.on,
   4,  UNIT_RPM,     0, 0,  0,            "RPMDmd",    &user_data.rpm_demand,
   4,  UNIT_RPM,     0, 0,  0,            "RPMMeas",   &user_data.rpm_measured,
   1,  UNIT_BOOLEAN, 0, 0,  0,            "AckErr",    &user_data.ack_error,
   4,  UNIT_DWORD,   0, 0,  0,            "Status",    &user_data.status,

   4,  UNIT_CELSIUS, 0, 0,  MSCBF_FLOAT,  "TConv",     &user_data.temp_converter,
   4,  UNIT_CELSIUS, 0, 0,  MSCBF_FLOAT,  "TMotor",    &user_data.temp_motor,
   4,  UNIT_AMPERE,  0, 0,  MSCBF_FLOAT,  "IMotor",    &user_data.current_motor,

   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d0",        &user_data.d[0],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d1",        &user_data.d[1],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d2",        &user_data.d[2],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d3",        &user_data.d[3],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d4",        &user_data.d[4],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d5",        &user_data.d[5],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d6",        &user_data.d[6],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d7",        &user_data.d[7],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d8",        &user_data.d[8],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d9",        &user_data.d[9],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d10",       &user_data.d[10],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d11",       &user_data.d[11],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d12",       &user_data.d[12],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d13",       &user_data.d[13],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d14",       &user_data.d[14],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d15",       &user_data.d[15],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d16",       &user_data.d[16],
   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "d17",       &user_data.d[17],

   1,  UNIT_BYTE,    0, 0,  MSCBF_HIDDEN, "demo",      &user_data.demo,
   0
};

MSCB_INFO_VAR *variables = vars;

/********************************************************************\

  Application specific init and inout/output routines

\********************************************************************/

void user_write(unsigned char channel) reentrant;
void write_gain(void);
void putb(unsigned char *buf, unsigned char len);
int getb(unsigned char *buf);
void clear_rbuf(void);

/*---- User init function ------------------------------------------*/

void user_init(unsigned char init)
{
   unsigned char i;
   
   if (init) {
      user_data.rpm_demand = 0;
      user_data.rpm_measured = 0;
      user_data.on = 0;
      user_data.demo = 0;
   }

   user_data.status = 0;

   for (i=0 ; i<16 ; i++)   
      user_data.d[i]  = 0;
   
   /* initialize UART1 */
   uart_init(1, BD_57600);

   SFRPAGE = CONFIG_PAGE;
   P0MDOUT = 0x81;      // P0.0: TX = Push Pull, P0.7 = Push Pull

   /* indicate we can receive data */
   CTS = 0;

   update_flag[0] = 0;
   update_flag[1] = 0;
   
   /* turn off LED r/w blinking, since we handle that ourselves */
   led_rw_blink_set(0);
}

/*---- User write function -----------------------------------------*/

void user_write(unsigned char index) reentrant
{
   if (index < 2)
      update_flag[index] = 1;
}

/*---- User read function ------------------------------------------*/

unsigned char user_read(unsigned char channel)
{
   if (channel); // avoid compiler warning
   return 0;
}

/*---- User function called vid CMD_USER command -------------------*/

unsigned char user_func(unsigned char *data_in, unsigned char *data_out)
{
   /* echo input data */
   data_out[0] = data_in[0];
   data_out[1] = data_in[1];
   return 2;
}

/*---- Serial communication functions ------------------------------*/

// Sends a buffer of length len to RS-232 as binary data
void putb(unsigned char *buf, unsigned char len)
{
   unsigned int i;
   
   // put bytes in buffer
   for (i=0 ; i<len ; i++)
      putchar(buf[i]);
   
   // start sending
   flush();
}

// Reads a char from the serial interface. Return 0x00 on timeout
char getbyte_wait(unsigned char timeout)
{
   char c;
   unsigned long xdata start;

   start = time();
   do {
      if (rbuf_wp != rbuf_rp) {
         c = *rbuf_rp++;
         if (rbuf_rp == rbuf + sizeof(rbuf))
            rbuf_rp = rbuf;
         return c;
      }
   } while (time() < start + timeout);

   return 0x00;
}

void clear_rbuf()
{
   rbuf_rp = rbuf_wp = rbuf;
}

/*------------------------------------------------------------------*/

char xdata tx_buf[20];
char xdata rx_buf[20];

void sd2s_write8b(unsigned short control, unsigned long speed)
{
   unsigned char i, n;
   unsigned char check = 0xFF;
   unsigned short spd;
   
   for (i=0 ; i<16 ; i++)
     user_data.d[i] = 0;   

   tx_buf[0] = 0x00;      // zero
   tx_buf[1] = 0x0D;      // length
   tx_buf[2] = 0x02;      // module number plus 2
   tx_buf[3] = 0x01;      // PC
   tx_buf[4] = 0x10;      // command

   tx_buf[5] = 0x01;      // Header 0
   tx_buf[6] = 0x00;      // Header 1

   tx_buf[7] = control & 0xFF;
   tx_buf[8] = (control >> 8) & 0xFF;
   
   spd = (unsigned short) ((float) speed / 135000.0 * 0x3FFF);
   tx_buf[9]  = spd & 0xFF;
   tx_buf[10] = (spd >> 8) & 0xFF;
   
   for (i=11 ; i<15 ; i++) // Data 0-7
      tx_buf[i] = 0;
   
   for (i=0 ; i<15 ; i++)
      check -= tx_buf[i];

   tx_buf[15] = check;

   putb(tx_buf, 16);
   
   n = gets_wait(rx_buf, 16, 100);
   if (n == 16)
      user_data.ack_error = 0;
   else
      user_data.ack_error = 1;

   for (i=0 ; i<16 ; i++)
     user_data.d[i] = rx_buf[i];   
}

/*------------------------------------------------------------------*/

void sd2s_write(unsigned short index, unsigned char *d, unsigned char len)
{
   unsigned char i, n;
   unsigned char check;
   
   for (i=0 ; i<18 ; i++)
     user_data.d[i] = 0;   

   tx_buf[0] = 0;         // zero
   tx_buf[1] = 11;        // length
   tx_buf[2] = 2;         // module number plus 2
   tx_buf[3] = 1;         // PC
   tx_buf[4] = 0x0e;      // command

   tx_buf[5] = index & 0xFF;
   tx_buf[6] = (index >> 8) & 0xFF;

   tx_buf[7] = 0;         // subindex
   tx_buf[8] = 0;
   tx_buf[9] = 0;
   tx_buf[10] = 0;

   tx_buf[11] = len;

   for (i=0 ; i<len ; i++)
      tx_buf[11+len-i] = d[i]; // endian conversion
   
   check = 0xFF;
   for (i=0 ; i<12+len ; i++)
      check -= tx_buf[i];

   tx_buf[12+len] = check;

   putb(tx_buf, 13+len);
   
   n = gets_wait(rx_buf, 7, 100);
   if (n == 7)
      user_data.ack_error = 0;
   else
      user_data.ack_error = 1;

   for (i=0 ; i<13+len ; i++)
     user_data.d[i] = tx_buf[i];   
}

/*------------------------------------------------------------------*/

unsigned char sd2s_read(unsigned short index, unsigned char *d, unsigned char len)
{
   unsigned char check = 0xFF;
   unsigned char i, n, rlen;
   
   led_blink(1, 1, 50);

   tx_buf[0] = 0;         // zero
   tx_buf[1] = 9;         // length
   tx_buf[2] = 2;         // module number plus 2
   tx_buf[3] = 1;         // PC
   tx_buf[4] = 0x0d;      // command

   tx_buf[5] = index & 0xFF;
   tx_buf[6] = (index >> 8) & 0xFF;

   tx_buf[7] = 0;         // subindex
   tx_buf[8] = 0;
   tx_buf[9] = 0;
   tx_buf[10] = 0;

   for (i=0 ; i<11 ; i++)
      check -= tx_buf[i];

   tx_buf[11] = check;

   putb(tx_buf, 12);
   
   if (len == 2)
      rlen = 10;
   else
      rlen = 12;
   
   n = gets_wait(rx_buf, rlen, 100);
   if (n == 0) {
      user_data.ack_error = 1;
      return 0;
   } else if (n < rlen) {
      user_data.ack_error = 2;
      return 0;
   } else
      user_data.ack_error = 0;
   
   check = 0xFF;
   for (i=0 ; i<rlen-1 ; i++)
      check -= rx_buf[i];
   
   if (check != rx_buf[rlen-1])
      user_data.ack_error = 3;
   
   if (rx_buf[6] != 0)
      user_data.ack_error = rx_buf[6];
   
   for (i=0 ; i<len ; i++)
      d[i] = rx_buf[7+i];
   
   /*
   for (i=0 ; i<rlen && i<18 ; i++)
      user_data.d[i] = rx_buf[i];
   */
   
   led_blink(0, 1, 50);
   
   return len;
}

/*------------------------------------------------------------------*/

void sd2s_set_rpm(unsigned long rpm)
{
   // shutdown command to enable drive
   unsigned long d32 = rpm * 1000; // value is in 0.001/min
   sd2s_write(395, (unsigned char *) &d32, 4);
}

/*------------------------------------------------------------------*/

void sd2s_start(unsigned char flag)
{
   unsigned short d16;
   
   if (flag) {

      // shutdown command to enable drive
      unsigned int d16 = 6;
      sd2s_write(68, (unsigned char *) &d16, 2);
      
      // switch on controller
      d16 = 7;
      sd2s_write(68, (unsigned char *) &d16, 2);

      // set RPM
      sd2s_set_rpm(user_data.rpm_demand);

      // start motor
      d16 = 15;
      sd2s_write(68, (unsigned char *) &d16, 2);

   } else {

      // stop motor
      d16 = 7;
      sd2s_write(68, (unsigned char *) &d16, 2);
      
      // switch off controller
      d16 = 6;
      sd2s_write(68, (unsigned char *) &d16, 2);

   }
}

/*------------------------------------------------------------------*/

void sd2s_get_status()
{
   unsigned char d[2], i;
   unsigned short d16;

   // DEVICE_CTRL_ERROR_CODE_ACTUAL
   i = sd2s_read(67, d, 2);
      
   if (i == 2) {
      d16 = d[1];
      d16 = (d16 << 8) + d[0];
      user_data.status = d16;
   }
}

/*------------------------------------------------------------------*/

void sd2s_get_rpm(unsigned long *rpm)
{
   unsigned char d[4], i;
   unsigned long d32;

   // read RPM
   i = sd2s_read(398, d, 4);
      
   if (i == 4) {
      d32 = d[3];
      d32 = (d32 << 8) + d[2];
      d32 = (d32 << 8) + d[1];
      d32 = (d32 << 8) + d[0];
      
      DISABLE_INTERRUPTS;
      *rpm = d32 / 1000;
      ENABLE_INTERRUPTS;
   }
}

/*------------------------------------------------------------------*/

void sd2s_get_temp(unsigned short index, float *temp)
{
   unsigned char d[2], i;
   unsigned short d16;

   // read data
   i = sd2s_read(index, d, 2);
      
   if (i == 2) {
      d16 = d[1];
      d16 = (d16 << 8) + d[0];
      DISABLE_INTERRUPTS;
      *temp = (float) d16 / 10;
      ENABLE_INTERRUPTS;
   }
}

/*------------------------------------------------------------------*/

void sd2s_get_current(float *current)
{
   unsigned char d[2], i;
   unsigned short d16;
   float cur;

   // read data
   i = sd2s_read(431, d, 2);
      
   if (i == 2) {
      d16 = d[1];
      d16 = (d16 << 8) + d[0];
      cur = (float) d16 * 0.01 / 1.4142; // peak value [0.01A] / sqrt(2);
      cur = floor(cur * 100 + 0.5) / 100;
      DISABLE_INTERRUPTS;
      *current = cur;
      ENABLE_INTERRUPTS;
   }
}

/*---- User loop function ------------------------------------------*/
   
unsigned long xdata last_read = 0;

void user_loop(void)
{
   unsigned int d16;

   // turn compressor on/off
   if (update_flag[0]) {
      update_flag[0] = 0;
      
      if (user_data.on == 0 || user_data.on == 1) {
         if (!user_data.demo) {
            led_blink(1, 2, 50);
            sd2s_set_rpm(user_data.rpm_demand);
            sd2s_start(user_data.on);
         }
      }
      
      if (user_data.on > 1) {
         // used for debugging:
         // 6:  shutdown command to enable drive
         // 7:  switch on controller
         // 15: start motor
         led_blink(1, 2, 50);
         d16 = user_data.on;
         sd2s_write(68, (unsigned char *) &d16, 2);
      }    
   }
   
   // set demand RPM
   if (update_flag[1]) {
      update_flag[1] = 0;
      if (user_data.demo)
         user_data.rpm_measured = user_data.rpm_demand + 1;
      else {
         led_blink(1, 2, 50);
         sd2s_set_rpm(user_data.rpm_demand);
      }
   }
   
   // read parameters once each second
   if (time() > last_read + 100) {
      
      if (!user_data.demo)
         sd2s_get_rpm(&user_data.rpm_measured);
         
      sd2s_get_temp(41, &user_data.temp_converter);
      sd2s_get_temp(63, &user_data.temp_motor);
      sd2s_get_current(&user_data.current_motor);
      sd2s_get_status();
      
      last_read = time();
   }

}