component wj200_vfd "Hitachi wj200 modbus driver";
param rw unsigned mbslaveaddr     "Modbus slave address";
pin in float commanded_frequency  "Frequency of vfd";
pin in bit reverse                "1 when reverse 0 when forward";
pin in bit run                    "run the vfd";
pin in bit enable                 "1 to enable the vfd. 0 will remote trip the vfd, thereby disabling it.";
pin out bit is_running            "1 when running";
pin out bit is_at_speed           "1 when running at assigned frequency";
pin out bit is_ready              "1 when vfd is ready to run";
pin out bit is_alarm              "1 when vfd alarm is set";
pin out bit watchdog_out          "Alternates between 1 and 0 after every update cycle. Feed into a watchdog component to ensure vfd driver is communicating with the vfd properly.";
option userspace;
option userinit yes;
license "GPLv2 or greater";
;;
/*
  Userspace HAL component to control a Hitatchi WJ200 series VFD

  Written by Curtis Dutton, inspired by vfs11_vfd.c in linuxcnc

  Copyright (C) 2012 Curtis Dutton, OK Computers LLC

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation, version 2.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA.

  see 'man wj200_vfd' and the WJ200 section in the Drivers manual.
*/
#include<stdio.h>
#include<errno.h>
#include<getopt.h>
#include<stdbool.h>
#include<math.h>
#include<modbus.h>
#include<unistd.h>
#include<ctype.h>

typedef struct
{
        uint8_t running;
        uint8_t ready;
        uint8_t direction;
        uint8_t at_speed;
        uint8_t alarm;
        uint16_t frequency;
} wj200_status;

/*sets the operating frequency of the vfd*/
bool wj200_setFrequency(modbus_t* ctx, uint16_t frequency)
{
        return modbus_write_registers(ctx, 0x001, 1, &frequency) > 0;
}

/*resets the trip status of the VFD*/
bool wj200_reset(modbus_t* ctx)
{
        /*after the reset, the wj200 vfd seem to need a second
        before it will reply to more modbus commands*/
        int rc = modbus_write_bit(ctx, 0x003, TRUE);

        sleep(1);

        return rc > 0;
}

bool wj200_setDirection(modbus_t* ctx, bool direction)
{
	return modbus_write_bit(ctx, 0x001, direction) > 0;
}

bool wj200_trip(modbus_t* ctx)
{
        return modbus_write_bit(ctx, 0x002, TRUE) > 0;
}

bool wj200_run(modbus_t* ctx, bool runBit)
{
        return modbus_write_bit(ctx, 0x000, runBit) > 0;
}


bool wj200_getStatus(modbus_t* ctx, wj200_status* status)
{
        int rc;
        uint8_t bits[10];
        uint16_t registers[2];

        /*read coils 0x000F thru 0x0019 in one step*/
        rc = modbus_read_bits(ctx, 0x000F-1, 11, bits);

        if(rc < 0)
        {
                return false;
        }

        /*read the first 2 registers*/
        rc = modbus_read_registers(ctx, 0x000, 2, registers);

        if(rc < 0)
        {
                return false;
        }

        status->running = bits[0];
        status->direction = bits[1];
        status->ready = bits[2];
        status->alarm = bits[9];
        status->at_speed = bits[5];
        status->frequency = registers[1];

        return true;
}

void print_modbus_error(struct __comp_state *__comp_inst, const char* msg)
{
	fprintf(stderr, 
		"Error: wj200_vfd slave(%d): %s - Modbus error (%d) -  %s\n",
		mbslaveaddr,
		msg,
		errno,
		modbus_strerror(errno));
}


/* modbus connection settings*/
char *device = "/dev/ttyS0";
int baud = 9600;
char parity = 'N';
int data_bits = 8;
int stop_bits = 1;
modbus_t *ctx;

void userinit(int argc, char **argv)
{
	int opt_index = 0;
	int c = 0;

	static struct option options[] = {
		{"baud",   required_argument, 0, 0 },
		{"parity", required_argument, 0, 0 },
		{"databits", required_argument, 0, 0 },
		{"stopbits", required_argument, 0, 0 },
		{0, 0, 0, 0}
	};	

	while(1) {
		c = getopt_long(argc, argv, "", options, &opt_index);

		if(c == -1)
			break;

		switch(opt_index) {
			case 0:
				baud = atoi(optarg);

				if(baud == 0)
				{
					fprintf(stderr, 
						"Invalid argument: buad must be a number. Given '%s'\n", 
						optarg);
					exit(1);
				}
				break;

			case 1:
				parity = toupper(optarg[0]);

				if(parity != 'Y' && parity != 'N')
				{
					fprintf(stderr, 
						"Invalid argument: parity must be 'y' or 'n'. Given '%s'\n", 
						optarg);
					exit(1);
				}
				break;

			case 2:
				data_bits = atoi(optarg);

				if(data_bits == 0)
				{
					fprintf(stderr, 
						"Invalid argument: databits must be a number. Given '%s'\n", 
						optarg);
					exit(1);
				}
				break;

			case 3:
				stop_bits = atoi(optarg);
		
				if(stop_bits == 0)
				{
					fprintf(stderr, 
						"Invalid argument: stopbits must be a number. Given '%s'\n", 
						optarg);
					exit(1);
				}
				break;

			default:
				exit(1);
		}

	}
	
	ctx = modbus_new_rtu(device, baud, parity, data_bits, stop_bits);

	if (ctx == NULL) {
		fprintf(stderr, 
			"ERROR: wj200_vfd unable to create libmodbus context. - %s\n", 
			modbus_strerror(errno));
		fprintf(stderr, "Check your commandline!\n");
		exit(1);	
	}
	
	if (modbus_connect(ctx)) {
		fprintf(stderr, 
			"ERROR: wj200_vfd unable to create libmodbus connection. - %s\n", 
			modbus_strerror(errno));
		exit(1);
	}
}

void user_mainloop(void) {
	wj200_status status;
	uint16_t calculated_frequency;

	while(1) {
	FOR_ALL_INSTS() {
		/*
                  until the params are set we just wait a bit
		  and then skip to the next instance.

		  if every instance does not get a slave address,
                  this could cause bad behavior
		*/
		if(mbslaveaddr == 0) {
			sleep(1);
			continue; 
		}

		modbus_set_slave(ctx, mbslaveaddr);

		/*
                  for each slave, receive info from the slave,
		  update our output pins based upon vfd status,
		  then set the vfd according to our input pins

		  if we hit an error we just re-loop. The watchdog
		  pin won't change until we make it all the way through
		  the loop.
                */
		if(!wj200_getStatus(ctx, &status)) {	
			print_modbus_error(__comp_inst, "failed to get status");
			continue;
		}

		is_running = status.running;
		is_at_speed = status.at_speed;
		is_ready = status.ready;
		is_alarm = status.alarm;

		if(!status.alarm && !enable && !wj200_trip(ctx)) {
			print_modbus_error(__comp_inst, "failed to trip");
			continue;
		}
		else if(status.alarm && enable && !wj200_reset(ctx)) {
			print_modbus_error(__comp_inst, "failed to reset");
			continue;
		}
		else {
			calculated_frequency = (uint16_t)(fabs(commanded_frequency) * 100);

			if(calculated_frequency != status.frequency 
			   && !wj200_setFrequency(ctx, calculated_frequency)) {
				print_modbus_error(__comp_inst, "failed to set frequency");
				continue;
			}

			if(reverse != status.direction && !wj200_setDirection(ctx, reverse)) {
				print_modbus_error(__comp_inst, "failed to set direction");
				continue;
			}

			if(status.running ^ run && !wj200_run(ctx, run)) {
				print_modbus_error(__comp_inst, "failed to run");
				continue;
			}

			watchdog_out = !watchdog_out;
		}
	}
	}
}