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

src/modules/commander/accelerometer_calibration.c

@@ -1,12 +1,45 @@
-/*
+/****************************************************************************
- * accelerometer_calibration.c
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
- * Transform acceleration vector to true orientation and scale
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
  *
- * * * * Model * * *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file accelerometer_calibration.c
+ *
+ * Implementation of accelerometer calibration.
+ *
+ * Transform acceleration vector to true orientation, scale and offset
+ *
+ * ===== Model =====
  * accel_corr = accel_T * (accel_raw - accel_offs)
  *
  * accel_corr[3] - fully corrected acceleration vector in body frame
@@ -14,7 +47,7 @@
  * accel_raw[3]  - raw acceleration vector
  * accel_offs[3] - acceleration offset vector
  *
- * * * * Calibration * * *
+ * ===== Calibration =====
  *
  * Reference vectors
  * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
@@ -34,7 +67,6 @@
  *
  * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
  *
- *
  * Find accel_T
  *
  * 9 unknown constants
@@ -67,6 +99,8 @@
  *
  * accel_T = A^-1 * g
  * g = 9.80665
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include "accelerometer_calibration.h"

src/modules/commander/accelerometer_calibration.h

@@ -1,8 +1,43 @@
-/*
+/****************************************************************************
- * accelerometer_calibration.h
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file accelerometer_calibration.h
+ *
+ * Definition of accelerometer calibration.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef ACCELEROMETER_CALIBRATION_H_

src/modules/fixedwing_att_control/fixedwing_att_control_att.c

@@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	if (!initialized) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
+		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
-		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
+		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
 		initialized = true;
 	}
 

src/modules/fixedwing_att_control/fixedwing_att_control_rate.c

@@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (!initialized) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
+		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller
-		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
 		initialized = true;
 	}
 

src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c

@@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 	parameters_init(&h);
 	parameters_update(&h, &p);
-	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
+	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit
-	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
+	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
-	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
+	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
-	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
+	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value
 
 	/* error and performance monitoring */
 	perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");

src/modules/multirotor_att_control/multirotor_attitude_control.c

@@ -1,12 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.c
- * Implementation of attitude controller
+ *
+ * Implementation of attitude controller for multirotors.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include "multirotor_attitude_control.h"
@@ -163,16 +171,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
 	float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
 	last_run = hrt_absolute_time();
 
 	if (last_input != att_sp->timestamp) {
 		last_input = att_sp->timestamp;
 	}
 
-	static int sensor_delay;
-	sensor_delay = hrt_absolute_time() - att->timestamp;
-
 	static int motor_skip_counter = 0;
 
 	static PID_t pitch_controller;
@@ -190,10 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_init(&h);
 		parameters_update(&h, &p);
 
-		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
+		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
-			 1000.0f, PID_MODE_DERIVATIV_SET);
+		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
-		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-			 1000.0f, PID_MODE_DERIVATIV_SET);
 
 		initialized = true;
 	}

src/modules/multirotor_att_control/multirotor_attitude_control.h

@@ -1,12 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.h
- * Attitude control for multi rotors.
+ *
+ * Definition of attitude controller for multirotors.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef MULTIROTOR_ATTITUDE_CONTROL_H_

src/modules/multirotor_att_control/multirotor_rate_control.c

@@ -1,8 +1,10 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
  *           Lorenz Meier <lm@inf.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,10 +38,12 @@
 /**
  * @file multirotor_rate_control.c
  *
- * Implementation of rate controller
+ * Implementation of rate controller for multirotors.
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include "multirotor_rate_control.h"
@@ -150,14 +154,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 			      const float rates[], struct actuator_controls_s *actuators)
 {
-	static float roll_control_last  = 0;
-	static float pitch_control_last = 0;
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 	static uint64_t last_input = 0;
 
-	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
-
 	if (last_input != rate_sp->timestamp) {
 		last_input = rate_sp->timestamp;
 	}
@@ -166,6 +166,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static int motor_skip_counter = 0;
 
+	static PID_t pitch_rate_controller;
+	static PID_t roll_rate_controller;
+
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
 
@@ -176,43 +179,35 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		parameters_init(&h);
 		parameters_update(&h, &p);
 		initialized = true;
+
+		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
+		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
+
 	}
 
 	/* load new parameters with lower rate */
 	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
+		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
-		// 	(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
 	}
 
-	/* calculate current control outputs */
+	/* reset integral if on ground */
+	if (rate_sp->thrust < 0.01f) {
+		pid_reset_integral(&pitch_rate_controller);
+		pid_reset_integral(&roll_rate_controller);
+	}
 
 	/* control pitch (forward) output */
-	float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
+	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
-
+					    rates[1], 0.0f, deltaT);
-	/* increase resilience to faulty control inputs */
-	if (isfinite(pitch_control)) {
-		pitch_control_last = pitch_control;
-
-	} else {
-		pitch_control = 0.0f;
-		warnx("rej. NaN ctrl pitch");
-	}
 
 	/* control roll (left/right) output */
-	float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
+	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
+					   rates[0], 0.0f, deltaT);
 
-	/* increase resilience to faulty control inputs */
+	/* control yaw rate */ //XXX use library here
-	if (isfinite(roll_control)) {
-		roll_control_last = roll_control;
-
-	} else {
-		roll_control = 0.0f;
-		warnx("rej. NaN ctrl roll");
-	}
-
-	/* control yaw rate */
 	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
 
 	/* increase resilience to faulty control inputs */

src/modules/multirotor_att_control/multirotor_rate_control.h

@@ -1,12 +1,12 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.h
- * Attitude control for multi rotors.
+ *
+ * Definition of rate controller for multirotors.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef MULTIROTOR_RATE_CONTROL_H_

src/modules/sdlog2/logbuffer.c

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
  *
  * Ring FIFO buffer for binary log data.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <string.h>

src/modules/sdlog2/logbuffer.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
  *
  * Ring FIFO buffer for binary log data.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef SDLOG2_RINGBUFFER_H_

src/modules/sdlog2/sdlog2.c

@@ -2,7 +2,7 @@
  *
  *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <rk3dov@gmail.com>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,7 +40,7 @@
  * does the heavy SD I/O in a low-priority worker thread.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -685,6 +685,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_AIRS_s log_AIRS;
 			struct log_ARSP_s log_ARSP;
 			struct log_FLOW_s log_FLOW;
+			struct log_GPOS_s log_GPOS;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1056,7 +1057,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- GLOBAL POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-				// TODO not implemented yet
+				log_msg.msg_type = LOG_GPOS_MSG;
+				log_msg.body.log_GPOS.lat = buf.global_pos.lat;
+				log_msg.body.log_GPOS.lon = buf.global_pos.lon;
+				log_msg.body.log_GPOS.alt = buf.global_pos.alt;
+				log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
+				log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
+				log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
+				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
 			/* --- VICON POSITION --- */

src/modules/sdlog2/sdlog2_format.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
  *
  * General log format structures and macro.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 /*

src/modules/sdlog2/sdlog2_messages.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
  *
  * Log messages and structures definition.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef SDLOG2_MESSAGES_H_
@@ -198,6 +198,17 @@ struct log_FLOW_s {
 	uint8_t	quality;
 	uint8_t sensor_id;
 };
+
+/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
+#define LOG_GPOS_MSG 16
+struct log_GPOS_s {
+	int32_t lat;
+	int32_t lon;
+	float alt;
+	float vel_n;
+	float vel_e;
+	float vel_d;
+};
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -218,6 +229,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
+	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);

src/modules/systemlib/pid/pid.c

@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@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->dt_min = dt_min;
-	pid->saturated = 0;
+	pid->count = 0.0f;
-	pid->last_output = 0;
+	pid->saturated = 0.0f;
-
+	pid->last_output = 0.0f;
-	pid->sp = 0;
+	pid->sp = 0.0f;
-	pid->error_previous = 0;
+	pid->error_previous = 0.0f;
-	pid->integral = 0;
+	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,16 +183,18 @@ __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 (output < -pid->limit) output = -pid->limit;
 
 	if (isfinite(output)) {
-		pid->last_output = output;
+		if (output > pid->limit) {
+			output = pid->limit;
+
+		} else if (output < -pid->limit) {
+			output = -pid->limit;
 		}
 
+		pid->last_output = output;
+	}
 
 	return pid->last_output;
 }

src/modules/systemlib/pid/pid.h

@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@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);