Added controller parameters, added vicon position reading

6 files changed, 262 insertions, 9 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index cec2593c1..580287805 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -68,6 +68,7 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -134,6 +135,8 @@ static struct vehicle_command_s vcmd;
 
 static struct actuator_armed_s armed;
 
+static struct vehicle_vicon_position_s vicon_position;
+
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -191,8 +194,10 @@ static struct mavlink_subscriptions {
 
 static struct mavlink_publications {
 	orb_advert_t offboard_control_sp_pub;
+	orb_advert_t vicon_position_pub;
 } mavlink_pubs = {
-	.offboard_control_sp_pub = -1
+	.offboard_control_sp_pub = -1,
+	.vicon_position_pub = -1
 };
 
 
@@ -1240,9 +1245,26 @@ void handleMessage(mavlink_message_t *msg)
 		/* check if topic is advertised */
 		if (cmd_pub <= 0) {
 			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+		} else {
+			/* create command */
+			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
+		}
+	}
+
+	/* Handle Vicon position estimates */
+	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
+		mavlink_vicon_position_estimate_t pos;
+		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
+
+		vicon_position.x = pos.x;
+		vicon_position.y = pos.y;
+		vicon_position.z = pos.z;
+
+		if (mavlink_pubs.vicon_position_pub <= 0) {
+			mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+		} else {
+			orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position);
 		}
-		/* create command */
-		orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 	}
 
 	/* Handle quadrotor motor setpoints */
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c
index 52f90bb93..7cf687306 100644
--- a/apps/multirotor_pos_control/multirotor_pos_control.c
+++ b/apps/multirotor_pos_control/multirotor_pos_control.c
@@ -55,9 +55,11 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_vicon_position.h>
 #include <systemlib/systemlib.h>
 
+#include "multirotor_pos_control_params.h"
+
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
@@ -144,7 +146,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
 	struct vehicle_attitude_s att;
 	//struct vehicle_global_position_setpoint_s global_pos_sp;
 	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_local_position_s local_pos;
+	struct vehicle_vicon_position_s local_pos;
 	struct manual_control_setpoint_s manual;
 	struct vehicle_attitude_setpoint_s att_sp;
 
@@ -152,7 +154,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
 	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 
@@ -161,6 +163,14 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
 
 	thread_running = true;
 
+	int loopcounter = 0;
+
+	struct multirotor_position_control_params p;
+	struct multirotor_position_control_param_handles h;
+	parameters_init(&h);
+	parameters_update(&h, &p);
+
+
 	while (1) {
 		/* get a local copy of the vehicle state */
 		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
@@ -169,16 +179,27 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
 		/* get a local copy of attitude */
 		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
 		/* get a local copy of local position setpoint */
 		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
 
+		if (loopcounter == 500) {
+			parameters_update(&h, &p);
+			loopcounter = 0;
+		}
+
 		// if (state.state_machine == SYSTEM_STATE_AUTO) {
 			
 			// XXX IMPLEMENT POSITION CONTROL HERE
 
-			att_sp.roll_body = 0.1f;
-			att_sp.pitch_body = 0.0f;
+			float dT = 1.0f / 50.0f;
+
+			float x_setpoint = 0.0f;
+
+			/* local pos is the Vicon position */
+
+			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT;
+			att_sp.roll_body = 0.0f;
 			att_sp.yaw_body = 0.0f;
 			att_sp.thrust = 0.4f;
 			att_sp.timestamp = hrt_absolute_time();
@@ -199,6 +220,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
 
 		/* run at approximately 50 Hz */
 		usleep(20000);
+		loopcounter++;
 
 	}
 
diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.c b/apps/multirotor_pos_control/multirotor_pos_control_params.c
new file mode 100644
index 000000000..6b73dc405
--- /dev/null
+++ b/apps/multirotor_pos_control/multirotor_pos_control_params.c
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.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
+ * 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 multirotor_position_control_params.c
+ * 
+ * Parameters for EKF filter
+ */
+
+#include "multirotor_pos_control_params.h"
+
+/* Extended Kalman Filter covariances */
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+
+int parameters_init(struct multirotor_position_control_param_handles *h)
+{
+	/* PID parameters */
+	h->p 	=	param_find("MC_POS_P");
+
+	return OK;
+}
+
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
+{
+	param_get(h->p, &(p->p));
+
+	return OK;
+}
diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.h b/apps/multirotor_pos_control/multirotor_pos_control_params.h
new file mode 100644
index 000000000..274f6c22a
--- /dev/null
+++ b/apps/multirotor_pos_control/multirotor_pos_control_params.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.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
+ * 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 multirotor_position_control_params.h
+ * 
+ * Parameters for position controller
+ */
+
+#include <systemlib/param/param.h>
+
+struct multirotor_position_control_params {
+	float p;
+	float i;
+	float d;
+};
+
+struct multirotor_position_control_param_handles {
+	param_t p;
+	param_t i;
+	param_t d;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct multirotor_position_control_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index f211648a9..b5e1d739c 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -77,6 +77,9 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 #include "topics/vehicle_local_position.h"
 ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
 
+#include "topics/vehicle_vicon_position.h"
+ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
+
 #include "topics/vehicle_rates_setpoint.h"
 ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
 
diff --git a/apps/uORB/topics/vehicle_vicon_position.h b/apps/uORB/topics/vehicle_vicon_position.h
new file mode 100644
index 000000000..91d933f44
--- /dev/null
+++ b/apps/uORB/topics/vehicle_vicon_position.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author 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
+ * 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 vehicle_vicon_position.h
+ * Definition of the raw VICON Motion Capture position
+ */
+
+#ifndef TOPIC_VEHICLE_VICON_POSITION_H_
+#define TOPIC_VEHICLE_VICON_POSITION_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Fused local position in NED.
+ */
+struct vehicle_vicon_position_s
+{
+	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
+	bool valid;				/**< true if position satisfies validity criteria of estimator */
+
+	float x;				/**< X positin in meters in NED earth-fixed frame */
+	float y;				/**< X positin in meters in NED earth-fixed frame */
+	float z;				/**< Z positin in meters in NED earth-fixed frame (negative altitude) */
+	float vx;
+	float vy;
+	float vz;
+
+	// TODO Add covariances here
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_vicon_position);
+
+#endif