Rename 3d order SPG and add 1st order

controller/block/block_setpoint_generator.h

-/*
-	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2009
-	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2009
-
-	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/>.
- */
-
-#ifndef _INCLUDE_BLOCK_SETPOINT_GENERATOR_
-#define _INCLUDE_BLOCK_SETPOINT_GENERATOR_
-
-#include "controller_block.h"
-
-struct controller_block * block_setpoint_generator_create(char *name, va_list ap);
-
-#endif /* _INCLUDE_BLOCK_SETPOINT_GENERATOR_ */

controller/block/block_setpoint_generator_1d.c

+/*
+	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2014
+	Copyright Stichting C.A. Muller Radioastronomiestation, 2014
+
+	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/>.
+
+ */
+#define _ISOC99_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <float.h>
+
+#include <controller/controller_block.h>
+#include <controller/controller_command.h>
+#include <controller/controller_sample.h>
+#include <log/log.h>
+
+/*
+       inputs     outputs
+       nr name    nr name
+
+     --------------------------
+     |                        |
+ ----| 0  reset_x 0  x        |----
+     |                        |
+ ----| 1  reset   1  v        |----
+     |                        |
+     |            2  setpoint |----
+     |                        |
+     --------------------------
+
+	'setpoint' is the setpoint as the user requested it.
+	'x' is the output of the setpoint generator.
+	'v', is the first order derivative of 'x'.
+
+ */
+
+struct controller_block_private {
+	/*
+		In the setpoint generator blocks the following physics notations
+		will be used.
+	
+		x = position	
+		v = velocity (1st derivative of x)
+	*/
+
+
+	float *reset_x;
+	bool  *reset;
+	
+	/* beware: 'samples' is the time unit, not 'seconds', 
+	   all parameters and commands are scaled on block entry/exit */
+	float cmd_x;
+	float cmd_v;
+	float max_x;
+	float min_x;
+	float max_v;
+
+	/* conversion factor for seconds/tick and its inverse */
+	float freq;	/* ticks per second */
+
+	/* parameters in real world format (time unit: second) */
+	float max_v_sec;
+
+	/* current internal state */
+	float cur_x;
+	float cur_v;
+	
+	struct command_entry cur_command;
+	bool cur_done;
+	bool cur_start;
+
+	struct controller_command *command;
+};
+
+
+static void setpoint_generator_1d_calculate(struct controller_block *spg)
+{
+	struct controller_block_private *priv = spg->private;
+	float cur_x, cur_v;
+
+	cur_x = priv->cur_x;
+	cur_v = priv->cur_v;
+
+	if (*priv->reset) {
+		priv->cmd_x = *priv->reset_x;
+		cur_x = priv->cmd_x;
+		priv->cur_done = true;
+		priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
+		priv->cmd_v = 0.0;
+		cur_v = 0.0;
+	}
+
+	if (priv->cur_done) {
+		int r;
+		
+		r = controller_command_queue_read(priv->command, &priv->cur_command);
+		if (r == 0) {
+			priv->cur_done = false;
+			priv->cur_start = false;
+		} else {
+			if (priv->cur_command.type ==
+			    COMMAND_PTYPE_SETPOINT_TIME) {
+				priv->cmd_v = 0.0;
+				priv->cmd_x = priv->cur_command.value.f;
+			}
+		}
+	}
+	if (!priv->cur_done) {
+		double t;
+			
+		switch (priv->cur_command.type) {
+			case COMMAND_PTYPE_SETPOINT:
+				priv->cmd_x = priv->cur_command.value.f;
+				priv->cmd_v = 0.0;
+				priv->cur_done = true;
+				break;
+			case COMMAND_PTYPE_SPEED:
+				priv->cmd_v = priv->cur_command.value.f;
+				priv->cur_done = true;
+				break;
+			case COMMAND_PTYPE_SETPOINT_TIME:
+				if (!priv->cur_start) {
+					double t_samplenr;
+
+					t_samplenr = floor(
+					    ((double)priv->cur_command.t.tv_nsec * priv->freq) /
+					    1000000.0);
+
+					t = (double)(priv->cur_command.t.tv_sec
+					    - controller_time_seconds) * priv->freq +
+					    t_samplenr -
+					    (double)controller_time_samplenr;
+
+					if (t <= 0.0) {
+						/* Command is to old, pretend it
+						   is a setpoint without time */
+						log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%lld < %d)",
+						    spg->name, (long long)priv->cur_command.t.tv_sec, controller_time_seconds);
+						priv->cmd_x = priv->cur_command.value.f;
+						priv->cmd_v = 0.0;
+						priv->cur_done = true;
+					} else {
+						priv->cmd_v =
+						    (priv->cur_command.value.f -
+						    priv->cmd_x) / (t+1);
+						priv->cur_start = true;
+					}
+				}
+				
+				if (priv->cur_command.t.tv_sec <=
+				    controller_time_seconds) {
+					priv->cur_done = true;
+					priv->cmd_x =
+					   priv->cur_command.value.f -
+					   priv->cmd_v;
+				}
+				break;
+			default:
+				break;
+		}
+	}
+
+	if (fabs(priv->cmd_v) >= priv->max_v) {
+		priv->cmd_v = copysign(priv->max_v, priv->cmd_v);
+	}	
+
+	if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
+		priv->cmd_x = cur_x + priv->cmd_v * controller_sample_period();
+		if (priv->cmd_x > priv->max_x)
+			priv->cmd_x = priv->max_x;
+		if (priv->cmd_x < priv->min_x)
+			priv->cmd_x = priv->min_x;
+		cur_x = priv->cmd_x;
+		cur_v = priv->cmd_v;
+	} else {
+		float x_diff = priv->cmd_x - cur_x;
+
+		if (fabs(x_diff) <= priv->max_v) {
+			cur_x = priv->cmd_x;
+			cur_v = 0;			
+		} else {
+			float v = copysign(priv->max_v, x_diff);
+
+			cur_x += v;
+			cur_v = v;
+		}
+	}
+	
+	priv->cur_x = cur_x;
+	priv->cur_v = cur_v * priv->freq;
+}
+
+static int block_setpoint_generator_command_filter(struct controller_command *command,
+    struct command_entry  *entry)
+{
+	struct controller_block_private *priv = command->block->private;
+	int r = 0;
+	
+	switch (entry->type) {
+		case COMMAND_PTYPE_SETPOINT_TIME:
+		case COMMAND_PTYPE_SETPOINT: {
+			float value = entry->value.f;
+			
+			if (value > priv->max_x)
+				value = priv->max_x;
+			if (value < priv->min_x)
+				value = priv->min_x;
+				
+			entry->value.f = value;
+			break;
+		}
+		case COMMAND_PTYPE_SPEED: {
+			float value = entry->value.f;
+			
+			if (fabs(value) > priv->max_v_sec)
+				value = copysign(priv->max_v_sec, value);
+			
+			entry->value.f = value;
+			
+			break;
+		}
+		default:
+			r = -1;
+			break;
+	}
+
+	return r;
+}
+
+static struct controller_block_param_list params[] = {
+	{ "setpoint",    true  },
+	{ "max_x",       true  },
+	{ "min_x",       true  },
+	{ "max_v",       true  },
+	{ NULL },
+};
+
+static void param_get(struct controller_block *spg, int param, void *val)
+{
+	switch (param) {
+		case 0:
+			*(float*)val =
+			    spg->private->cmd_x;
+			break;
+		case 1:
+			*(float*)val =
+			    spg->private->max_x;
+			break;
+		case 2:
+			*(float*)val =
+			    spg->private->min_x;
+			break;
+		case 3:
+			*(float*)val =
+			    spg->private->max_v_sec;
+			break;
+	}
+}
+
+static void param_set(struct controller_block *spg, int param, va_list val)
+{
+	switch (param) {
+		case 0:
+			spg->private->cmd_x = va_arg(val, double);
+			spg->private->cur_x = spg->private->cmd_x;
+			spg->private->cur_done = true;
+			spg->private->cmd_v = 0.0;
+			spg->private->cur_v = 0.0;
+			break;
+		case 1:
+			spg->private->max_x = va_arg(val, double);
+			break;
+		case 2:
+			spg->private->min_x = va_arg(val, double);
+			break;
+		case 3:
+			spg->private->max_v_sec = va_arg(val, double);
+			break;
+	}
+
+	/* Scale all settings to sample time unit */
+	spg->private->max_v = spg->private->max_v_sec * controller_sample_period();
+
+	spg->private->freq = 1.0 / controller_sample_period();
+}
+
+static struct controller_block_interm_list interms[] = {
+	{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
+	{ "reset",   CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, reset)   },
+	{ NULL }
+};
+
+static struct controller_block_outterm_list outterms[] = {
+	{ "x",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x) },
+	{ "v",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v) },
+	{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x) },
+	{ NULL }
+};
+
+
+struct controller_block * block_setpoint_generator_1d_create(char *name, va_list ap)
+{
+	struct controller_block *spg;
+	char *server_name;
+	char *spg_unit;
+	
+	server_name = va_arg(ap, char *);
+	spg_unit = va_arg(ap, char *);
+
+	spg = malloc(sizeof(struct controller_block));
+	if (!spg)
+		return NULL;
+
+	spg->type = "setpoint_generator_1d";
+	spg->name = malloc(strlen(name)+1);
+	if (!spg->name)
+		goto err_spg;
+	strcpy(spg->name, name);
+
+	spg->private = malloc(sizeof(struct controller_block_private));
+	if (!spg->private)
+		goto err_name;
+	spg->private->cmd_x = 0.0;
+	spg->private->cmd_v = 0.0;
+	spg->private->max_x = 0.0;
+	spg->private->min_x = 0.0;
+	spg->private->max_v = 0.0;
+	spg->private->max_v_sec = 0.0;
+	spg->private->freq = 1.0;
+
+	spg->private->cur_x = 0.0;
+	spg->private->cur_v = 0.0;
+
+	if (controller_block_interm_list_init(spg, interms))
+		goto err_private;
+
+	if (controller_block_outterm_list_init(spg, outterms))
+		goto err_input;
+
+	spg->calculate = setpoint_generator_1d_calculate;
+
+	if (controller_block_param_list_init(spg, params))
+		goto err_output;
+
+	spg->param_get = param_get;
+	spg->param_set = param_set;
+
+	spg->private->cur_done = true;
+	spg->private->cur_start = false;
+
+	controller_block_add(spg);
+
+	spg->private->command = controller_command_create(spg, 
+	    server_name, spg_unit);
+	spg->private->command->value_type = COMMAND_VALUE_TYPE_FLOAT;
+	spg->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;
+	spg->private->command->command_types[1] = COMMAND_PTYPE_SPEED;
+	spg->private->command->command_types[2] = COMMAND_PTYPE_SETPOINT_TIME;
+	spg->private->command->filter =
+	    block_setpoint_generator_command_filter;
+	
+	return spg;
+
+err_output:
+	free(spg->output);
+err_input:
+	free(spg->input);
+err_private:
+	free(spg->private);
+err_name:
+	free(spg->name);
+err_spg:
+	free(spg);
+	return NULL;
+}

controller/block/block_setpoint_generator.c → controller/block/block_setpoint_generator_3d.c

 /*
-	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013
+	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013, 2014
 	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013
 
 	This program is free software: you can redistribute it and/or modify
@@ -215,7 +215,7 @@ static double x_after_ticks(struct controller_block_private *priv,
 	return x_at_t;
 }
 
-static void setpoint_generator_calculate(struct controller_block *spg)
+static void setpoint_generator_3d_calculate(struct controller_block *spg)
 {
 	struct controller_block_private *priv = spg->private;
 	double cur_x, cur_v;
@@ -943,7 +943,7 @@ static struct controller_block_outterm_list outterms[] = {
 };
 
 
-struct controller_block * block_setpoint_generator_create(char *name, va_list ap)
+struct controller_block * block_setpoint_generator_3d_create(char *name, va_list ap)
 {
 	struct controller_block *spg;
 	char *server_name;
@@ -956,7 +956,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
 	if (!spg)
 		return NULL;
 
-	spg->type = "setpoint_generator";
+	spg->type = "setpoint_generator_3d";
 	spg->name = malloc(strlen(name)+1);
 	if (!spg->name)
 		goto err_spg;
@@ -1009,7 +1009,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
 	if (controller_block_outterm_list_init(spg, outterms))
 		goto err_input;
 
-	spg->calculate = setpoint_generator_calculate;
+	spg->calculate = setpoint_generator_3d_calculate;
 
 	if (controller_block_param_list_init(spg, params))
 		goto err_output;

controller/block/build.mk

@@ -27,7 +27,8 @@ BLOCKS := \
 	quantize \
 	random \
 	rangecheck \
-	setpoint_generator \
+	setpoint_generator_1d \
+	setpoint_generator_3d \
 	servo_state \
 	sine \
 	state_machine \

controller/dt_ctrl.ctrl

@@ -28,52 +28,52 @@ import "dt_ctrl_az_sim.ctrl"
 import "dt_ctrl_ec_sim.ctrl"
 
 blocks {
-	{ "setpoint_generator", "azimuth_spg", 			"Azimuth_Setpoint", "rad" }
-	{ "servo_state",        "azimuth_servo_state"              }
-	{ "and2",               "azimuth_safe_and"                 }
-	{ "subtract",	        "azimuth_setpoint_error"	   }
-	{ "subtract",	        "azimuth_error"			   }
-	{ "pid_aw",	        "azimuth_pid"			   }
-	{ "filter_iir",	        "azimuth_pid_filter"		   }
-	{ "limit_var",	        "azimuth_pid_limit"		   }
-	{ "add",  	        "azimuth_speed_ff" 		   }
-	{ "limit",	        "azimuth_speed_limit"		   }
-	{ "dt_az_safety",       "azimuth_safety"		   }
-	{ "gain", 	        "azimuth_speed_servo"		   }
-	{ "value",	        "azimuth_torque"		   }
-	{ "value",	        "azimuth_position_offset"  	   }
-	{ "add",  	        "azimuth_position_offset_sum"	   }
-	{ "gain", 	        "azimuth_position_gain"		   }
-	{ "rangecheck",		"azimuth_speed_warning"            }
+	{ "setpoint_generator_3d", "azimuth_spg", 	"Azimuth_Setpoint", "rad" }
+	{ "servo_state",           "azimuth_servo_state"           }
+	{ "and2",                  "azimuth_safe_and"              }
+	{ "subtract",	           "azimuth_setpoint_error"	   }
+	{ "subtract",              "azimuth_error"		   }
+	{ "pid_aw",	           "azimuth_pid"		   }
+	{ "filter_iir",	           "azimuth_pid_filter"		   }
+	{ "limit_var",	           "azimuth_pid_limit"		   }
+	{ "add",  	           "azimuth_speed_ff" 		   }
+	{ "limit",	           "azimuth_speed_limit"	   }
+	{ "dt_az_safety",          "azimuth_safety"		   }
+	{ "gain", 	           "azimuth_speed_servo"	   }
+	{ "value",	           "azimuth_torque"		   }
+	{ "value",	           "azimuth_position_offset"  	   }
+	{ "add",  	           "azimuth_position_offset_sum"   }
+	{ "gain", 	           "azimuth_position_gain"	   }
+	{ "rangecheck",		   "azimuth_speed_warning"         }
 	
-	{ "matrix_2x2",	        "elevation_input_matrix"	   }
-	{ "setpoint_generator", "elevation_spg", 		"Elevation_Setpoint", "rad" }
-	{ "servo_state",        "elevation_servo_state"            }
- 	{ "and2",               "elevation_safe_and"               }
-	{ "setpoint_generator", "elevation_torsion_spg",	"Elevation_Torsion_Setpoint", "rad" }
-	{ "subtract",	        "elevation_setpoint_error" 	   }
-	{ "subtract",  	        "elevation_error"  		   }
-	{ "subtract",	        "elevation_torsion_error"  	   }
-	{ "subtract",	        "elevation_torsion_setpoint_error" }
-	{ "subtract",	        "elevation_torsion_torque" 	   }
-	{ "filter_lp",	        "elevation_torsion_torque_lp"	   }
-	{ "pid_aw",	        "elevation_pid"			   }
-	{ "pid_aw",  	        "elevation_torsion_pid"		   }
-	{ "add",  	        "elevation_speed_ff"		   }
-	{ "switch",             "elevation_position_switch"        }
-	{ "limit",	        "elevation_torsion_speed_limit"	   }
-	{ "limit",	        "elevation_speed_limit"		   }
-	{ "limit_2nd",	        "elevation_jerk_limit"		   }
-	{ "dt_el_safety",       "elevation_safety" 		   }
-	{ "matrix_2x2",	        "elevation_output_matrix"  	   }
-	{ "value",	        "elevation_torque_r"		   }
-	{ "value",	        "elevation_torque_l"		   }
-	{ "value",	        "elevation_position_offset_r"      }
-	{ "value",	        "elevation_position_offset_l"	   }
-	{ "add",  	        "elevation_position_offset_r_sum"  }
-	{ "add",  	        "elevation_position_offset_l_sum"  }
+	{ "matrix_2x2",	           "elevation_input_matrix"	      }
+	{ "setpoint_generator_3d", "elevation_spg", 		"Elevation_Setpoint", "rad" }
+	{ "servo_state",           "elevation_servo_state"            }
+ 	{ "and2",                  "elevation_safe_and"               }
+	{ "setpoint_generator_1d", "elevation_torsion_spg",	"Elevation_Torsion_Setpoint", "rad" }
+	{ "subtract",	           "elevation_setpoint_error" 	      }
+	{ "subtract",  	           "elevation_error"  		      }
+	{ "subtract",	           "elevation_torsion_error"  	      }
+	{ "subtract",	           "elevation_torsion_setpoint_error" }
+	{ "subtract",	           "elevation_torsion_torque" 	      }
+	{ "filter_lp",	           "elevation_torsion_torque_lp"      }
+	{ "pid_aw",	           "elevation_pid"	      	      }
+	{ "pid_aw",  	           "elevation_torsion_pid"	      }
+	{ "add",  	           "elevation_speed_ff"		      }
+	{ "switch",                "elevation_position_switch"        }
+	{ "limit",	           "elevation_torsion_speed_limit"    }
+	{ "limit",	           "elevation_speed_limit"	      }
+	{ "limit_2nd",	           "elevation_jerk_limit"	      }
+	{ "dt_el_safety",          "elevation_safety" 		      }
+	{ "matrix_2x2",	           "elevation_output_matrix"  	      }
+	{ "value",	           "elevation_torque_r"		      }
+	{ "value",	           "elevation_torque_l"		      }
+	{ "value",	           "elevation_position_offset_r"      }
+	{ "value",	           "elevation_position_offset_l"      }
+	{ "add",  	           "elevation_position_offset_r_sum"  }
+	{ "add",  	           "elevation_position_offset_l_sum"  }
 
-	{ "value_bool", 	"false"	}
+	{ "value_bool", 	   "false"	}
 }
 
 links {
@@ -325,15 +325,9 @@ params {
 	{ "elevation_servo_state", "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
 	{ "elevation_servo_state", "max_a",               (float) 0.0003   	}
 	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
-	{ "elevation_torsion_spg", "t",                   (float) 0.004         }
 	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
 	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
 	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
-	{ "elevation_torsion_spg", "max_a",               (float) 0.00004 	}
-	{ "elevation_torsion_spg", "max_j",               (float) 0.00001 	}
-	{ "elevation_torsion_spg", "precision_x",         (float) 0.000001      }
-	{ "elevation_torsion_spg", "precision_a",         (float) 0.000001      }
-	{ "elevation_torsion_spg", "precision_v",         (float) 0.000001      }
 	{ "elevation_pid",         "kp",                  (float) 0.80     	}
 	{ "elevation_pid",         "ki",                  (float) 0.001  	}
 	{ "elevation_pid",         "kd",                  (float) 0.001   	}