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

9 files changed, 116 insertions, 77 deletions

diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 5d485b01f..064426f21 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,11 +43,7 @@
 #define CONVERSIONS_H_
 #include <float.h>
 #include <stdint.h>
-
-#define CONSTANTS_ONE_G						9.80665f		// m/s^2
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f			// kg/m^3
-#define CONSTANTS_AIR_GAS_CONST				287.1f 			// J/(kg * K)
-#define CONSTANTS_ABSOLUTE_NULL_CELSIUS		-273.15f		// °C
+#include <systemlib/geo/geo.h>
 
 __BEGIN_DECLS
 
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 20b711fa6..8fdff8ac0 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -1,9 +1,8 @@
 /****************************************************************************
- * configs/px4fmu/src/up_leds.c
- * arch/arm/src/board/up_leds.c
  *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ * 	     Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +14,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -34,18 +33,26 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
+/**
+ * @file cpuload.c
+ *
+ * Measurement of CPU load of each individual task.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ */
 #include <nuttx/config.h>
+#include <nuttx/sched.h>
 
+#include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
+
+#include <arch/arch.h>
+
 #include <debug.h>
 
 #include <sys/time.h>
-#include <sched.h>
 
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
@@ -54,26 +61,13 @@
 
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 
-/****************************************************************************
- * Definitions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-__EXPORT void sched_note_start(FAR _TCB *tcb);
-__EXPORT void sched_note_stop(FAR _TCB *tcb);
-__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb);
-
-/****************************************************************************
- * Name:
- ****************************************************************************/
+__EXPORT void sched_note_start(FAR struct tcb_s *tcb);
+__EXPORT void sched_note_stop(FAR struct tcb_s *tcb);
+__EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb);
 
 __EXPORT struct system_load_s system_load;
 
-extern FAR _TCB *sched_gettcb(pid_t pid);
+extern FAR struct _TCB *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
@@ -109,7 +103,7 @@ void cpuload_initialize_once()
 	//	}
 }
 
-void sched_note_start(FAR _TCB *tcb)
+void sched_note_start(FAR struct tcb_s *tcb)
 {
 	/* search first free slot */
 	int i;
@@ -128,7 +122,7 @@ void sched_note_start(FAR _TCB *tcb)
 	}
 }
 
-void sched_note_stop(FAR _TCB *tcb)
+void sched_note_stop(FAR struct tcb_s *tcb)
 {
 	int i;
 
@@ -145,7 +139,7 @@ void sched_note_stop(FAR _TCB *tcb)
 	}
 }
 
-void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
+void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb)
 {
 	uint64_t new_time = hrt_absolute_time();
 
diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h
index a97047ea8..c7aa18d3c 100644
--- a/src/modules/systemlib/cpuload.h
+++ b/src/modules/systemlib/cpuload.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,7 +43,7 @@ struct system_load_taskinfo_s {
 	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
 	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
 	uint64_t start_time;         ///< FIRST start time of task
-	FAR struct _TCB *tcb;               ///<
+	FAR struct tcb_s *tcb;               ///<
 	bool valid;                  ///< Task is currently active / valid
 };
 
@@ -60,4 +60,6 @@ __EXPORT extern struct system_load_s system_load;
 
 __EXPORT void cpuload_initialize_once(void);
 
+__END_DECLS
+
 #endif
diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index daf17ef8b..6c0e876d1 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -61,7 +61,7 @@ const char *
 getprogname(void)
 {
 #if CONFIG_TASK_NAME_SIZE > 0
-	_TCB	*thisproc = sched_self();
+	FAR struct tcb_s	*thisproc = sched_self();
 
 	return thisproc->name;
 #else
diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
index 84097b49f..dadec51ec 100644
--- a/src/modules/systemlib/geo/geo.h
+++ b/src/modules/systemlib/geo/geo.h
@@ -45,9 +45,23 @@
  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
 
+#pragma once
+
+__BEGIN_DECLS
 
 #include <stdbool.h>
 
+#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
+#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
+
+/* compatibility aliases */
+#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
+#define GRAVITY_MSS		CONSTANTS_ONE_G
+
+// XXX remove
 struct crosstrack_error_s {
 	bool past_end;		// Flag indicating we are past the end of the line/arc segment
 	float distance;		// Distance in meters to closest point on line/arc
@@ -111,3 +125,5 @@ __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
+
+__END_DECLS
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..eca228464 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_
@@ -44,11 +53,16 @@
 
 #include <stdint.h>
 
+__BEGIN_DECLS
+
 /* 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,17 +76,19 @@ 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);
 
 __EXPORT void pid_reset_integral(PID_t *pid);
 
+__END_DECLS
 
 #endif /* PID_H_ */
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index afb7eca29..3283aad8a 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -50,7 +50,7 @@
 
 #include "systemlib.h"
 
-static void kill_task(FAR _TCB *tcb, FAR void *arg);
+static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
 
 void killall()
 {
@@ -60,12 +60,12 @@ void killall()
 	sched_foreach(kill_task, NULL);
 }
 
-static void kill_task(FAR _TCB *tcb, FAR void *arg)
+static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
 {
 	kill(tcb->pid, SIGUSR1);
 }
 
-int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
 {
 	int pid;
 
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 2c53c648b..0194b5e52 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
 #include <float.h>
 #include <stdint.h>
 
-__BEGIN_DECLS
-
 /** Reboots the board */
 extern void up_systemreset(void) noreturn_function;
 
+__BEGIN_DECLS
+
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
@@ -58,7 +58,7 @@ __EXPORT void killall(void);
 #endif
 
 /** Starts a task and performs any specific accounting, scheduler setup, etc. */
-__EXPORT int task_spawn(const char *name,
+__EXPORT int task_spawn_cmd(const char *name,
 			int priority,
 			int scheduler,
 			int stack_size,