Merge branch 'master' of github.com:PX4/Firmware into origin_integration

2 files changed, 60 insertions, 32 deletions

diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 49315cdc9..4996a8f66 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,14 +38,21 @@
 
 /**
  * @file pid.c
- * Implementation of generic PID control interface
+ *
+ * Implementation of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include "pid.h"
 #include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-		       float limit, uint8_t mode)
+		       float limit, uint8_t mode, float dt_min)
 {
 	pid->kp = kp;
 	pid->ki = ki;
@@ -51,13 +60,13 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->intmax = intmax;
 	pid->limit = limit;
 	pid->mode = mode;
-	pid->count = 0;
-	pid->saturated = 0;
-	pid->last_output = 0;
-
-	pid->sp = 0;
-	pid->error_previous = 0;
-	pid->integral = 0;
+	pid->dt_min = dt_min;
+	pid->count = 0.0f;
+	pid->saturated = 0.0f;
+	pid->last_output = 0.0f;
+	pid->sp = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->integral = 0.0f;
 }
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
 {
@@ -136,14 +145,14 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculated current error value
 	float error = pid->sp - val;
 
-	if (isfinite(error)) {				// Why is this necessary?  DEW
-		pid->error_previous = error;
-	}
-
 	// Calculate or measured current error derivative
-
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / dt;
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
 
 	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
 		d = -val_dot;
@@ -152,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
+
 	// Calculate the error integral and check for saturation
 	i = pid->integral + (error * dt);
 
@@ -162,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 	} else {
 		if (!isfinite(i)) {
-			i = 0;
+			i = 0.0f;
 		}
 
 		pid->integral = i;
@@ -170,17 +183,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	}
 
 	// Calculate the output.  Limit output magnitude to pid->limit
-	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
-	if (output > pid->limit) output = pid->limit;
+	if (isfinite(output)) {
+		if (output > pid->limit) {
+			output = pid->limit;
 
-	if (output < -pid->limit) output = -pid->limit;
+		} else if (output < -pid->limit) {
+			output = -pid->limit;
+		}
 
-	if (isfinite(output)) {
 		pid->last_output = output;
 	}
 
-
 	return pid->last_output;
 }
 
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index 64d668867..714bf988f 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +38,14 @@
 
 /**
  * @file pid.h
- * Definition of generic PID control interface
+ *
+ * Definition of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef PID_H_
@@ -47,8 +56,11 @@
 /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
  * val_dot in pid_calculate() will be ignored */
 #define PID_MODE_DERIVATIV_CALC	0
+/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
+ * val_dot in pid_calculate() will be ignored */
+#define PID_MODE_DERIVATIV_CALC_NO_SP	1
 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-#define PID_MODE_DERIVATIV_SET	1
+#define PID_MODE_DERIVATIV_SET	2
 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
 #define PID_MODE_DERIVATIV_NONE 9
 
@@ -62,12 +74,13 @@ typedef struct {
 	float error_previous;
 	float last_output;
 	float limit;
+	float dt_min;
 	uint8_t mode;
 	uint8_t count;
 	uint8_t saturated;
 } PID_t;
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);