dt_el_safety.c

/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	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 General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <controller/controller_block.h>
#include <dt_elevation/dt_el_safety.h>
#include <log/log.h>

/*
       inputs          outputs
       nr name         nr name

     ----------------------------------
     |                                |
  ---| 0  speed_in_r    0 speed_out_r |----
     |	                              |
  ---| 1  speed_in_l    1 speed_out_l |----
     |	                              |
  ---| 2  position_in_r 2  safe_out   |----
     |	                              |
  ---| 3  position_in_l               |
     |	                              |
  ---| 4  torsion_in                  |
     |	                              |
  ---| 5  torque_in                   |
     |	                              |
  ---| 6  torque_in_r  3 torque_out_r |----
     |	                              |
  ---| 7  torque_in_l  4 torque_out_l |----
     |	                              |
  ---| 8  safety_in_top               |
     |                                |
  ---| 9  safety_in_bottom            |
     |                                |
  ---| 10 enable                      |
     |                                |
     ----------------------------------

	This is not a generic block.
	It implements the safety checks specific for the dt elevation
	control loop.

	Checks:	
		+ Enabled
		+ Direction limit if position beyond max/min
		+ Speed limit if within safe-zone.

       min           safe_zone_min                safe_zone_max           max
        |------------------|----------------------------|------------------|
        safe_zone_min_speed       normal operation      safe_zone_max_speed

 */

struct controller_block_private {
	float *input_r;
	float *input_l;
	float *position_in_r;
	float *position_in_l;
	float *torsion_in;
	float *torque_in;
	float *torque_in_r;
	float *torque_in_l;
	bool *safety_in_top;
	bool *safety_in_bottom;
	float speed_out_r;
	float speed_out_l;
	bool safe_out;
	float torque_out_r;
	float torque_out_l;
	bool *enable_in;

	float position_min;
	float position_max;
	
	float safe_zone_min;
	float safe_zone_max;
	float safe_zone_min_speed;
	float safe_zone_max_speed;
	
	float torsion_max;
	float torsion_recover_max;
	float torque_max;
	float emergency_torque;

	int recover;

	/* flags to prevent log flooding */
	bool err_max_r;
	bool err_min_r;
	bool err_max_l;
	bool err_min_l;
	bool err_torque_max;
	bool err_torsion;
	bool err_torsion_recover;
	bool warn_safe_max_l;
	bool warn_safe_min_l;
	bool warn_safe_max_r;
	bool warn_safe_min_r;
	bool err_hwsafe_top;
	bool err_hwsafe_bottom;
};

static void dt_el_safety_calculate(struct controller_block *safety)
{
	float out0, out1;
	float position_in_r, position_in_l;
	float torsion_in, torque_in, torque_in_r, torque_in_l;
	bool safe;
	bool enabled = *safety->private->enable_in;

	safe = true;
	out0 = *safety->private->input_r;
	out1 = -*safety->private->input_l;
	position_in_r = *safety->private->position_in_r;
	position_in_l = -*safety->private->position_in_l;
	torsion_in = *safety->private->torsion_in;
	torque_in = *safety->private->torque_in;
	torque_in_r = *safety->private->torque_in_r;
	torque_in_l = *safety->private->torque_in_l;

	/* outside maximum range and not moving back */
	if (position_in_r >= safety->private->position_max &&
	    out0 > 0.0) {
		out0 = 0.0;
		safe = false;
		if (!safety->private->err_max_r) {
			safety->private->err_max_r = true;
			log_send(LOG_T_ERROR,
			    "Elevation position right %e above maximum %e",
			    position_in_r,
			    safety->private->position_max);
		}
	} else {
		safety->private->err_max_r = false;
	}

	/* inside safe zone and not moving at a safe speed */
	if (position_in_r >= safety->private->safe_zone_max &&
	    out0 > safety->private->safe_zone_max_speed) {
		out0 = safety->private->safe_zone_max_speed;
		if (!safety->private->warn_safe_max_r) {
			safety->private->warn_safe_max_r = true;
			log_send(LOG_T_WARNING,
			    "Elevation position right %e in safe zone %e",
			    position_in_r,
			    safety->private->safe_zone_max);
		}
	} else {
		safety->private->warn_safe_max_r = false;
	}

	/* outside minimum and not moving back */
	if (position_in_r <= safety->private->position_min &&
	    out0 < 0.0) {
		out0 = 0.0;
		safe = false;
		if (!safety->private->err_min_r) {
			safety->private->err_min_r = true;
			log_send(LOG_T_ERROR, 
			"Elevation position right under minimum, %e < %e",
			position_in_r, safety->private->position_min);
		}
	} else {
		safety->private->err_min_r = false;
	}
	
	/* inside safe zone and not moving back */
	if (position_in_r <= safety->private->safe_zone_min &&
	    out0 < safety->private->safe_zone_min_speed) {
		out0 = safety->private->safe_zone_min_speed;
		if (!safety->private->warn_safe_min_r) {
			safety->private->warn_safe_min_r = true;
			log_send(LOG_T_WARNING, 
			    "Elevation position right in safe zone");
		}
	} else {
		safety->private->warn_safe_min_r = false;
	}


	/* outside maximum range and not moving back */
	if (position_in_l >= safety->private->position_max &&
	    out1 > 0.0) {
		out1 = 0.0;
		safe = false;
		if (!safety->private->err_max_l) {
			safety->private->err_max_l = true;
			log_send(LOG_T_ERROR, 
			    "Elevation left position %e above maximum %e",
			    position_in_l,
			    safety->private->position_max);
		}
	} else {
		safety->private->err_max_l = false;
	}

	/* inside safe zone and not moving at a safe speed */
	if (position_in_l >= safety->private->safe_zone_max &&
	    out1 > safety->private->safe_zone_max_speed) {
		out1 = safety->private->safe_zone_max_speed;
		if (!safety->private->warn_safe_max_l) {
			safety->private->warn_safe_max_l = true;
			log_send(LOG_T_WARNING,
			    "Elevation left position %e in safe zone %e",
			    position_in_l,
			    safety->private->safe_zone_max);
		}
	} else {
		safety->private->warn_safe_max_l = false;
	}

	/* outside minimum and not moving back */
	if (position_in_l <= safety->private->position_min &&
	    out1 < 0.0) {
		out1 = 0.0;
		safe = false;
		if (!safety->private->err_min_l) {
			safety->private->err_min_l = true;
			log_send(LOG_T_ERROR, 
			    "Elevation left position %e under minimum %e",
			    position_in_l, safety->private->position_min);
		}
	} else {
		safety->private->err_min_l = false;
	}

	/* inside safe zone and not moving back */
	if (position_in_l <= safety->private->safe_zone_min &&
	    out1 < safety->private->safe_zone_min_speed) {
		out1 = safety->private->safe_zone_min_speed;
		if (!safety->private->warn_safe_min_l) {
			safety->private->warn_safe_min_l = true;
			log_send(LOG_T_WARNING, 
			    "Elevation left position in safe zone");
		}
	} else {
		safety->private->warn_safe_min_l = false;
	}

	if (fabsf(torsion_in) >= safety->private->torsion_recover_max) {
		if (!safety->private->recover) {
			out0 = 0.0;
			out1 = 0.0;
		}
		safe = false;
		if (!safety->private->err_torsion_recover) {
			safety->private->err_torsion_recover = true;
			log_send(LOG_T_ERROR,
			    "Elevation torsion to big (%e) to recover, check calibration",
			    torsion_in);
		}
	} else {
		safety->private->err_torsion_recover = false;
	}

	/* torsion to big */
	if (fabsf(torsion_in) >= safety->private->torsion_max) {
		/* moving back? */
		if (!safety->private->recover && position_in_r > position_in_l) {
			if (out0 > 0.0) {
				out0 = 0.0;
				safe = false;
			}
			if (out1 < 0.0) {
				out1 = 0.0;
				safe = false;
			}
		}
		if (!safety->private->recover && position_in_r < position_in_l) {
			if (out0 < 0.0) {
				out0 = 0.0;
				safe = false;
			}
			if (out1 > 0.0) {
				out1 = 0.0;
				safe = false;
			}
		}
		if (!safety->private->err_torsion) {
			safety->private->err_torsion = true;
			log_send(LOG_T_ERROR, "Elevation torsion (%e) to big",
			    torsion_in);
		}
	} else {
		safety->private->err_torsion = false;
	}

	/* (filtered) total torque to big (elevation stuck?) */
	if (fabs(torque_in) > safety->private->torque_max) {
		if (!safety->private->err_torque_max) {
			safety->private->err_torque_max = true;
			log_send(LOG_T_ERROR, "Elevation torque to big. %e > %e",
			    torque_in, safety->private->torque_max);
		}
		safe = false;
	} else {
		safety->private->err_torque_max = false;
	}

	/* top safety switch */
	if (!*safety->private->safety_in_top) {
		safe = false;
		if (!safety->private->err_hwsafe_top) {
			safety->private->err_hwsafe_top = true;
			log_send(LOG_T_ERROR, 
			    "Elevation HW top safety switch is open.");
		}
	} else {
		if (safety->private->err_hwsafe_top) {
			safety->private->err_hwsafe_top = false;
			log_send(LOG_T_INFO, 
			    "Elevation HW top safety switch is closed.");
		}
	}
	
	/* bottom safety switch */
	if (!*safety->private->safety_in_bottom) {
		safe = false;
		if (!safety->private->err_hwsafe_bottom) {
			safety->private->err_hwsafe_bottom = true;
			log_send(LOG_T_ERROR, 
			    "Elevation HW bottom safety switch is open.");
		}
	} else {
		if (safety->private->err_hwsafe_bottom) {
			safety->private->err_hwsafe_bottom = false;
			log_send(LOG_T_INFO, 
			    "Elevation HW bottom safety switch is closed.");
		}
	}
	
	if (!safe)
		enabled = false;

	if (!enabled && !safety->private->recover) {
		out0 = 0.0;
		out1 = 0.0;
		safety->private->torque_out_r =
		    safety->private->emergency_torque;
		safety->private->torque_out_l =
		    safety->private->emergency_torque;
	} else {
		safety->private->torque_out_r = torque_in_r;
		safety->private->torque_out_l = torque_in_l;
	}

	safety->private->speed_out_r = out0;
	safety->private->speed_out_l = -out1;
	safety->private->safe_out = safe || safety->private->recover;
}


static struct controller_block_param_list params[] = {
	{ "position_min",        false },
	{ "position_max",        false },
	{ "safe_zone_min",       false },
	{ "safe_zone_max",       false },
	{ "safe_zone_min_speed", false },
	{ "safe_zone_max_speed", false },
	{ "torsion_max",         false },
	{ "torsion_recover_max", false },
	{ "recover",             false },
	{ "torque_max",          false },
	{ "emergency_torque",    false },
	{ NULL },
};

static void param_get(struct controller_block *safety, int param, void *val)
{
	switch (param) {
		case 0:
			*(float*)val = safety->private->position_min;
			break;
		case 1:
			*(float*)val = safety->private->position_max;
			break;
		case 2:
			*(float*)val = safety->private->safe_zone_min;
			break;
		case 3:
			*(float*)val = safety->private->safe_zone_max;
			break;
		case 4:
			*(float*)val = safety->private->safe_zone_min_speed;
			break;
		case 5:
			*(float*)val = safety->private->safe_zone_max_speed;
			break;
		case 6:
			*(float*)val = safety->private->torsion_max;
			break;
		case 7:
			*(float*)val = safety->private->torsion_recover_max;
			break;
		case 8:
			*(int*)val = safety->private->recover;
			break;
		case 9:
			*(float*)val = safety->private->torque_max;
			break;
		case 10:
			*(float*)val = safety->private->emergency_torque;
			break;
	}
}

static void param_set(struct controller_block *safety, int param, va_list val)
{
	switch (param) {
		case 0:
			safety->private->position_min = va_arg(val, double);
			break;
		case 1:
			safety->private->position_max = va_arg(val, double);
			break;
		case 2:
			safety->private->safe_zone_min = va_arg(val, double);
			break;
		case 3:
			safety->private->safe_zone_max = va_arg(val, double);
			break;
		case 4:
			safety->private->safe_zone_min_speed = va_arg(val, double);
			break;
		case 5:
			safety->private->safe_zone_max_speed = va_arg(val, double);
			break;
		case 6:
			safety->private->torsion_max = va_arg(val, double);
			break;
		case 7:
			safety->private->torsion_recover_max = va_arg(val, double);
			break;
		case 8: {
			bool recover = va_arg(val, int);
		
			if (recover && !safety->private->recover) {
				log_send(LOG_T_WARNING,
				    "%s: Switching to recover mode, beware!",
				    safety->name);
			}
			if (!recover && safety->private->recover) {
				log_send(LOG_T_INFO,
				    "%s: Switching back to normal mode",
				    safety->name);
			}
		
			safety->private->recover = recover;
		
			break;
		}
		case 9:
			safety->private->torque_max = va_arg(val, double);
			break;
		case 10:
			safety->private->emergency_torque = va_arg(val, double);
			break;
	}
}

static struct controller_block_outterm_list outterms[] = {
	{ "speed_out_r",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_out_r)   },
	{ "speed_out_l",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_out_l)   },
	{ "safe_out",     CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safe_out)       },
	{ "torque_out_r", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_out_r) },
	{ "torque_out_l", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_out_l) },
	{ NULL }
};

static struct controller_block_interm_list interms[] = {
	{ "speed_in_r",       CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, input_r)         },
	{ "speed_in_l",       CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, input_l)         },
	{ "position_in_r",    CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position_in_r)   },
	{ "position_in_l",    CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position_in_l)   },
	{ "torsion_in",       CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torsion_in)      },
	{ "torque_in",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in)       },
	{ "torque_in_r",      CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in_r)     },
	{ "torque_in_l",      CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in_l)     },
	{ "safety_in_top",    CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_top)    },
	{ "safety_in_bottom", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_bottom) },
	{ "enable",           CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_in)        },
	{ NULL }
};

struct controller_block * block_dt_el_safety_create(char *name)
{
	struct controller_block *safety;
	
	safety = malloc(sizeof(struct controller_block));
	if (!safety)
		return NULL;

	safety->type = "dt_el_safety";
	safety->name = malloc(strlen(name)+1);
	if (!safety->name)
		goto err_safety;
	strcpy(safety->name, name);

	safety->private = malloc(sizeof(struct controller_block_private));
	if (!safety->private)
		goto err_name;
	safety->private->speed_out_r = 0.0;
	safety->private->speed_out_l = 0.0;
	safety->private->safe_out = 0.0;
	safety->private->position_min = 0.0;
	safety->private->position_max = 0.0;
	safety->private->safe_zone_min = 0.0;
	safety->private->safe_zone_max = 0.0;
	safety->private->safe_zone_min_speed = 0.0;
	safety->private->safe_zone_max_speed = 0.0;
	safety->private->recover = 0;
	safety->private->torsion_max = 0.0;
	safety->private->torsion_recover_max = 0.0;
	safety->private->torque_max = 0.0;
	safety->private->torque_out_r = 0.0;
	safety->private->torque_out_l = 0.0;
	safety->private->emergency_torque = 0.0;

	safety->private->err_max_r = false;
	safety->private->err_min_r = false;
	safety->private->err_max_l = false;
	safety->private->err_min_l = false;
	safety->private->err_torsion = false;
	safety->private->warn_safe_max_r = false;
	safety->private->warn_safe_min_r = false;
	safety->private->warn_safe_max_l = false;
	safety->private->warn_safe_min_l = false;
	safety->private->err_hwsafe_top = false;
	safety->private->err_hwsafe_bottom = false;
	safety->private->err_torsion_recover = false;

	if (controller_block_interm_list_init(safety, interms))
		goto err_private;

	if (controller_block_outterm_list_init(safety, outterms))
		goto err_input;


	safety->calculate = dt_el_safety_calculate;

	if (controller_block_param_list_init(safety, params))
		goto err_output;

	safety->param_get = param_get;
	safety->param_set = param_set;

	controller_block_add(safety);
	return safety;

err_output:
	free(safety->output);
err_input:
	free(safety->input);
err_private:
	free(safety->private);
err_name:
	free(safety->name);
err_safety:
	free(safety);
	return NULL;
}