Merged master

src/modules/att_pos_estimator_ekf/KalmanNav.hpp

@@ -47,8 +47,8 @@
 #include <mathlib/mathlib.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
-#include <controllib/block/UOrbSubscription.hpp>
-#include <controllib/block/UOrbPublication.hpp>
+#include <controllib/uorb/UOrbSubscription.hpp>
+#include <controllib/uorb/UOrbPublication.hpp>
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -224,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 4);
+	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
+	orb_set_interval(sub_raw, 3);
 
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -236,7 +236,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
-
 	int loopcounter = 0;
 	int printcounter = 0;
 
@@ -384,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
+					if (!const_initialized && dt < 0.05f && dt > 0.001f) {
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
@@ -424,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+					if (last_data > 0 && raw.timestamp - last_data > 12000)
+						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
 
 					last_data = raw.timestamp;
 

src/modules/commander/accelerometer_calibration.cpp

@@ -275,8 +275,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
 	/* EMA time constant in seconds*/
 	float ema_len = 0.2f;
-	/* set "still" threshold to 0.1 m/s^2 */
-	float still_thr2 = pow(0.1f, 2);
+	/* set "still" threshold to 0.25 m/s^2 */
+	float still_thr2 = pow(0.25f, 2);
 	/* set accel error threshold to 5m/s^2 */
 	float accel_err_thr = 5.0f;
 	/* still time required in us */

src/modules/controllib/block/Block.cpp

@@ -43,8 +43,8 @@
 
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include "../uorb/UOrbSubscription.hpp"
+#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {

src/modules/controllib/blocks.hpp

@@ -42,6 +42,7 @@
 #include <assert.h>
 #include <time.h>
 #include <stdlib.h>
+#include <math.h>
 #include <mathlib/math/test/test.hpp>
 
 #include "block/Block.hpp"

src/modules/controllib/module.mk

@@ -37,7 +37,7 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   block/UOrbPublication.cpp \
-		   block/UOrbSubscription.cpp \
-		   blocks.cpp \
-		   fixedwing.cpp
+		   uorb/UOrbPublication.cpp \
+		   uorb/UOrbSubscription.cpp \
+		   uorb/blocks.cpp \
+		   blocks.cpp

src/modules/controllib/uorb/UOrbPublication.hpp

@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control

src/modules/controllib/uorb/UOrbSubscription.hpp

@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control

src/modules/controllib/uorb/blocks.cpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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
+ * 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 uorb_blocks.cpp
+ *
+ * uorb block library code
+ */
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
+	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
+	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
+	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
+	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+} // namespace control
+

src/modules/controllib/uorb/blocks.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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
+ * 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 uorb_blocks.h
+ *
+ * uorb block library code
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+
+extern "C" {
+#include <systemlib/geo/geo.h>
+}
+
+#include "../blocks.hpp"
+#include "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	virtual ~BlockWaypointGuidance();
+	void update(vehicle_global_position_s &pos,
+		    vehicle_attitude_s &att,
+		    vehicle_global_position_setpoint_s &posCmd,
+		    vehicle_global_position_setpoint_s &lastPosCmd);
+	float getPsiCmd() { return _psiCmd; }
+};
+
+/**
+ * UorbEnabledAutopilot
+ */
+class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
+{
+protected:
+	// subscriptions
+	UOrbSubscription<vehicle_attitude_s> _att;
+	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
+	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
+	UOrbSubscription<vehicle_global_position_s> _pos;
+	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
+	UOrbSubscription<manual_control_setpoint_s> _manual;
+	UOrbSubscription<vehicle_status_s> _status;
+	UOrbSubscription<parameter_update_s> _param_update;
+	// publications
+	UOrbPublication<actuator_controls_s> _actuators;
+public:
+	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
+	virtual ~BlockUorbEnabledAutopilot();
+};
+
+} // namespace control
+

src/modules/fixedwing_backside/fixedwing.cpp

@@ -88,61 +88,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
 	_yawDamper.update(rCmd, r, outputScale);
 }
 
-BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_xtYawLimit(this, "XT2YAW"),
-	_xt2Yaw(this, "XT2YAW"),
-	_psiCmd(0)
-{
-}
-
-BlockWaypointGuidance::~BlockWaypointGuidance() {};
-
-void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
-				   vehicle_attitude_s &att,
-				   vehicle_global_position_setpoint_s &posCmd,
-				   vehicle_global_position_setpoint_s &lastPosCmd)
-{
-
-	// heading to waypoint
-	float psiTrack = get_bearing_to_next_waypoint(
-				 (double)pos.lat / (double)1e7d,
-				 (double)pos.lon / (double)1e7d,
-				 (double)posCmd.lat / (double)1e7d,
-				 (double)posCmd.lon / (double)1e7d);
-
-	// cross track
-	struct crosstrack_error_s xtrackError;
-	get_distance_to_line(&xtrackError,
-			     (double)pos.lat / (double)1e7d,
-			     (double)pos.lon / (double)1e7d,
-			     (double)lastPosCmd.lat / (double)1e7d,
-			     (double)lastPosCmd.lon / (double)1e7d,
-			     (double)posCmd.lat / (double)1e7d,
-			     (double)posCmd.lon / (double)1e7d);
-
-	_psiCmd = _wrap_2pi(psiTrack -
-			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
-}
-
-BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	// subscriptions
-	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
-	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
-	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
-	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
-	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
-	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
-	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
-	// publications
-	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
-{
-}
-
-BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
-
 BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
 	BlockUorbEnabledAutopilot(parent, name),
 	_stabilization(this, ""), // no name needed, already unique
@@ -385,4 +330,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
 
 } // namespace control
 
-#endif
+#endif

src/modules/fixedwing_backside/fixedwing.hpp

@@ -39,31 +39,8 @@
 
 #pragma once
 
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <drivers/drv_hrt.h>
-#include <poll.h>
-
-#include "blocks.hpp"
-#include "block/UOrbSubscription.hpp"
-#include "block/UOrbPublication.hpp"
-
-extern "C" {
-#include <systemlib/geo/geo.h>
-}
+#include <controllib/blocks.hpp>
+#include <controllib/uorb/blocks.hpp>
 
 namespace control
 {
@@ -250,47 +227,6 @@ public:
  * than frontside at high speeds.
  */
 
-/**
- * Waypoint Guidance block
- */
-class __EXPORT BlockWaypointGuidance : public SuperBlock
-{
-private:
-	BlockLimitSym _xtYawLimit;
-	BlockP _xt2Yaw;
-	float _psiCmd;
-public:
-	BlockWaypointGuidance(SuperBlock *parent, const char *name);
-	virtual ~BlockWaypointGuidance();
-	void update(vehicle_global_position_s &pos,
-		    vehicle_attitude_s &att,
-		    vehicle_global_position_setpoint_s &posCmd,
-		    vehicle_global_position_setpoint_s &lastPosCmd);
-	float getPsiCmd() { return _psiCmd; }
-};
-
-/**
- * UorbEnabledAutopilot
- */
-class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
-{
-protected:
-	// subscriptions
-	UOrbSubscription<vehicle_attitude_s> _att;
-	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
-	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
-	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
-	// publications
-	UOrbPublication<actuator_controls_s> _actuators;
-public:
-	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
-	virtual ~BlockUorbEnabledAutopilot();
-};
-
 /**
  * Multi-mode Autopilot
  */

src/modules/fixedwing_backside/fixedwing_backside_main.cpp

@@ -45,12 +45,13 @@
 #include <stdlib.h>
 #include <string.h>
 #include <systemlib/systemlib.h>
-#include <controllib/fixedwing.hpp>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
 
+#include "fixedwing.hpp"
+
 static bool thread_should_exit = false;     /**< Deamon exit flag */
 static bool thread_running = false;     /**< Deamon status flag */
 static int deamon_task;             /**< Handle of deamon task / thread */

src/modules/fixedwing_backside/module.mk

@@ -38,4 +38,5 @@
 MODULE_COMMAND	= fixedwing_backside
 
 SRCS		= fixedwing_backside_main.cpp \
+		  fixedwing.cpp \
 		  params.c

src/modules/mathlib/math/filter/LowPassFilter2p.cpp

@@ -0,0 +1,77 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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
+ * 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	LowPassFilter.cpp
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+#include "LowPassFilter2p.hpp"
+#include "math.h"
+
+namespace math
+{
+
+void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+    _cutoff_freq = cutoff_freq;
+    float fr = sample_freq/_cutoff_freq;
+    float ohm = tanf(M_PI_F/fr);
+    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
+    _b0 = ohm*ohm/c;
+    _b1 = 2.0f*_b0;
+    _b2 = _b0;
+    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
+    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
+}
+
+float LowPassFilter2p::apply(float sample)
+{
+    // do the filtering
+    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+    if (isnan(delay_element_0) || isinf(delay_element_0)) {
+        // don't allow bad values to propogate via the filter
+        delay_element_0 = sample;
+    }
+    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+    
+    _delay_element_2 = _delay_element_1;
+    _delay_element_1 = delay_element_0;
+
+    // return the value.  Should be no need to check limits
+    return output;
+}
+
+} // namespace math
+

src/modules/mathlib/math/filter/LowPassFilter2p.hpp

@@ -0,0 +1,78 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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
+ * 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	LowPassFilter.h
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+/// Adapted for PX4 by Andrew Tridgell
+
+#pragma once
+
+namespace math
+{
+class __EXPORT LowPassFilter2p
+{
+public:
+    // constructor
+    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+        // set initial parameters
+        set_cutoff_frequency(sample_freq, cutoff_freq);
+        _delay_element_1 = _delay_element_2 = 0;
+    }
+
+    // change parameters
+    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
+
+    // apply - Add a new raw value to the filter 
+    // and retrieve the filtered result
+    float apply(float sample);
+
+    // return the cutoff frequency
+    float get_cutoff_freq(void) const {
+        return _cutoff_freq;
+    }
+
+private:
+    float           _cutoff_freq; 
+    float           _a1;
+    float           _a2;
+    float           _b0;
+    float           _b1;
+    float           _b2;
+    float           _delay_element_1;        // buffered sample -1
+    float           _delay_element_2;        // buffered sample -2
+};
+
+} // namespace math

src/modules/mathlib/math/filter/module.mk

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# filter library
+#
+SRCS		 = LowPassFilter2p.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
+-include $(TOPDIR)/.config

src/modules/px4iofirmware/controls.c

@@ -300,6 +300,8 @@ controls_tick() {
 		} else {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		}
+	} else {
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 	}
 }
 

src/modules/px4iofirmware/dsm.c

@@ -125,9 +125,9 @@ dsm_bind(uint16_t cmd, int pulses)
 	case dsm_bind_send_pulses:
 		for (int i = 0; i < pulses; i++) {
 			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(50);
+			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(50);
+			up_udelay(25);
 		}
 		break;
 	case dsm_bind_reinit_uart:

src/modules/sdlog2/sdlog2.c

@@ -606,6 +606,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
+	const char *converter_in = "/etc/logging/conv.zip";
+	char* converter_out = malloc(150);
+	sprintf(converter_out, "%s/conv.zip", folder_path);
+
+	if (file_copy(converter_in, converter_out)) {
+		errx(1, "unable to copy conversion scripts, exiting.");
+	}
+	free(converter_out);
+
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -1318,7 +1327,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return ret;
+	return OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)

src/modules/segway/BlockSegwayController.cpp

@@ -0,0 +1,57 @@
+#include "BlockSegwayController.hpp"
+
+void BlockSegwayController::update() {
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// only update guidance in auto mode
+	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+	}
+
+	// compute speed command
+	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_AUTO ||
+	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+		_actuators.control[0] = spdCmd;
+		_actuators.control[1] = spdCmd;
+
+	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+			_actuators.control[CH_LEFT] = _manual.throttle;
+			_actuators.control[CH_RIGHT] = _manual.pitch;
+
+		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+			_actuators.control[0] = spdCmd;
+			_actuators.control[1] = spdCmd;
+		}
+	}
+
+	// update all publications
+	updatePublications();
+
+}
+

src/modules/segway/BlockSegwayController.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <controllib/uorb/blocks.hpp>
+
+using namespace control;
+
+class BlockSegwayController : public control::BlockUorbEnabledAutopilot {
+public:
+	BlockSegwayController() :
+		BlockUorbEnabledAutopilot(NULL,"SEG"),
+		th2v(this, "TH2V"),
+		q2v(this, "Q2V"),
+		_attPoll(),
+		_timeStamp(0)
+	{
+		_attPoll.fd = _att.getHandle();
+		_attPoll.events = POLLIN;
+	}
+	void update();
+private:
+	enum {CH_LEFT, CH_RIGHT};
+	BlockPI th2v;
+	BlockP q2v;
+	struct pollfd _attPoll;
+	uint64_t _timeStamp;
+};
+

src/modules/segway/module.mk

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# segway controller
+#
+
+MODULE_COMMAND	= segway
+
+SRCS		= segway_main.cpp \
+		  BlockSegwayController.cpp \
+		  params.c

src/modules/segway/params.c

@@ -0,0 +1,8 @@
+#include <systemlib/param/param.h>
+
+// 16 is max name length
+PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter
+PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage
+

src/modules/segway/segway_main.cpp

@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: James Goppert
+ *
+ * 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 segway_main.cpp
+ * @author James Goppert
+ *
+ * Segway controller using control library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+
+#include "BlockSegwayController.hpp"
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int deamon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int segway_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int segway_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: segway {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int segway_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+
+		deamon_task = task_spawn_cmd("segway",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 5120,
+					 segway_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int segway_thread_main(int argc, char *argv[])
+{
+
+	warnx("starting");
+
+	using namespace control;
+
+	BlockSegwayController autopilot;
+
+	thread_running = true;
+
+	while (!thread_should_exit) {
+		autopilot.update();
+	}
+
+	warnx("exiting.");
+
+	thread_running = false;
+
+	return 0;
+}

src/modules/sensors/sensor_params.c

@@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
@@ -156,6 +156,7 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
+PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
 
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));

src/modules/sensors/sensors.cpp

@@ -60,6 +60,7 @@
 #include <drivers/drv_baro.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_adc.h>
+#include <drivers/drv_airspeed.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -91,8 +92,35 @@
 #define BARO_HEALTH_COUNTER_LIMIT_OK 5
 #define ADC_HEALTH_COUNTER_LIMIT_OK  5
 
-#define ADC_BATTERY_VOLTAGE_CHANNEL	10
-#define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+/**
+ * Analog layout:
+ * FMU:
+ * IN2 - battery voltage
+ * IN3 - battery current
+ * IN4 - 5V sense
+ * IN10 - spare (we could actually trim these from the set)
+ * IN11 - spare (we could actually trim these from the set)
+ * IN12 - spare (we could actually trim these from the set)
+ * IN13 - aux1
+ * IN14 - aux2
+ * IN15 - pressure sensor
+ * 
+ * IO:
+ * IN4 - servo supply rail
+ * IN5 - analog RSSI
+ */
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+  #define ADC_BATTERY_VOLTAGE_CHANNEL	10
+  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+  #define ADC_BATTERY_VOLTAGE_CHANNEL	2
+  #define ADC_BATTERY_CURRENT_CHANNEL	3
+  #define ADC_5V_RAIL_SENSE		4
+  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+#endif
 
 #define BAT_VOL_INITIAL 0.f
 #define BAT_VOL_LOWPASS_1 0.99f
@@ -197,7 +225,7 @@ private:
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
-		int diff_pres_offset_pa;
+		float diff_pres_offset_pa;
 
 		int rc_type;
 
@@ -228,7 +256,6 @@ private:
 
 		float battery_voltage_scaling;
 
-		int   rc_rl1_DSM_VCC_control;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -277,7 +304,6 @@ private:
 
 		param_t battery_voltage_scaling;
 
-		param_t rc_rl1_DSM_VCC_control;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -389,7 +415,7 @@ namespace sensors
 #endif
 static const int ERROR = -1;
 
-Sensors	*g_sensors;
+Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
@@ -512,9 +538,6 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
-	/* DSM VCC relay control */
-	_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -552,25 +575,11 @@ Sensors::parameters_update()
 	/* rc values */
 	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
 
-		if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
-			warnx("Failed getting min for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
-			warnx("Failed getting trim for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
-			warnx("Failed getting max for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
-			warnx("Failed getting rev for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
-			warnx("Failed getting dead zone for chan %d", i);
-		}
+		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
+		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
+		param_get(_parameter_handles.max[i], &(_parameters.max[i]));
+		param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
+		param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
 
 		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 
@@ -656,21 +665,10 @@ Sensors::parameters_update()
 		warnx("Failed getting mode aux 5 index");
 	}
 
-	if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
-		warnx("Failed getting rc scaling for roll");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
-		warnx("Failed getting rc scaling for pitch");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
-		warnx("Failed getting rc scaling for yaw");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
-		warnx("Failed getting rc scaling for flaps");
-	}
+	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
+	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
+	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
+	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -726,11 +724,6 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
-	/* relay 1 DSM VCC control */
-	if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
-		warnx("Failed updating relay 1 DSM VCC control");
-	}
-
 	return OK;
 }
 
@@ -746,11 +739,26 @@ Sensors::accel_init()
 		errx(1, "FATAL: no accelerometer found");
 
 	} else {
-		/* set the accel internal sampling rate up to at leat 500Hz */
-		ioctl(fd, ACCELIOCSSAMPLERATE, 500);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		// XXX do the check more elegantly
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+		/* set the accel internal sampling rate up to at leat 1000Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
+
+		/* set the driver to poll at 1000Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
+
+		#else
+
+		/* set the accel internal sampling rate up to at leat 800Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 800Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#endif
 
 		warnx("using system accel");
 		close(fd);
@@ -769,11 +777,28 @@ Sensors::gyro_init()
 		errx(1, "FATAL: no gyro found");
 
 	} else {
-		/* set the gyro internal sampling rate up to at leat 500Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 500);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		// XXX do the check more elegantly
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+		/* set the gyro internal sampling rate up to at least 1000Hz */
+		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
+			ioctl(fd, GYROIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 1000Hz */
+		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
+			ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#else
+
+		/* set the gyro internal sampling rate up to at leat 800Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 800Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#endif
 
 		warnx("using system gyro");
 		close(fd);
@@ -1034,6 +1059,20 @@ Sensors::parameter_update_poll(bool forced)
 
 		close(fd);
 
+		fd = open(AIRSPEED_DEVICE_PATH, 0);
+
+		/* this sensor is optional, abort without error */
+
+		if (fd > 0) {
+			struct airspeed_scale airscale = {
+				_parameters.diff_pres_offset_pa,
+				1.0f,
+			};
+
+			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
+				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+		}
+
 #if 0
 		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
 		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
@@ -1364,6 +1403,9 @@ Sensors::task_main()
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
 
+	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
+	orb_set_interval(_gyro_sub, 4);
+
 	/*
 	 * do advertisements
 	 */
@@ -1399,7 +1441,7 @@ Sensors::task_main()
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 500ms for data */
+		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */

src/modules/systemlib/hx_stream.c

@@ -53,14 +53,26 @@
 
 
 struct hx_stream {
-	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
-	unsigned		frame_bytes;
-	bool			escaped;
-	bool			txerror;
+	/* RX state */
+	uint8_t			rx_buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		rx_frame_bytes;
+	bool			rx_escaped;
+	hx_stream_rx_callback	rx_callback;
+	void			*rx_callback_arg;
 
+	/* TX state */
 	int			fd;
-	hx_stream_rx_callback	callback;
-	void			*callback_arg;
+	bool			tx_error;
+	uint8_t			*tx_buf;
+	unsigned		tx_resid;
+	uint32_t		tx_crc;
+	enum {
+		TX_IDLE = 0,
+		TX_SEND_START,
+		TX_SEND_DATA,
+		TX_SENT_ESCAPE,
+		TX_SEND_END
+	}			tx_state;
 
 	perf_counter_t		pc_tx_frames;
 	perf_counter_t		pc_rx_frames;
@@ -81,21 +93,7 @@ static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
 {
 	if (write(stream->fd, &c, 1) != 1)
-		stream->txerror = true;
-}
-
-static void
-hx_tx_byte(hx_stream_t stream, uint8_t c)
-{
-	switch (c) {
-	case FBO:
-	case CEO:
-		hx_tx_raw(stream, CEO);
-		c ^= 0x20;
-		break;
-	}
-
-	hx_tx_raw(stream, c);
+		stream->tx_error = true;
 }
 
 static int
@@ -105,11 +103,11 @@ hx_rx_frame(hx_stream_t stream)
 		uint8_t	b[4];
 		uint32_t w;
 	} u;
-	unsigned length = stream->frame_bytes;
+	unsigned length = stream->rx_frame_bytes;
 
 	/* reset the stream */
-	stream->frame_bytes = 0;
-	stream->escaped = false;
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
 
 	/* not a real frame - too short */
 	if (length < 4) {
@@ -122,11 +120,11 @@ hx_rx_frame(hx_stream_t stream)
 	length -= 4;
 
 	/* compute expected CRC */
-	u.w = crc32(&stream->buf[0], length);
+	u.w = crc32(&stream->rx_buf[0], length);
 
 	/* compare computed and actual CRC */
 	for (unsigned i = 0; i < 4; i++) {
-		if (u.b[i] != stream->buf[length + i]) {
+		if (u.b[i] != stream->rx_buf[length + i]) {
 			perf_count(stream->pc_rx_errors);
 			return 0;
 		}
@@ -134,7 +132,7 @@ hx_rx_frame(hx_stream_t stream)
 
 	/* frame is good */
 	perf_count(stream->pc_rx_frames);
-	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
 	return 1;
 }
 
@@ -150,8 +148,8 @@ hx_stream_init(int fd,
 	if (stream != NULL) {
 		memset(stream, 0, sizeof(struct hx_stream));
 		stream->fd = fd;
-		stream->callback = callback;
-		stream->callback_arg = arg;
+		stream->rx_callback = callback;
+		stream->rx_callback_arg = arg;
 	}
 
 	return stream;
@@ -179,49 +177,112 @@ hx_stream_set_counters(hx_stream_t stream,
 	stream->pc_rx_errors = rx_errors;
 }
 
+void
+hx_stream_reset(hx_stream_t stream)
+{
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
+
+	stream->tx_buf = NULL;
+	stream->tx_resid = 0;
+	stream->tx_state = TX_IDLE;
+}
+
+int
+hx_stream_start(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	if (count > HX_STREAM_MAX_FRAME)
+		return -EINVAL;
+
+	stream->tx_buf = data;
+	stream->tx_resid = count;
+	stream->tx_state = TX_SEND_START;
+	stream->tx_crc = crc32(data, count);
+	return OK;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
+
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
+
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
+
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
+
+		switch (c) {
+		case FBO:
+		case CEO:
+			stream->tx_state = TX_SENT_ESCAPE;
+			return CEO;
+		}
+		break;
+
+	case TX_SENT_ESCAPE:
+		c = *stream->tx_buf ^ 0x20;
+		stream->tx_state = TX_SEND_DATA;
+		break;
+
+	case TX_SEND_END:
+		stream->tx_state = TX_IDLE;
+		return FBO;
+
+	case TX_IDLE:
+	default:
+		return -1;
+	}
+
+	/* if we are here, we have consumed a byte from the buffer */
+	stream->tx_resid--;
+	stream->tx_buf++;
+
+	/* buffer exhausted */
+	if (stream->tx_resid == 0) {
+		uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+		/* was the buffer the frame CRC? */
+		if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+			stream->tx_state = TX_SEND_END;
+		} else {
+			/* no, it was the payload - switch to sending the CRC */
+			stream->tx_buf = pcrc;
+			stream->tx_resid = sizeof(stream->tx_crc);
+		}
+	}
+	return c;
+}
+
 int
 hx_stream_send(hx_stream_t stream,
 	       const void *data,
 	       size_t count)
 {
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} u;
-	const uint8_t *p = (const uint8_t *)data;
-	unsigned resid = count;
+	int result;
 
-	if (resid > HX_STREAM_MAX_FRAME)
-		return -EINVAL;
+	result = hx_stream_start(stream, data, count);
+	if (result != OK)
+		return result;
 
-	/* start the frame */
-	hx_tx_raw(stream, FBO);
-
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
-
-	/* compute the CRC */
-	u.w = crc32(data, count);
-
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
-
-	while (resid--)
-		hx_tx_byte(stream, *p++);
-
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+	int c;
+	while ((c = hx_stream_send_next(stream)) >= 0)
+		hx_tx_raw(stream, c);
 
 	/* check for transmit error */
-	if (stream->txerror) {
-		stream->txerror = false;
+	if (stream->tx_error) {
+		stream->tx_error = false;
 		return -EIO;
 	}
 
 	perf_count(stream->pc_tx_frames);
-	return 0;
+	return OK;
 }
 
 void
@@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
 	}
 
 	/* escaped? */
-	if (stream->escaped) {
-		stream->escaped = false;
+	if (stream->rx_escaped) {
+		stream->rx_escaped = false;
 		c ^= 0x20;
 
 	} else if (c == CEO) {
-		/* now escaped, ignore the byte */
-		stream->escaped = true;
+		/* now rx_escaped, ignore the byte */
+		stream->rx_escaped = true;
 		return;
 	}
 
 	/* save for later */
-	if (stream->frame_bytes < sizeof(stream->buf))
-		stream->buf[stream->frame_bytes++] = c;
+	if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+		stream->rx_buf[stream->rx_frame_bytes++] = c;
 }

src/modules/systemlib/hx_stream.h

@@ -58,7 +58,8 @@ __BEGIN_DECLS
  * Allocate a new hx_stream object.
  *
  * @param fd		The file handle over which the protocol will
- *			communicate.
+ *			communicate, or -1 if the protocol will use
+ *			hx_stream_start/hx_stream_send_next.
  * @param callback	Called when a frame is received.
  * @param callback_arg	Passed to the callback.
  * @return		A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void		hx_stream_free(hx_stream_t stream);
  *
  * Any counter may be set to NULL to disable counting that datum.
  *
+ * @param stream	A handle returned from hx_stream_init.
  * @param tx_frames	Counter for transmitted frames.
  * @param rx_frames	Counter for received frames.
  * @param rx_errors	Counter for short and corrupt received frames.
@@ -89,6 +91,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_frames,
 		perf_counter_t rx_errors);
 
+/**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to 
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_start(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		The byte to send, or -1 if there is 
+ *			nothing left to send.
+ */
+__EXPORT extern int		hx_stream_send_next(hx_stream_t stream);
+
 /**
  * Send a frame.
  *

src/modules/systemlib/perf_counter.c

@@ -201,23 +201,50 @@ perf_end(perf_counter_t handle)
 	switch (handle->type) {
 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
-			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			pce->event_count++;
-			pce->time_total += elapsed;
+			if (pce->time_start != 0) {
+				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			if ((pce->time_least > elapsed) || (pce->time_least == 0))
-				pce->time_least = elapsed;
+				pce->event_count++;
+				pce->time_total += elapsed;
 
-			if (pce->time_most < elapsed)
-				pce->time_most = elapsed;
+				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+					pce->time_least = elapsed;
+
+				if (pce->time_most < elapsed)
+					pce->time_most = elapsed;
+
+				pce->time_start = 0;
+			}
 		}
+		break;
 
 	default:
 		break;
 	}
 }
 
+void
+perf_cancel(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			pce->time_start = 0;
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+
 void
 perf_reset(perf_counter_t handle)
 {

src/modules/systemlib/perf_counter.h

@@ -92,13 +92,25 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
  */
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
 /**
- * Reset a performance event.
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_cancel(perf_counter_t handle);
+
+/**
+ * Reset a performance counter.
  *
  * This call resets performance counter to initial state
  *