Merge branch 'master' into navigator_rewrite_offboard2_merge

Conflicts:
	mavlink/include/mavlink/v1.0/common/common.h
	src/modules/mavlink/mavlink_receiver.cpp

ROMFS/px4fmu_common/init.d/rc.usb

@@ -3,7 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 10000 -d /dev/ttyACM0
+mavlink start -r 10000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
 usleep 100000

ROMFS/px4fmu_common/init.d/rcS

@@ -428,11 +428,10 @@ then
 	#
 	sh /etc/init.d/rc.sensors
 
-	if [ $HIL == no ]
+	#
-	then
+	# Start logging in all modes, including HIL
-		echo "[init] Start logging"
+	#
-		sh /etc/init.d/rc.logging
+	sh /etc/init.d/rc.logging
-	fi
 
 	if [ $GPS == yes ]
 	then

makefiles/config_aerocore_default.mk

@@ -80,8 +80,8 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 

makefiles/config_px4fmu-v1_default.mk

@@ -107,8 +107,8 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 

makefiles/config_px4fmu-v2_default.mk

@@ -120,8 +120,8 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 

mavlink/include/mavlink/v1.0/common/common.h

@@ -6,7 +6,7 @@
 #define COMMON_H
 
 #ifndef MAVLINK_H
-    #error Wrong include order: COMMON.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call.
+ #error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually.
 #endif
 
 #ifdef __cplusplus

src/drivers/mkblctrl/mkblctrl.cpp

@@ -131,8 +131,8 @@ public:
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
-	int		set_px4mode(int px4mode);
+	void		set_px4mode(int px4mode);
-	int		set_frametype(int frametype);
+	void		set_frametype(int frametype);
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
@@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate)
 	return OK;
 }
 
-int
+void
 MK::set_px4mode(int px4mode)
 {
 	_px4mode = px4mode;
 }
 
-int
+void
 MK::set_frametype(int frametype)
 {
 	_frametype = frametype;

src/drivers/mpu6000/mpu6000.cpp

@@ -544,7 +544,7 @@ void MPU6000::reset()
 	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
         irqrestore(state);
 
-	up_udelay(1000);
+	usleep(1000);
 
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);

src/drivers/px4fmu/fmu.cpp

@@ -741,7 +741,7 @@ PX4FMU::task_main()
 	}
 
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
-		if (_control_subs > 0) {
+		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
 		}

src/drivers/roboclaw/RoboClaw.cpp

@@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor)
 		return _motor1Position;
 	} else if (motor == MOTOR_2) {
 		return _motor2Position;
-	}
+	} else {
+		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+		return NAN;
+    }
 }
 
 float RoboClaw::getMotorSpeed(e_motor motor)
@@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor)
 		return _motor1Speed;
 	} else if (motor == MOTOR_2) {
 		return _motor2Speed;
-	}
+	} else {
+		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+		return NAN;
+    }
 }
 
 int RoboClaw::setMotorSpeed(e_motor motor, float value)

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0; //xxx: param
-
 	/* input conditioning */
 //	warnx("airspeed pre %.4f", (double)airspeed);
 	if (!isfinite(airspeed)) {

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
 	_rate_setpoint = 0.0f;
 	if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
 		float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
-		if(denumerator != 0.0f) { //XXX: floating point comparison
+		if(fabsf(denumerator) > FLT_EPSILON) {
 			_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
 //			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
 		}
@@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 	/* airspeed is NaN, +- INF or not available, pick center of band */

src/lib/geo/geo.h

@@ -50,7 +50,7 @@
 
 __BEGIN_DECLS
 
-#include "geo_mag_declination.h"
+#include "geo_lookup/geo_mag_declination.h"
 
 #include <stdbool.h>
 

src/lib/geo/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2014 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
@@ -35,5 +35,4 @@
 # Geo library
 #
 
-SRCS		=	geo.c \
+SRCS		=	geo.c
-			geo_mag_declination.c

src/lib/geo_lookup/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 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.
+#
+############################################################################
+
+#
+# Geo lookup table / data library
+#
+
+SRCS		 =	geo_mag_declination.c
+
+MAXOPTIMIZATION	 = -Os

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
 /* offset estimation - UNUSED */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
-/* offsets in roll, pitch and yaw of sensor plane and body */
-PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
-
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
@@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
 	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
-	h->roll_off  =	param_find("ATT_ROLL_OFF3");
-	h->pitch_off =	param_find("ATT_PITCH_OFF3");
-	h->yaw_off   =	param_find("ATT_YAW_OFF3");
-
 	h->mag_decl   =	param_find("ATT_MAG_DECL");
 
 	h->acc_comp   =	param_find("ATT_ACC_COMP");
@@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r2, &(p->r[2]));
 	param_get(h->r3, &(p->r[3]));
 
-	param_get(h->roll_off, &(p->roll_off));
-	param_get(h->pitch_off, &(p->pitch_off));
-	param_get(h->yaw_off, &(p->yaw_off));
-
 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI / 180.0f;
 

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h

@@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params {
 struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2, r3;
 	param_t q0, q1, q2, q3, q4;
-	param_t roll_off, pitch_off, yaw_off;
 	param_t mag_decl;
 	param_t acc_comp;
 };

src/modules/commander/commander.cpp

@@ -609,6 +609,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		// XXX TODO
 	}
 
+	return true;
 }
 
 int commander_thread_main(int argc, char *argv[])
@@ -1622,7 +1623,6 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 	switch (sp_man->mode_switch) {
 	case SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
-		warnx("NONE");
 		break;
 
 	case SWITCH_POS_OFF:		// MANUAL

src/modules/ekf_att_pos_estimator/estimator_21states.h

@@ -0,0 +1,247 @@
+#pragma once
+
+#include "estimator_utilities.h"
+
+class AttPosEKF {
+
+public:
+
+    AttPosEKF();
+    ~AttPosEKF();
+
+    /* ##############################################
+     *
+     *   M A I N    F I L T E R    P A R A M E T E R S
+     *
+     * ########################################### */
+
+    /*
+     * parameters are defined here and initialised in
+     * the InitialiseParameters() (which is just 20 lines down)
+     */
+
+    float covTimeStepMax; // maximum time allowed between covariance predictions
+    float covDelAngMax; // maximum delta angle between covariance predictions
+    float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+
+    float yawVarScale;
+    float windVelSigma;
+    float dAngBiasSigma;
+    float dVelBiasSigma;
+    float magEarthSigma;
+    float magBodySigma;
+    float gndHgtSigma;
+
+    float vneSigma;
+    float vdSigma;
+    float posNeSigma;
+    float posDSigma;
+    float magMeasurementSigma;
+    float airspeedMeasurementSigma;
+
+    float gyroProcessNoise;
+    float accelProcessNoise;
+
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    void InitialiseParameters()
+    {
+        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+        EAS2TAS = 1.0f;
+
+        yawVarScale = 1.0f;
+        windVelSigma = 0.1f;
+        dAngBiasSigma = 5.0e-7f;
+        dVelBiasSigma = 1e-4f;
+        magEarthSigma = 3.0e-4f;
+        magBodySigma  = 3.0e-4f;
+        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
+
+        vneSigma = 0.2f;
+        vdSigma = 0.3f;
+        posNeSigma = 2.0f;
+        posDSigma = 2.0f;
+
+        magMeasurementSigma = 0.05;
+        airspeedMeasurementSigma = 1.4f;
+        gyroProcessNoise = 1.4544411e-2f;
+        accelProcessNoise = 0.5f;
+    }
+
+    // Global variables
+    float KH[n_states][n_states]; //  intermediate result used for covariance updates
+    float KHP[n_states][n_states]; // intermediate result used for covariance updates
+    float P[n_states][n_states]; // covariance matrix
+    float Kfusion[n_states]; // Kalman gains
+    float states[n_states]; // state matrix
+    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
+    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+
+    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
+    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+
+    Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
+    Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+    float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
+    Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
+    Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
+    Vector3f dVelIMU;
+    Vector3f dAngIMU;
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+    float innovVelPos[6]; // innovation output
+    float varInnovVelPos[6]; // innovation variance output
+
+    float velNED[3]; // North, East, Down velocity obs (m/s)
+    float posNE[2]; // North, East position obs (m)
+    float hgtMea; //  measured height (m)
+    float posNED[3]; // North, East Down position (m)
+
+    float innovMag[3]; // innovation output
+    float varInnovMag[3]; // innovation variance output
+    Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
+    float innovVtas; // innovation output
+    float varInnovVtas; // innovation variance output
+    float VtasMeas; // true airspeed measurement (m/s)
+    float magDeclination;
+    float latRef; // WGS-84 latitude of reference point (rad)
+    float lonRef; // WGS-84 longitude of reference point (rad)
+    float hgtRef; // WGS-84 height of reference point (m)
+    Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
+    uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+
+    // GPS input data variables
+    float gpsCourse;
+    float gpsVelD;
+    float gpsLat;
+    float gpsLon;
+    float gpsHgt;
+    uint8_t GPSstatus;
+
+    // Baro input
+    float baroHgt;
+
+    bool statesInitialised;
+
+    bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+    bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+    bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+    bool fuseMagData; // boolean true when magnetometer data is to be fused
+    bool fuseVtasData; // boolean true when airspeed data is to be fused
+
+    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
+    bool staticMode;    ///< boolean true if no position feedback is fused
+    bool useAirspeed;    ///< boolean true if airspeed data is being used
+    bool useCompass;    ///< boolean true if magnetometer data is being used
+
+    struct ekf_status_report current_ekf_state;
+    struct ekf_status_report last_ekf_error;
+
+    bool numericalProtection;
+
+    unsigned storeIndex;
+
+
+void  UpdateStrapdownEquationsNED();
+
+void CovariancePrediction(float dt);
+
+void FuseVelposNED();
+
+void FuseMagnetometer();
+
+void FuseAirspeed();
+
+void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+
+// store staes along with system time stamp in msces
+void StoreStates(uint64_t timestamp_ms);
+
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ *         Does only copy valid states, if the statesForFusion vector was initialized
+ *         correctly by the caller, the result can be safely used, but is a mixture
+ *         time-wise where valid states were updated and invalid remained at the old
+ *         value.
+ */
+int RecallStates(float statesForFusion[n_states], uint64_t msec);
+
+void ResetStoredStates();
+
+void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
+
+void calcEarthRateNED(Vector3f &omega, float latitude);
+
+static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+
+static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+
+static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+
+static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+
+static float sq(float valIn);
+
+void OnGroundCheck();
+
+void CovarianceInit();
+
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
+
+float ConstrainFloat(float val, float min, float max);
+
+void ConstrainVariances();
+
+void ConstrainStates();
+
+void ForceSymmetry();
+
+int CheckAndBound();
+
+void ResetPosition();
+
+void ResetVelocity();
+
+void ZeroVariables();
+
+void GetFilterState(struct ekf_status_report *state);
+
+void GetLastErrorState(struct ekf_status_report *last_error);
+
+bool StatesNaN(struct ekf_status_report *err_report);
+void FillErrorReport(struct ekf_status_report *err);
+
+void InitializeDynamic(float (&initvelNED)[3], float declination);
+
+protected:
+
+bool FilterHealthy();
+
+void ResetHeight(void);
+
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
+
+};
+
+uint32_t millis();
+

src/modules/ekf_att_pos_estimator/estimator_23states.cpp

@@ -1,143 +1,9 @@
-#include "estimator.h"
+#include "estimator_23states.h"
 #include <string.h>
+#include <stdio.h>
 #include <stdarg.h>
 
-// Define EKF_DEBUG here to enable the debug print calls
+#define EKF_COVARIANCE_DIVERGED 1.0e8f
-// if the macro is not set, these will be completely
-// optimized out by the compiler.
-//#define EKF_DEBUG
-
-#ifdef EKF_DEBUG
-#include <stdio.h>
-
-static void
-ekf_debug_print(const char *fmt, va_list args)
-{
-    fprintf(stderr, "%s: ", "[ekf]");
-    vfprintf(stderr, fmt, args);
-
-    fprintf(stderr, "\n");
-}
-
-static void
-ekf_debug(const char *fmt, ...)
-{
-    va_list args;
-
-    va_start(args, fmt);
-    ekf_debug_print(fmt, args);
-}
-
-#else
-
-static void ekf_debug(const char *fmt, ...) { while(0){} }
-#endif
-
-float Vector3f::length(void) const
-{
-    return sqrt(x*x + y*y + z*z);
-}
-
-void Vector3f::zero(void)
-{
-    x = 0.0f;
-    y = 0.0f;
-    z = 0.0f;
-}
-
-Mat3f::Mat3f() {
-    identity();
-}
-
-void Mat3f::identity() {
-    x.x = 1.0f;
-    x.y = 0.0f;
-    x.z = 0.0f;
-
-    y.x = 0.0f;
-    y.y = 1.0f;
-    y.z = 0.0f;
-
-    z.x = 0.0f;
-    z.y = 0.0f;
-    z.z = 1.0f;
-}
-
-Mat3f Mat3f::transpose(void) const
-{
-    Mat3f ret = *this;
-    swap_var(ret.x.y, ret.y.x);
-    swap_var(ret.x.z, ret.z.x);
-    swap_var(ret.y.z, ret.z.y);
-    return ret;
-}
-
-// overload + operator to provide a vector addition
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x + vecIn2.x;
-    vecOut.y = vecIn1.y + vecIn2.y;
-    vecOut.z = vecIn1.z + vecIn2.z;
-    return vecOut;
-}
-
-// overload - operator to provide a vector subtraction
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x - vecIn2.x;
-    vecOut.y = vecIn1.y - vecIn2.y;
-    vecOut.z = vecIn1.z - vecIn2.z;
-    return vecOut;
-}
-
-// overload * operator to provide a matrix vector product
-Vector3f operator*( Mat3f matIn, Vector3f vecIn)
-{
-    Vector3f vecOut;
-    vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
-    vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
-    vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
-    return vecOut;
-}
-
-// overload % operator to provide a vector cross product
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
-    vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
-    vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
-    return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(Vector3f vecIn1, float sclIn1)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x * sclIn1;
-    vecOut.y = vecIn1.y * sclIn1;
-    vecOut.z = vecIn1.z * sclIn1;
-    return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(float sclIn1, Vector3f vecIn1)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x * sclIn1;
-    vecOut.y = vecIn1.y * sclIn1;
-    vecOut.z = vecIn1.z * sclIn1;
-    return vecOut;
-}
-
-void swap_var(float &d1, float &d2)
-{
-    float tmp = d1;
-    d1 = d2;
-    d2 = tmp;
-}
 
 AttPosEKF::AttPosEKF()
 
@@ -145,7 +11,42 @@ AttPosEKF::AttPosEKF()
      * instead to allow clean in-air re-initialization.
      */
 {
+    summedDelAng.zero();
+    summedDelVel.zero();
 
+    fusionModeGPS = 0;
+    fuseVelData = false;
+    fusePosData = false;
+    fuseHgtData = false;
+    fuseMagData = false;
+    fuseVtasData = false;
+    onGround = true;
+    staticMode = true;
+    useAirspeed = true;
+    useCompass = true;
+    useRangeFinder = true;
+    numericalProtection = true;
+    refSet = false;
+    storeIndex = 0;
+    gpsHgt = 0.0f;
+    baroHgt = 0.0f;
+    GPSstatus = 0;
+    VtasMeas = 0.0f;
+    magDeclination = 0.0f;
+    dAngIMU.zero();
+    dVelIMU.zero();
+    velNED[0] = 0.0f;
+    velNED[1] = 0.0f;
+    velNED[2] = 0.0f;
+    accelGPSNED[0] = 0.0f;
+    accelGPSNED[1] = 0.0f;
+    accelGPSNED[2] = 0.0f;
+    delAngTotal.zero();
+    ekfDiverged = false;
+    lastReset = 0;
+
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
     ZeroVariables();
     InitialiseParameters();
 }
@@ -181,6 +82,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     dVelIMU.y = dVelIMU.y;
     dVelIMU.z = dVelIMU.z - states[13];
 
+    delAngTotal.x += correctedDelAng.x;
+    delAngTotal.y += correctedDelAng.y;
+    delAngTotal.z += correctedDelAng.z;
+
 // Save current measurements
     Vector3f  prevDelAng = correctedDelAng;
 
@@ -199,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     }
     else
     {
-        deltaQuat[0] = cosf(0.5f*rotationMag);
+        // We are using double here as we are unsure how small
-        float rotScaler = (sinf(0.5f*rotationMag))/rotationMag;
+        // the angle differences are and if we get into numeric
+        // issues with float. The runtime impact is not measurable
+        // for these quantities.
+        deltaQuat[0] = cos(0.5*(double)rotationMag);
+        float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
         deltaQuat[1] = correctedDelAng.x*rotScaler;
         deltaQuat[2] = correctedDelAng.y*rotScaler;
         deltaQuat[3] = correctedDelAng.z*rotScaler;
@@ -312,7 +221,8 @@ void AttPosEKF::CovariancePrediction(float dt)
     float nextP[n_states][n_states];
 
     // calculate covariance prediction process noise
-    for (uint8_t i= 0; i<=9;  i++) processNoise[i] = 1.0e-9f;
+    for (uint8_t i= 0; i<4;  i++) processNoise[i] = 1.0e-9f;
+    for (uint8_t i= 4; i<10;  i++) processNoise[i] = 1.0e-9f;
     for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
     // scale gyro bias noise when on ground to allow for faster bias estimation
     for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
@@ -977,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt)
         // propagate
         for (unsigned i = 0; i <= 13; i++) {
             P[i][i] = nextP[i][i];
+        }
 
-            // force symmetry for observable states
+        // force symmetry for observable states
-            // force zero for non-observable states
+        // force zero for non-observable states
-            for (unsigned i = 1; i < n_states; i++)
+        for (unsigned i = 1; i < n_states; i++)
+        {
+            for (uint8_t j = 0; j < i; j++)
             {
-                for (uint8_t j = 0; j < i; j++)
+                if ((i > 13) || (j > 13)) {
-                {
+                    P[i][j] = 0.0f;
-                    if ((i > 13) || (j > 13)) {
+                } else {
-                        P[i][j] = 0.0f;
+                    P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
-                    } else {
-                        P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
-                    }
-                    P[j][i] = P[i][j];
                 }
+                P[j][i] = P[i][j];
             }
         }
 
@@ -1020,9 +930,9 @@ void AttPosEKF::FuseVelposNED()
 {
 
 // declare variables used by fault isolation logic
-    uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
+    uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
-    uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
+    uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
-    uint32_t hgtRetryTime = 5000; // height measurement retry time
+    uint32_t hgtRetryTime = 500; // height measurement retry time
     uint32_t horizRetryTime;
 
 // declare variables used to check measurement errors
@@ -1178,7 +1088,7 @@ void AttPosEKF::FuseVelposNED()
                 stateIndex = 4 + obsIndex;
                 // Calculate the measurement innovation, using states from a
                 // different time coordinate if fusing height data
-                if (obsIndex >= 0 && obsIndex <= 2)
+                if (obsIndex <= 2)
                 {
                     innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
                 }
@@ -1193,7 +1103,7 @@ void AttPosEKF::FuseVelposNED()
                 // Calculate the Kalman Gain
                 // Calculate innovation variances - also used for data logging
                 varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
-                SK = 1.0/varInnovVelPos[obsIndex];
+                SK = 1.0/(double)varInnovVelPos[obsIndex];
                 for (uint8_t i= 0; i<=indexLimit; i++)
                 {
                     Kfusion[i] = P[i][stateIndex]*SK;
@@ -1277,7 +1187,7 @@ void AttPosEKF::FuseMagnetometer()
 // data fit is the only assumption we can make
 // so we might as well take advantage of the computational efficiencies
 // associated with sequential fusion
-    if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
+    if (useCompass && fuseMagData && (obsIndex < 3))
     {
         // Limit range of states modified when on ground
         if(!onGround)
@@ -1293,7 +1203,7 @@ void AttPosEKF::FuseMagnetometer()
         // three prediction time steps.
 
         // Calculate observation jacobians and Kalman gains
-        if (fuseMagData)
+        if (obsIndex == 0)
         {
             // Copy required states to local variable names
             q0       = statesAtMagMeasTime[0];
@@ -1388,11 +1298,6 @@ void AttPosEKF::FuseMagnetometer()
             Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
             varInnovMag[0] = 1.0f/SK_MX[0];
             innovMag[0] = MagPred[0] - magData.x;
-
-            // reset the observation index to 0 (we start by fusing the X
-            // measurement)
-            obsIndex = 0;
-            fuseMagData = false;
         }
         else if (obsIndex == 1) // we are now fusing the Y measurement
         {
@@ -1508,7 +1413,7 @@ void AttPosEKF::FuseMagnetometer()
         }
 
         // Check the innovation for consistency and don't fuse if > 5Sigma
-        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
+        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
         {
             // correct the state vector
             for (uint8_t j= 0; j < indexLimit; j++)
@@ -1517,7 +1422,7 @@ void AttPosEKF::FuseMagnetometer()
             }
             // normalise the quaternion states
             float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-            if (quatMag > 1e-12)
+            if (quatMag > 1e-12f)
             {
                 for (uint8_t j= 0; j<=3; j++)
                 {
@@ -1612,7 +1517,7 @@ void AttPosEKF::FuseAirspeed()
         SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
         SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
         SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
-        
+
         float H_TAS[n_states];
         for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f;
         H_TAS[4] = SH_TAS[2];
@@ -1661,7 +1566,7 @@ void AttPosEKF::FuseAirspeed()
         // Calculate the measurement innovation
         innovVtas = VtasPred - VtasMeas;
         // Check the innovation for consistency and don't fuse if > 5Sigma
-        if ((innovVtas*innovVtas*SK_TAS) < 25.0)
+        if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
         {
             // correct the state vector
             for (uint8_t j=0; j <= 22; j++)
@@ -1758,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder()
 
     // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
     SH_RNG[4] = sin(rngFinderPitch);
-    cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch);
+    cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
     if (useRangeFinder && cosRngTilt > 0.87f)
     {
         // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
@@ -1855,21 +1760,21 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
 
     int64_t bestTimeDelta = 200;
     unsigned bestStoreIndex = 0;
-    for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
     {
         // Work around a GCC compiler bug - we know 64bit support on ARM is
         // sketchy in GCC.
         uint64_t timeDelta;
 
-        if (msec > statetimeStamp[storeIndex]) {
+        if (msec > statetimeStamp[storeIndexLocal]) {
-            timeDelta = msec - statetimeStamp[storeIndex];
+            timeDelta = msec - statetimeStamp[storeIndexLocal];
         } else {
-            timeDelta = statetimeStamp[storeIndex] - msec;
+            timeDelta = statetimeStamp[storeIndexLocal] - msec;
         }
 
-        if (timeDelta < bestTimeDelta)
+        if (timeDelta < (uint64_t)bestTimeDelta)
         {
-            bestStoreIndex = storeIndex;
+            bestStoreIndex = storeIndexLocal;
             bestTimeDelta = timeDelta;
         }
     }
@@ -1926,7 +1831,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
     Tnb.y.z = 2*(q23 + q01);
 }
 
-void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
+void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4])
 {
     // Calculate the body to nav cosine matrix
     float q00 = sq(quat[0]);
@@ -1940,15 +1845,15 @@ void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
     float q13 =  quat[1]*quat[3];
     float q23 =  quat[2]*quat[3];
 
-    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn_ret.x.x = q00 + q11 - q22 - q33;
-    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn_ret.y.y = q00 - q11 + q22 - q33;
-    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn_ret.z.z = q00 - q11 - q22 + q33;
-    Tbn.x.y = 2*(q12 - q03);
+    Tbn_ret.x.y = 2*(q12 - q03);
-    Tbn.x.z = 2*(q13 + q02);
+    Tbn_ret.x.z = 2*(q13 + q02);
-    Tbn.y.x = 2*(q12 + q03);
+    Tbn_ret.y.x = 2*(q12 + q03);
-    Tbn.y.z = 2*(q23 - q01);
+    Tbn_ret.y.z = 2*(q23 - q01);
-    Tbn.z.x = 2*(q13 - q02);
+    Tbn_ret.z.x = 2*(q13 - q02);
-    Tbn.z.y = 2*(q23 + q01);
+    Tbn_ret.z.y = 2*(q23 + q01);
 }
 
 void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
@@ -1979,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd,
     velNED[2] = gpsVelD;
 }
 
-void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef)
+void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
 {
-    posNED[0] = earthRadius * (lat - latRef);
+    posNED[0] = earthRadius * (lat - latReference);
-    posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
+    posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
-    posNED[2] = -(hgt - hgtRef);
+    posNED[2] = -(hgt - hgtReference);
 }
 
-void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
 {
-    lat = latRef + posNED[0] * earthRadiusInv;
+    lat = latRef + (double)posNED[0] * earthRadiusInv;
-    lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
+    lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
     hgt = hgtRef - posNED[2];
 }
 
@@ -2194,10 +2099,71 @@ void AttPosEKF::ForceSymmetry()
         {
             P[i][j] = 0.5f * (P[i][j] + P[j][i]);
             P[j][i] = P[i][j];
+
+            if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) ||
+                (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) {
+                current_ekf_state.covariancesExcessive = true;
+                current_ekf_state.error |= true;
+                InitializeDynamic(velNED, magDeclination);
+                return;
+            }
+
+            float symmetric = 0.5f * (P[i][j] + P[j][i]);
+            P[i][j] = symmetric;
+            P[j][i] = symmetric;
         }
     }
 }
 
+bool AttPosEKF::GyroOffsetsDiverged()
+{
+    // Detect divergence by looking for rapid changes of the gyro offset
+    Vector3f current_bias;
+    current_bias.x = states[10];
+    current_bias.y = states[11];
+    current_bias.z = states[12];
+
+    Vector3f delta = current_bias - lastGyroOffset;
+    float delta_len = delta.length();
+    float delta_len_scaled = 0.0f;
+
+    // Protect against division by zero
+    if (delta_len > 0.0f) {
+        float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
+        delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
+    }
+
+    bool diverged = (delta_len_scaled > 1.0f);
+    lastGyroOffset = current_bias;
+    current_ekf_state.error |= diverged;
+    current_ekf_state.gyroOffsetsExcessive = diverged;
+
+    return diverged;
+}
+
+bool AttPosEKF::VelNEDDiverged()
+{
+    Vector3f current_vel;
+    current_vel.x = states[4];
+    current_vel.y = states[5];
+    current_vel.z = states[6];
+
+    Vector3f gps_vel;
+    gps_vel.x = velNED[0];
+    gps_vel.y = velNED[1];
+    gps_vel.z = velNED[2];
+
+    Vector3f delta = current_vel - gps_vel;
+    float delta_len = delta.length();
+
+    bool excessive = (delta_len > 20.0f);
+
+    current_ekf_state.error |= excessive;
+    current_ekf_state.velOffsetExcessive = excessive;
+
+    return excessive;
+}
+
 bool AttPosEKF::FilterHealthy()
 {
     if (!statesInitialised) {
@@ -2262,42 +2228,26 @@ void AttPosEKF::ResetVelocity(void)
     }
 }
 
-
+bool AttPosEKF::StatesNaN() {
-void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
-{
-    for (unsigned i = 0; i < n_states; i++)
-    {
-        err->states[i] = states[i];
-    }
-
-    err->velHealth = current_ekf_state.velHealth;
-    err->posHealth = current_ekf_state.posHealth;
-    err->hgtHealth = current_ekf_state.hgtHealth;
-    err->velTimeout = current_ekf_state.velTimeout;
-    err->posTimeout = current_ekf_state.posTimeout;
-    err->hgtTimeout = current_ekf_state.hgtTimeout;
-}
-
-bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
     bool err = false;
 
     // check all integrators
     if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
-        err_report->statesNaN = true;
+        current_ekf_state.angNaN = true;
         ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
         err = true;
         goto out;
     } // delta angles
 
     if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
-        err_report->statesNaN = true;
+        current_ekf_state.angNaN = true;
         ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
         err = true;
         goto out;
     } // delta angles
 
     if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
-        err_report->statesNaN = true;
+        current_ekf_state.summedDelVelNaN = true;
         ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
         err = true;
         goto out;
@@ -2308,7 +2258,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
         for (unsigned j = 0; j < n_states; j++) {
             if (!isfinite(KH[i][j])) {
 
-                err_report->covarianceNaN = true;
+                current_ekf_state.KHNaN = true;
                 err = true;
                 ekf_debug("KH NaN");
                 goto out;
@@ -2316,7 +2266,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
             if (!isfinite(KHP[i][j])) {
 
-                err_report->covarianceNaN = true;
+                current_ekf_state.KHPNaN = true;
                 err = true;
                 ekf_debug("KHP NaN");
                 goto out;
@@ -2324,7 +2274,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
             if (!isfinite(P[i][j])) {
 
-                err_report->covarianceNaN = true;
+                current_ekf_state.covarianceNaN = true;
                 err = true;
                 ekf_debug("P NaN");
             } // covariance matrix
@@ -2332,7 +2282,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
         if (!isfinite(Kfusion[i])) {
 
-            err_report->kalmanGainsNaN = true;
+            current_ekf_state.kalmanGainsNaN = true;
             ekf_debug("Kfusion NaN");
             err = true;
             goto out;
@@ -2340,7 +2290,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
         if (!isfinite(states[i])) {
 
-            err_report->statesNaN = true;
+            current_ekf_state.statesNaN = true;
             ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
             err = true;
             goto out;
@@ -2349,7 +2299,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
 out:
     if (err) {
-        FillErrorReport(err_report);
+        current_ekf_state.error |= true;
     }
 
     return err;
@@ -2365,47 +2315,105 @@ out:
  * updated, but before any of the fusion steps are
  * executed.
  */
-int AttPosEKF::CheckAndBound()
+int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
 {
 
     // Store the old filter state
     bool currStaticMode = staticMode;
 
+    // Limit reset rate to 5 Hz to allow the filter
+    // to settle
+    if (millis() - lastReset < 200) {
+        return 0;
+    }
+
+    if (ekfDiverged) {
+        ekfDiverged = false;
+    }
+
+    int ret = 0;
+
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
     // Reset the filter if the states went NaN
-    if (StatesNaN(&last_ekf_error)) {
+    if (StatesNaN()) {
         ekf_debug("re-initializing dynamic");
 
-        InitializeDynamic(velNED, magDeclination);
+        // Reset and fill error report
+	    InitializeDynamic(velNED, magDeclination);
 
-        return 1;
+        ret = 1;
     }
 
     // Reset the filter if the IMU data is too old
     if (dtIMU > 0.3f) {
 
+        current_ekf_state.imuTimeout = true;
+
+        // Fill error report
+        GetFilterState(&last_ekf_error);
+
         ResetVelocity();
         ResetPosition();
         ResetHeight();
         ResetStoredStates();
 
-        // that's all we can do here, return
+        // Timeout cleared with this reset
-        return 2;
+        current_ekf_state.imuTimeout = false;
-    }
 
-    // Check if we're on ground - this also sets static mode.
+        // that's all we can do here, return
-    OnGroundCheck();
+        ret = 2;
+    }
 
     // Check if we switched between states
     if (currStaticMode != staticMode) {
+        // Fill error report, but not setting error flag
+        GetFilterState(&last_ekf_error);
+
         ResetVelocity();
         ResetPosition();
         ResetHeight();
         ResetStoredStates();
 
-        return 3;
+        ret = 3;
     }
 
-    return 0;
+    // Reset the filter if gyro offsets are excessive
+    if (GyroOffsetsDiverged()) {
+
+        // Reset and fill error report
+        InitializeDynamic(velNED, magDeclination);
+
+        // that's all we can do here, return
+        ret = 4;
+    }
+
+    // Reset the filter if it diverges too far from GPS
+    if (VelNEDDiverged()) {
+
+        // Reset and fill error report
+        InitializeDynamic(velNED, magDeclination);
+
+        // that's all we can do here, return
+        ret = 5;
+    }
+
+    // The excessive covariance detection already
+    // reset the filter. Just need to report here.
+    if (last_ekf_error.covariancesExcessive) {
+        ret = 6;
+    }
+
+    if (ret) {
+        ekfDiverged = true;
+        lastReset = millis();
+
+        // This reads the last error and clears it
+        GetLastErrorState(last_error);
+    }
+
+    return ret;
 }
 
 void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
@@ -2456,6 +2464,30 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
 
 void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
 {
+    if (current_ekf_state.error) {
+        GetFilterState(&last_ekf_error);
+    }
+
+    ZeroVariables();
+
+    // Reset error states
+    current_ekf_state.error = false;
+    current_ekf_state.angNaN = false;
+    current_ekf_state.summedDelVelNaN = false;
+    current_ekf_state.KHNaN = false;
+    current_ekf_state.KHPNaN = false;
+    current_ekf_state.PNaN = false;
+    current_ekf_state.covarianceNaN = false;
+    current_ekf_state.kalmanGainsNaN = false;
+    current_ekf_state.statesNaN = false;
+
+    current_ekf_state.velHealth = true;
+    //current_ekf_state.posHealth = ?;
+    //current_ekf_state.hgtHealth = ?;
+    
+    current_ekf_state.velTimeout = false;
+    //current_ekf_state.posTimeout = ?;
+    //current_ekf_state.hgtTimeout = ?;
 
     // Fill variables with valid data
     velNED[0] = initvelNED[0];
@@ -2494,7 +2526,11 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
     for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
-    for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel
+    // positions:
+    states[7] = posNE[0];
+    states[8] = posNE[1];
+    states[9] = -hgtMea;
+    for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
     states[16] = initMagNED.x; // Magnetic Field North
     states[17] = initMagNED.y; // Magnetic Field East
     states[18] = initMagNED.z; // Magnetic Field Down
@@ -2525,14 +2561,16 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
     hgtRef = referenceHgt;
     refSet = true;
 
-    // we are at reference altitude, so measurement must be zero
+    // we are at reference position, so measurement must be zero
-    hgtMea = 0.0f;
+    posNE[0] = 0.0f;
+    posNE[1] = 0.0f;
+
+    // we are at an unknown, possibly non-zero altitude - so altitude
+    // is not reset (hgtMea)
 
     // the baro offset must be this difference now
     baroHgtOffset = baroHgt - referenceHgt;
 
-    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
-
     InitializeDynamic(initvelNED, declination);
 }
 
@@ -2540,27 +2578,8 @@ void AttPosEKF::ZeroVariables()
 {
 
     // Initialize on-init initialized variables
-    fusionModeGPS = 0;
+
-    covSkipCount = 0;
-    statesInitialised = false;
-    fuseVelData = false;
-    fusePosData = false;
-    fuseHgtData = false;
-    fuseMagData = false;
-    fuseVtasData = false;
-    onGround = true;
-    staticMode = true;
-    useAirspeed = true;
-    useCompass = true;
-    useRangeFinder = true;
-    numericalProtection = true;
-    refSet = false;
     storeIndex = 0;
-    gpsHgt = 0.0f;
-    baroHgt = 0.0f;
-    GPSstatus = 0;
-    VtasMeas = 0.0f;
-    magDeclination = 0.0f;
 
     // Do the data structure init
     for (unsigned i = 0; i < n_states; i++) {
@@ -2577,9 +2596,7 @@ void AttPosEKF::ZeroVariables()
     correctedDelAng.zero();
     summedDelAng.zero();
     summedDelVel.zero();
-
+    lastGyroOffset.zero();
-    dAngIMU.zero();
-    dVelIMU.zero();
 
     for (unsigned i = 0; i < data_buffer_size; i++) {
 
@@ -2598,12 +2615,27 @@ void AttPosEKF::ZeroVariables()
 
 }
 
-void AttPosEKF::GetFilterState(struct ekf_status_report *state)
+void AttPosEKF::GetFilterState(struct ekf_status_report *err)
 {
-    memcpy(state, &current_ekf_state, sizeof(*state));
+
+    // Copy states
+    for (unsigned i = 0; i < n_states; i++) {
+        current_ekf_state.states[i] = states[i];
+    }
+    current_ekf_state.n_states = n_states;
+
+    memcpy(err, &current_ekf_state, sizeof(*err));
+
+    // err->velHealth = current_ekf_state.velHealth;
+    // err->posHealth = current_ekf_state.posHealth;
+    // err->hgtHealth = current_ekf_state.hgtHealth;
+    // err->velTimeout = current_ekf_state.velTimeout;
+    // err->posTimeout = current_ekf_state.posTimeout;
+    // err->hgtTimeout = current_ekf_state.hgtTimeout;
 }
 
 void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
 {
     memcpy(last_error, &last_ekf_error, sizeof(*last_error));
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
 }

src/modules/ekf_att_pos_estimator/estimator_23states.h

@@ -1,76 +1,10 @@
-#include <math.h>
-#include <stdint.h>
-
 #pragma once
 
-#define GRAVITY_MSS 9.80665f
+#include "estimator_utilities.h"
-#define deg2rad 0.017453292f
-#define rad2deg 57.295780f
-#define pi 3.141592657f
-#define earthRate 0.000072921f
-#define earthRadius 6378145.0f
-#define earthRadiusInv  1.5678540e-7f
-
-class Vector3f
-{
-private:
-public:
-    float x;
-    float y;
-    float z;
-
-    float length(void) const;
-    void zero(void);
-};
-
-class Mat3f
-{
-private:
-public:
-    Vector3f x;
-    Vector3f y;
-    Vector3f z;
-
-    Mat3f();
-
-    void identity();
-    Mat3f transpose(void) const;
-};
-
-Vector3f operator*(float sclIn1, Vector3f vecIn1);
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*( Mat3f matIn, Vector3f vecIn);
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*(Vector3f vecIn1, float sclIn1);
-
-void swap_var(float &d1, float &d2);
 
 const unsigned int n_states = 23;
 const unsigned int data_buffer_size = 50;
 
-enum GPS_FIX {
-    GPS_FIX_NOFIX = 0,
-    GPS_FIX_2D = 2,
-    GPS_FIX_3D = 3
-};
-
-struct ekf_status_report {
-    bool velHealth;
-    bool posHealth;
-    bool hgtHealth;
-    bool velTimeout;
-    bool posTimeout;
-    bool hgtTimeout;
-    uint32_t velFailTime;
-    uint32_t posFailTime;
-    uint32_t hgtFailTime;
-    float states[n_states];
-    bool statesNaN;
-    bool covarianceNaN;
-    bool kalmanGainsNaN;
-};
-
 class AttPosEKF {
 
 public:
@@ -141,7 +75,7 @@ public:
         accelProcessNoise = 0.5f;
     }
 
-    struct {
+    struct mag_state_struct {
         unsigned obsIndex;
         float MagPred[3];
         float SH_MAG[9];
@@ -157,7 +91,12 @@ public:
         float magZbias;
         float R_MAG;
         Mat3f DCM;
-    } magstate;
+    };
+
+    struct mag_state_struct magstate;
+    struct mag_state_struct resetMagState;
+
+
 
 
     // Global variables
@@ -166,6 +105,7 @@ public:
     float P[n_states][n_states]; // covariance matrix
     float Kfusion[n_states]; // Kalman gains
     float states[n_states]; // state matrix
+    float resetStates[n_states];
     float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
     uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
 
@@ -183,6 +123,8 @@ public:
     float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
     Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
     Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f lastGyroOffset;    // Last gyro offset
+    Vector3f delAngTotal;
 
     Mat3f Tbn; // transformation matrix from body to NED coordinates
     Mat3f Tnb; // transformation amtrix from NED to body coordinates
@@ -196,11 +138,11 @@ public:
     float varInnovVelPos[6]; // innovation variance output
 
     float velNED[3]; // North, East, Down velocity obs (m/s)
+    float accelGPSNED[3];   // Acceleration predicted by GPS in earth frame
     float posNE[2]; // North, East position obs (m)
     float hgtMea; //  measured height (m)
     float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
     float rngMea; // Ground distance
-    float posNED[3]; // North, East Down position (m)
 
     float innovMag[3]; // innovation output
     float varInnovMag[3]; // innovation variance output
@@ -243,6 +185,9 @@ public:
     bool useCompass;    ///< boolean true if magnetometer data is being used
     bool useRangeFinder;     ///< true when rangefinder is being used
 
+    bool ekfDiverged;
+    uint64_t lastReset;
+
     struct ekf_status_report current_ekf_state;
     struct ekf_status_report last_ekf_error;
 
@@ -299,9 +244,9 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
 
 static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
 
-static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
+void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
 
-static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
 
 static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
@@ -321,7 +266,7 @@ void ConstrainStates();
 
 void ForceSymmetry();
 
-int CheckAndBound();
+int CheckAndBound(struct ekf_status_report *last_error);
 
 void ResetPosition();
 
@@ -333,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state);
 
 void GetLastErrorState(struct ekf_status_report *last_error);
 
-bool StatesNaN(struct ekf_status_report *err_report);
+bool StatesNaN();
-void FillErrorReport(struct ekf_status_report *err);
 
 void InitializeDynamic(float (&initvelNED)[3], float declination);
 
@@ -342,6 +286,10 @@ protected:
 
 bool FilterHealthy();
 
+bool GyroOffsetsDiverged();
+
+bool VelNEDDiverged();
+
 void ResetHeight(void);
 
 void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);

src/modules/ekf_att_pos_estimator/estimator_utilities.cpp

@@ -0,0 +1,139 @@
+
+#include "estimator_utilities.h"
+
+// Define EKF_DEBUG here to enable the debug print calls
+// if the macro is not set, these will be completely
+// optimized out by the compiler.
+//#define EKF_DEBUG
+
+#ifdef EKF_DEBUG
+#include <stdio.h>
+
+static void
+ekf_debug_print(const char *fmt, va_list args)
+{
+    fprintf(stderr, "%s: ", "[ekf]");
+    vfprintf(stderr, fmt, args);
+
+    fprintf(stderr, "\n");
+}
+
+void
+ekf_debug(const char *fmt, ...)
+{
+    va_list args;
+
+    va_start(args, fmt);
+    ekf_debug_print(fmt, args);
+}
+
+#else
+
+void ekf_debug(const char *fmt, ...) { while(0){} }
+#endif
+
+float Vector3f::length(void) const
+{
+    return sqrt(x*x + y*y + z*z);
+}
+
+void Vector3f::zero(void)
+{
+    x = 0.0f;
+    y = 0.0f;
+    z = 0.0f;
+}
+
+Mat3f::Mat3f() {
+    identity();
+}
+
+void Mat3f::identity() {
+    x.x = 1.0f;
+    x.y = 0.0f;
+    x.z = 0.0f;
+
+    y.x = 0.0f;
+    y.y = 1.0f;
+    y.z = 0.0f;
+
+    z.x = 0.0f;
+    z.y = 0.0f;
+    z.z = 1.0f;
+}
+
+Mat3f Mat3f::transpose(void) const
+{
+    Mat3f ret = *this;
+    swap_var(ret.x.y, ret.y.x);
+    swap_var(ret.x.z, ret.z.x);
+    swap_var(ret.y.z, ret.z.y);
+    return ret;
+}
+
+// overload + operator to provide a vector addition
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x + vecIn2.x;
+    vecOut.y = vecIn1.y + vecIn2.y;
+    vecOut.z = vecIn1.z + vecIn2.z;
+    return vecOut;
+}
+
+// overload - operator to provide a vector subtraction
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x - vecIn2.x;
+    vecOut.y = vecIn1.y - vecIn2.y;
+    vecOut.z = vecIn1.z - vecIn2.z;
+    return vecOut;
+}
+
+// overload * operator to provide a matrix vector product
+Vector3f operator*( Mat3f matIn, Vector3f vecIn)
+{
+    Vector3f vecOut;
+    vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
+    vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
+    vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
+    return vecOut;
+}
+
+// overload % operator to provide a vector cross product
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
+    vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
+    vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(Vector3f vecIn1, float sclIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(float sclIn1, Vector3f vecIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+void swap_var(float &d1, float &d2)
+{
+    float tmp = d1;
+    d1 = d2;
+    d2 = tmp;
+}

src/modules/ekf_att_pos_estimator/estimator_utilities.h

@@ -0,0 +1,82 @@
+#include <math.h>
+#include <stdint.h>
+
+#pragma once
+
+#define GRAVITY_MSS 9.80665f
+#define deg2rad 0.017453292f
+#define rad2deg 57.295780f
+#define pi 3.141592657f
+#define earthRate 0.000072921f
+#define earthRadius 6378145.0
+#define earthRadiusInv  1.5678540e-7
+
+class Vector3f
+{
+private:
+public:
+    float x;
+    float y;
+    float z;
+
+    float length(void) const;
+    void zero(void);
+};
+
+class Mat3f
+{
+private:
+public:
+    Vector3f x;
+    Vector3f y;
+    Vector3f z;
+
+    Mat3f();
+
+    void identity();
+    Mat3f transpose(void) const;
+};
+
+Vector3f operator*(float sclIn1, Vector3f vecIn1);
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*( Mat3f matIn, Vector3f vecIn);
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*(Vector3f vecIn1, float sclIn1);
+
+void swap_var(float &d1, float &d2);
+
+enum GPS_FIX {
+    GPS_FIX_NOFIX = 0,
+    GPS_FIX_2D = 2,
+    GPS_FIX_3D = 3
+};
+
+struct ekf_status_report {
+    bool error;
+    bool velHealth;
+    bool posHealth;
+    bool hgtHealth;
+    bool velTimeout;
+    bool posTimeout;
+    bool hgtTimeout;
+    bool imuTimeout;
+    uint32_t velFailTime;
+    uint32_t posFailTime;
+    uint32_t hgtFailTime;
+    float states[32];
+    unsigned n_states;
+    bool angNaN;
+    bool summedDelVelNaN;
+    bool KHNaN;
+    bool KHPNaN;
+    bool PNaN;
+    bool covarianceNaN;
+    bool kalmanGainsNaN;
+    bool statesNaN;
+    bool gyroOffsetsExcessive;
+    bool covariancesExcessive;
+    bool velOffsetExcessive;
+};
+
+void ekf_debug(const char *fmt, ...);

src/modules/ekf_att_pos_estimator/module.mk

@@ -39,4 +39,5 @@ MODULE_COMMAND	= ekf_att_pos_estimator
 
 SRCS		= ekf_att_pos_estimator_main.cpp \
 		  ekf_att_pos_estimator_params.c \
-		  estimator.cpp
+		  estimator_23states.cpp \
+		  estimator_utilities.cpp

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -43,8 +43,8 @@
  *    Proceedings of the AIAA Guidance, Navigation and Control
  *    Conference, Aug 2004. AIAA-2004-4900.
  *
- * Original implementation for total energy control class:
+ * Implementation for total energy control class:
- *    Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
+ *    Thomas Gubler
  *
  * More details and acknowledgements in the referenced library headers.
  *
@@ -88,7 +88,6 @@
 #include <mavlink/mavlink_log.h>
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
-#include <external_lgpl/tecs/tecs.h>
 #include <drivers/drv_range_finder.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
@@ -199,7 +198,6 @@ private:
 	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
-	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
 	bool						_was_pos_control_mode;
 
@@ -565,23 +563,6 @@ FixedwingPositionControl::parameters_update()
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
-	_tecs.set_time_const(_parameters.time_const);
-	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
-	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
-	_tecs.set_throttle_damp(_parameters.throttle_damp);
-	_tecs.set_integrator_gain(_parameters.integrator_gain);
-	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
-	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
-	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
-	_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
-	_tecs.set_speed_weight(_parameters.speed_weight);
-	_tecs.set_pitch_damping(_parameters.pitch_damping);
-	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
-	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
-	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
-	_tecs.set_heightrate_p(_parameters.heightrate_p);
-	_tecs.set_speedrate_p(_parameters.speedrate_p);
-
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
 	    _parameters.airspeed_max < 5.0f ||
@@ -653,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 		}
 	}
 
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
-
 	return airspeed_updated;
 }
 
@@ -835,10 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
-	if (!_mTecs.getEnabled()) {
-		_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
-	}
-
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
@@ -863,9 +837,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
 
-		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
-		_tecs.set_speed_weight(_parameters.speed_weight);
-
 		/* current waypoint (the one currently heading for) */
 		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
@@ -1228,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* user switched off throttle */
 		if (_manual.z < 0.1f) {
 			throttle_max = 0.0f;
-			/* switch to pure pitch based altitude control, give up speed */
-			_tecs.set_speed_weight(0.0f);
 		}
 
 		/* climb out control */
@@ -1269,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	}
 	else {
-		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max);
+		_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
 	}
-	_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
+	_att_sp.pitch_body = _mTecs.getPitchSetpoint();
 
 	if (_control_mode.flag_control_position_enabled) {
 		last_manual = false;
@@ -1455,29 +1424,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		const math::Vector<3> &ground_speed,
 		tecs_mode mode)
 {
-	if (_mTecs.getEnabled()) {
+	/* Using mtecs library: prepare arguments for mtecs call */
-		/* Using mtecs library: prepare arguments for mtecs call */
+	float flightPathAngle = 0.0f;
-		float flightPathAngle = 0.0f;
+	float ground_speed_length = ground_speed.length();
-		float ground_speed_length = ground_speed.length();
+	if (ground_speed_length > FLT_EPSILON) {
-		if (ground_speed_length > FLT_EPSILON) {
+		flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
-			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
-		}
-		fwPosctrl::LimitOverride limitOverride;
-		if (climbout_mode) {
-			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
-		} else {
-			limitOverride.disablePitchMinOverride();
-		}
-		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
-				limitOverride);
-	} else {
-		/* Using tecs library */
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    climbout_mode, climbout_pitch_min_rad,
-					    throttle_min, throttle_max, throttle_cruise,
-					    pitch_min_rad, pitch_max_rad);
 	}
+	fwPosctrl::LimitOverride limitOverride;
+	if (climbout_mode) {
+		limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+	} else {
+		limitOverride.disablePitchMinOverride();
+	}
+	_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+			limitOverride);
 }
 
 int

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -58,6 +58,7 @@ mTecs::mTecs() :
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
+	_flightPathAngleLowpass(this, "FPA_LP"),
 	_airspeedLowpass(this, "A_LP"),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
@@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	/* time measurement */
 	updateTimeMeasurement();
 
-	/* Filter arispeed */
+	/* Filter airspeed */
 	float airspeedFiltered = _airspeedLowpass.update(airspeed);
 
 	/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
@@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	}
 
 	/* Write part of the status message */
-	_status.flightPathAngleSp = flightPathAngleSp;
-	_status.flightPathAngle = flightPathAngle;
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed = airspeed;
 	_status.airspeedFiltered = airspeedFiltered;
@@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* update parameters first */
 	updateParams();
 
+	/* Filter flightpathangle */
+	float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
+
 	/* calculate values (energies) */
-	float flightPathAngleError = flightPathAngleSp - flightPathAngle;
+	float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
 	float airspeedDerivative = 0.0f;
 	if(_airspeedDerivative.getDt() > 0.0f) {
 		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
@@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
 	float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
 
-	float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
+	float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
 	float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
 	float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
 	float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
 
-	float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
+	float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
 	float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
 	float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
 	float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
@@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
 	if (mode == TECS_MODE_TAKEOFF) {
-		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
+		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
 	} else if (mode == TECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
@@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
 	/* Write part of the status message */
+	_status.flightPathAngleSp = flightPathAngleSp;
+	_status.flightPathAngle = flightPathAngle;
+	_status.flightPathAngleFiltered = flightPathAngleFiltered;
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;
 	_status.airspeedDerivative = airspeedDerivative;
 	_status.totalEnergyRateSp = totalEnergyRateSp;

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -104,12 +104,17 @@ protected:
 	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
 
 	/* control blocks */
-	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
+	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
-	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
+								  is throttle */
-	BlockPDLimited	_controlAltitude;		/**< PD controller for altitude: output is the flight path angle setpoint */
+	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control:
-	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration setpoint */
+								  output is pitch */
+	BlockPDLimited	_controlAltitude;			/**< PD controller for altitude: output is the flight
+								  path angle setpoint */
+	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration
+								  setpoint */
 
 	/* Other calculation Blocks */
+	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
 	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
@@ -118,21 +123,22 @@ protected:
 	float _pitchSp;					/**< Pitch Setpoint from -pi to pi */
 
 	/* Output Limits in special modes */
-	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;		/**< Throttle Limits during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;	    /**< Throttle Limits during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;      	/**< Pitch Limit during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;	    /**< Pitch Limit during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;	/**< Throttle Limits when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;   /**< Throttle Limits when underspeed is detected */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;   	/**< Pitch Limit when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;	    /**< Pitch Limit when underspeed is detected */
-	BlockOutputLimiter _BlockOutputLimiterLandThrottle;		/**< Throttle Limits during landing (only in last phase)*/
+	BlockOutputLimiter _BlockOutputLimiterLandThrottle;	    /**< Throttle Limits during landing (only in
-	BlockOutputLimiter _BlockOutputLimiterLandPitch;		/**< Pitch Limit during landing */
+								      last phase)*/
+	BlockOutputLimiter _BlockOutputLimiterLandPitch;	    /**< Pitch Limit during landing */
 
 	/* Time measurements */
-	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
+	hrt_abstime timestampLastIteration;	/**< Saves the result of hrt_absolute_time() of the last iteration */
 
-	bool _firstIterationAfterReset;			/**< True during the first iteration after a reset */
+	bool _firstIterationAfterReset;		/**< True during the first iteration after a reset */
-	bool _dtCalculated;				/**< True if dt has been calculated in this iteration */
+	bool _dtCalculated;			/**< True if dt has been calculated in this iteration */
 
 	int _counter;
-	bool _debug;					///< Set true to enable debug output
+	bool _debug;				///< Set true to enable debug output
 
 
 	static void	debug_print(const char *fmt, va_list args);

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
  */
 PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
 
+/**
+ * Lowpass (cutoff freq.) for the flight path angle
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
+
 /**
  * P gain for the altitude control
  * Maps the altitude error to the flight path angle setpoint

src/modules/gpio_led/gpio_led.c

@@ -181,16 +181,13 @@ int gpio_led_main(int argc, char *argv[])
 			} else {
 				gpio_led_started = true;
 				warnx("start, using pin: %s", pin_name);
+				exit(0);
 			}
-
-			exit(0);
-
-
 		} else if (!strcmp(argv[1], "stop")) {
 			if (gpio_led_started) {
 				gpio_led_started = false;
 				warnx("stop");
-
+				exit(0);
 			} else {
 				errx(1, "not running");
 			}

src/modules/mavlink/mavlink_ftp.cpp

@@ -0,0 +1,415 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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.
+ *
+ ****************************************************************************/
+
+#include <crc32.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp.h"
+
+MavlinkFTP *MavlinkFTP::_server;
+
+MavlinkFTP *
+MavlinkFTP::getServer()
+{
+	// XXX this really cries out for some locking...
+	if (_server == nullptr) {
+		_server = new MavlinkFTP;
+	}
+	return _server;
+}
+
+MavlinkFTP::MavlinkFTP()
+{
+	// initialise the request freelist
+	dq_init(&_workFree);
+	sem_init(&_lock, 0, 1);
+    
+    // initialize session list
+    for (size_t i=0; i<kMaxSession; i++) {
+        _session_fds[i] = -1;
+    }
+
+	// drop work entries onto the free list
+	for (unsigned i = 0; i < kRequestQueueSize; i++) {
+		_qFree(&_workBufs[i]);
+	}
+
+}
+
+void
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+{
+	// get a free request
+	auto req = _dqFree();
+
+	// if we couldn't get a request slot, just drop it
+	if (req != nullptr) {
+
+		// decode the request
+		if (req->decode(mavlink, msg)) {
+
+			// and queue it for the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+		} else {
+			_qFree(req);
+		}
+	}
+}
+
+void
+MavlinkFTP::_workerTrampoline(void *arg)
+{
+	auto req = reinterpret_cast<Request *>(arg);
+	auto server = MavlinkFTP::getServer();
+
+	// call the server worker with the work item
+	server->_worker(req);
+}
+
+void
+MavlinkFTP::_worker(Request *req)
+{
+	auto hdr = req->header();
+	ErrorCode errorCode = kErrNone;
+	uint32_t messageCRC;
+
+	// basic sanity checks; must validate length before use
+	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	// check request CRC to make sure this is one of ours
+	messageCRC = hdr->crc32;
+	hdr->crc32 = 0;
+	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
+		errorCode = kErrNoRequest;
+		goto out;
+		printf("ftp: bad crc\n");
+	}
+
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+
+	switch (hdr->opcode) {
+	case kCmdNone:
+		break;
+
+	case kCmdTerminate:
+		errorCode = _workTerminate(req);
+		break;
+
+	case kCmdReset:
+		errorCode = _workReset();
+		break;
+
+	case kCmdList:
+		errorCode = _workList(req);
+		break;
+
+	case kCmdOpen:
+		errorCode = _workOpen(req, false);
+		break;
+
+	case kCmdCreate:
+		errorCode = _workOpen(req, true);
+		break;
+
+	case kCmdRead:
+		errorCode = _workRead(req);
+		break;
+
+	case kCmdWrite:
+		errorCode = _workWrite(req);
+		break;
+
+	case kCmdRemove:
+		errorCode = _workRemove(req);
+		break;
+
+	default:
+		errorCode = kErrNoRequest;
+		break;	
+	}
+
+out:
+	// handle success vs. error
+	if (errorCode == kErrNone) {
+		hdr->opcode = kRspAck;
+		printf("FTP: ack\n");
+	} else {
+		printf("FTP: nak %u\n", errorCode);
+		hdr->opcode = kRspNak;
+		hdr->size = 1;
+		hdr->data[0] = errorCode;
+	}
+
+	// respond to the request
+	_reply(req);
+
+	// free the request buffer back to the freelist
+	_qFree(req);
+}
+
+void
+MavlinkFTP::_reply(Request *req)
+{
+	auto hdr = req->header();
+
+	// message is assumed to be already constructed in the request buffer, so generate the CRC
+	hdr->crc32 = 0;
+	hdr->crc32 = crc32(req->rawData(), req->dataSize());
+
+	// then pack and send the reply back to the request source
+	req->reply();
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+	auto hdr = req->header();
+	DIR *dp = opendir(req->dataAsCString());
+
+	if (dp == nullptr) {
+		printf("FTP: can't open path '%s'\n", req->dataAsCString());
+		return kErrNotDir;
+	}
+
+	ErrorCode errorCode = kErrNone;
+	struct dirent entry, *result = nullptr;
+	unsigned offset = 0;
+
+	// move to the requested offset
+	seekdir(dp, hdr->offset);
+
+	for (;;) {
+		// read the directory entry
+		if (readdir_r(dp, &entry, &result)) {
+			errorCode = kErrIO;
+			break;
+		}
+
+		// no more entries?
+		if (result == nullptr) {
+			if (hdr->offset != 0 && offset == 0) {
+				// User is requesting subsequent dir entries but there were none. This means the user asked
+				// to seek past EOF.
+				errorCode = kErrEOF;
+			}
+			// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
+			break;
+		}
+
+		// name too big to fit?
+		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
+			break;
+		}
+
+		// store the type marker
+		switch (entry.d_type) {
+		case DTYPE_FILE:
+			hdr->data[offset++] = kDirentFile;
+			break;
+		case DTYPE_DIRECTORY:
+			hdr->data[offset++] = kDirentDir;
+			break;
+		default:
+			hdr->data[offset++] = kDirentUnknown;
+			break;
+		}
+
+		// copy the name, which we know will fit
+		strcpy((char *)&hdr->data[offset], entry.d_name);
+		offset += strlen(entry.d_name) + 1;
+		printf("FTP: list %s\n", entry.d_name);
+	}
+
+	closedir(dp);
+	hdr->size = offset;
+
+	return errorCode;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+	auto hdr = req->header();
+    
+	int session_index = _findUnusedSession();
+	if (session_index < 0) {
+		return kErrNoSession;
+	}
+
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+    
+	int fd = ::open(req->dataAsCString(), oflag);
+	if (fd < 0) {
+		return create ? kErrPerm : kErrNotFile;
+	}
+	_session_fds[session_index] = fd;
+
+	hdr->session = session_index;
+	hdr->size = 0;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+	auto hdr = req->header();
+
+	int session_index = hdr->session;
+
+	if (!_validSession(session_index)) {
+		return kErrNoSession;
+	}
+
+	// Seek to the specified position
+	printf("Seek %d\n", hdr->offset);
+	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+		// Unable to see to the specified location
+		return kErrEOF;
+	}
+
+	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
+	if (bytes_read < 0) {
+		// Negative return indicates error other than eof
+		return kErrIO;
+	}
+
+	printf("Read success %d\n", bytes_read);
+	hdr->size = bytes_read;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workWrite(Request *req)
+{
+#if 0
+    // NYI: Coming soon
+	auto hdr = req->header();
+
+	// look up session
+	auto session = getSession(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// append to file
+	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		// XXX might also be no space, I/O, etc.
+		return kErrNotAppend;
+	}
+
+	hdr->size = result;
+	return kErrNone;
+#else
+	return kErrPerm;
+#endif
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+	auto hdr = req->header();
+
+	// for now, send error reply
+	return kErrPerm;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
+{
+	auto hdr = req->header();
+    
+	if (!_validSession(hdr->session)) {
+		return kErrNoSession;
+	}
+    
+	::close(_session_fds[hdr->session]);
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] != -1) {
+			::close(_session_fds[i]);
+			_session_fds[i] = -1;
+		}
+	}
+
+	return kErrNone;
+}
+
+bool
+MavlinkFTP::_validSession(unsigned index)
+{
+	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
+		return false;
+	}
+	return true;
+}
+
+int
+MavlinkFTP::_findUnusedSession(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] == -1) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+	// guarantee nul termination
+	if (header()->size < kMaxDataLength) {
+		requestData()[header()->size] = '\0';
+	} else {
+		requestData()[kMaxDataLength - 1] = '\0';
+	}
+
+	// and return data
+	return (char *)&(header()->data[0]);
+}

src/modules/mavlink/mavlink_ftp.h

@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file mavlink_ftp.h
+ *
+ * MAVLink remote file server.
+ *
+ * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
+ * a session ID and sequence number.
+ *
+ * A limited number of requests (currently 2) may be outstanding at a time.
+ * Additional messages will be discarded.
+ *
+ * Messages consist of a fixed header, followed by a data area.
+ *
+ */
+ 
+#include <dirent.h>
+#include <queue.h>
+
+#include <nuttx/wqueue.h>
+#include <systemlib/err.h>
+
+#include "mavlink_messages.h"
+
+class MavlinkFTP
+{
+public:
+	MavlinkFTP();
+
+	static MavlinkFTP	*getServer();
+
+	// static interface
+	void			handle_message(Mavlink* mavlink,
+					mavlink_message_t *msg);
+
+private:
+
+	static const unsigned	kRequestQueueSize = 2;
+
+	static MavlinkFTP	*_server;
+
+	struct RequestHeader
+	{
+		uint8_t		magic;
+		uint8_t		session;
+		uint8_t		opcode;
+		uint8_t		size;
+		uint32_t	crc32;
+		uint32_t	offset;
+		uint8_t		data[];
+	};
+
+	enum Opcode : uint8_t
+	{
+		kCmdNone,	// ignored, always acked
+		kCmdTerminate,	// releases sessionID, closes file
+		kCmdReset,	// terminates all sessions
+		kCmdList,	// list files in <path> from <offset>
+		kCmdOpen,	// opens <path> for reading, returns <session>
+		kCmdRead,	// reads <size> bytes from <offset> in <session>
+		kCmdCreate,	// creates <path> for writing, returns <session>
+		kCmdWrite,	// appends <size> bytes at <offset> in <session>
+		kCmdRemove,	// remove file (only if created by server?)
+
+		kRspAck,
+		kRspNak
+	};
+
+	enum ErrorCode : uint8_t
+	{
+		kErrNone,
+		kErrNoRequest,
+		kErrNoSession,
+		kErrSequence,
+		kErrNotDir,
+		kErrNotFile,
+		kErrEOF,
+		kErrNotAppend,
+		kErrTooBig,
+		kErrIO,
+		kErrPerm
+	};
+
+    int _findUnusedSession(void);
+    bool _validSession(unsigned index);
+
+    static const unsigned kMaxSession = 2;
+    int	_session_fds[kMaxSession];
+
+	class Request
+	{
+	public:
+		union {
+			dq_entry_t	entry;
+			work_s		work;
+		};
+
+		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+				_mavlink = mavlink;
+				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				return true;
+			}
+			return false;
+		}
+
+		void		reply() {
+
+			// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
+			// flat memory architecture, as we're operating between threads here.
+			mavlink_message_t msg;
+			msg.checksum = 0;
+			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+				_mavlink->get_channel(), &msg, sequence(), rawData());
+
+            _mavlink->lockMessageBufferMutex();
+            bool fError = _mavlink->message_buffer_write(&msg, len);
+            _mavlink->unlockMessageBufferMutex();
+            
+            if (!fError) {
+				warnx("FTP TX ERR");
+			} else {
+				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+					_mavlink->get_system_id(),
+					_mavlink->get_component_id(),
+					_mavlink->get_channel(),
+					len,
+					msg.checksum);
+			}
+		}
+
+		uint8_t		*rawData() { return &_message.data[0]; }
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		uint8_t         *requestData() { return &(header()->data[0]); }
+		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
+		uint16_t	sequence() const { return _message.seqnr; }
+		mavlink_channel_t channel() { return _mavlink->get_channel(); }
+
+		char		*dataAsCString();
+
+	private:
+		Mavlink			*_mavlink;
+		mavlink_encapsulated_data_t _message;
+
+	};
+
+	static const uint8_t	kProtocolMagic = 'f';
+	static const char	kDirentFile = 'F';
+	static const char	kDirentDir = 'D';
+	static const char	kDirentUnknown = 'U';
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+
+	/// Request worker; runs on the low-priority work queue to service
+	/// remote requests.
+	///
+	static void		_workerTrampoline(void *arg);
+	void			_worker(Request *req);
+
+	/// Reply to a request (XXX should be a Request method)
+	///
+	void			_reply(Request *req);
+
+	ErrorCode		_workList(Request *req);
+	ErrorCode		_workOpen(Request *req, bool create);
+	ErrorCode		_workRead(Request *req);
+	ErrorCode		_workWrite(Request *req);
+	ErrorCode		_workRemove(Request *req);
+	ErrorCode		_workTerminate(Request *req);
+	ErrorCode		_workReset();
+
+	// work freelist
+	Request			_workBufs[kRequestQueueSize];
+	dq_queue_t		_workFree;
+	sem_t			_lock;
+
+	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
+	void			_qUnlock() { sem_post(&_lock); }
+
+	void			_qFree(Request *req) {
+		_qLock();
+		dq_addlast(&req->entry, &_workFree);
+		_qUnlock();
+	}
+
+	Request			*_dqFree() {
+		_qLock();
+		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
+		_qUnlock();
+		return req;
+	}
+    
+};

src/modules/mavlink/mavlink_main.cpp

@@ -83,6 +83,10 @@
 #include "mavlink_rate_limiter.h"
 #include "mavlink_commands.h"
 
+#ifndef MAVLINK_CRC_EXTRA
+ #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#endif
+
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0};
 void
 mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
 {
+
 	Mavlink *instance;
 
 	switch (channel) {
@@ -192,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 			if (buf_free < desired) {
 				/* we don't want to send anything just in half, so return */
+				instance->count_txerr();
 				return;
 			}
 		}
 
 		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
-			warnx("TX FAIL");
+			instance->count_txerr();
 		} else {
 			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
 		}
@@ -230,6 +236,7 @@ Mavlink::Mavlink() :
 	_verbose(false),
 	_forwarding_on(false),
 	_passing_on(false),
+	_ftp_on(false),
 	_uart_fd(-1),
 	_mavlink_param_queue_index(0),
 	_subscribe_to_stream(nullptr),
@@ -243,7 +250,8 @@ Mavlink::Mavlink() :
 	_param_use_hil_gps(0),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
 	_wpm = &_wpm_s;
 	mission.count = 0;
@@ -296,6 +304,7 @@ Mavlink::Mavlink() :
 Mavlink::~Mavlink()
 {
 	perf_free(_loop_perf);
+	perf_free(_txerr_perf);
 
 	if (_task_running) {
 		/* task wakes up every 10ms or so at the longest */
@@ -320,6 +329,12 @@ Mavlink::~Mavlink()
 	LL_DELETE(_mavlink_instances, this);
 }
 
+void
+Mavlink::count_txerr()
+{
+	perf_count(_txerr_perf);
+}
+
 void
 Mavlink::set_mode(enum MAVLINK_MODE mode)
 {
@@ -459,7 +474,7 @@ Mavlink::get_instance_id()
 	return _instance_id;
 }
 
-mavlink_channel_t
+const mavlink_channel_t
 Mavlink::get_channel()
 {
 	return _channel;
@@ -537,6 +552,16 @@ void Mavlink::mavlink_update_system(void)
 	_use_hil_gps = (bool)use_hil_gps;
 }
 
+int Mavlink::get_system_id()
+{
+	return mavlink_system.sysid;
+}
+
+int Mavlink::get_component_id()
+{
+	return mavlink_system.compid;
+}
+
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
 {
 	/* process baud rate */
@@ -1631,11 +1656,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 int
 Mavlink::message_buffer_init(int size)
 {
+
 	_message_buffer.size = size;
 	_message_buffer.write_ptr = 0;
 	_message_buffer.read_ptr = 0;
 	_message_buffer.data = (char*)malloc(_message_buffer.size);
-	return (_message_buffer.data == 0) ? ERROR : OK;
+
+	int ret;
+	if (_message_buffer.data == 0) {
+		ret = ERROR;
+		_message_buffer.size = 0;
+	} else {
+		ret = OK;
+	}
+
+	return ret;
 }
 
 void
@@ -1763,7 +1798,7 @@ Mavlink::task_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1819,6 +1854,10 @@ Mavlink::task_main(int argc, char *argv[])
 			_wait_to_transmit = true;
 			break;
 
+		case 'x':
+			_ftp_on = true;
+			break;
+
 		default:
 			err_flag = true;
 			break;
@@ -1884,9 +1923,12 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
-	if (_passing_on) {
+	if (_passing_on || _ftp_on) {
-		/* initialize message buffer if multiplexing is on */
+		/* initialize message buffer if multiplexing is on or its needed for FTP.
-		if (OK != message_buffer_init(300)) {
+		 * make space for two messages plus off-by-one space as we use the empty element
+		 * marker ring buffer approach.
+		 */
+		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 
@@ -2049,32 +2091,50 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-		/* pass messages from other UARTs */
+		/* pass messages from other UARTs or FTP worker */
-		if (_passing_on) {
+		if (_passing_on || _ftp_on) {
 
 			bool is_part;
-			void *read_ptr;
+			uint8_t *read_ptr;
+            uint8_t *write_ptr;
 
-			/* guard get ptr by mutex */
 			pthread_mutex_lock(&_message_buffer_mutex);
-			int available = message_buffer_get_ptr(&read_ptr, &is_part);
+			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
 			pthread_mutex_unlock(&_message_buffer_mutex);
 
 			if (available > 0) {
-				/* write first part of buffer */
+                // Reconstruct message from buffer
-				_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+
-				message_buffer_mark_read(available);
+				mavlink_message_t msg;
+                write_ptr = (uint8_t*)&msg;
+
+                // Pull a single message from the buffer
+                int read_count = available;
+                if (read_count > sizeof(mavlink_message_t)) {
+                    read_count = sizeof(mavlink_message_t);
+                }
+                
+                memcpy(write_ptr, read_ptr, read_count);
+                
+                // We hold the mutex until after we complete the second part of the buffer. If we don't
+                // we may end up breaking the empty slot overflow detection semantics when we mark the
+                // possibly partial read below.
+                pthread_mutex_lock(&_message_buffer_mutex);
+                
+				message_buffer_mark_read(read_count);
 
 				/* write second part of buffer if there is some */
-				if (is_part) {
+				if (is_part && read_count < sizeof(mavlink_message_t)) {
-					/* guard get ptr by mutex */
+                    write_ptr += read_count;
-					pthread_mutex_lock(&_message_buffer_mutex);
+					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
-					available = message_buffer_get_ptr(&read_ptr, &is_part);
+                    read_count = sizeof(mavlink_message_t) - read_count;
-					pthread_mutex_unlock(&_message_buffer_mutex);
+                    memcpy(write_ptr, read_ptr, read_count);
-
-					_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
 					message_buffer_mark_read(available);
 				}
+                
+                pthread_mutex_unlock(&_message_buffer_mutex);
+
+                _mavlink_resend_uart(_channel, &msg);
 			}
 		}
 
@@ -2124,7 +2184,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* close mavlink logging device */
 	close(_mavlink_fd);
 
-	if (_passing_on) {
+	if (_passing_on || _ftp_on) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
@@ -2142,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[])
 	/* create the instance in task context */
 	Mavlink *instance = new Mavlink();
 
-	/* this will actually only return once MAVLink exits */
+	int res;
-	int res = instance->task_main(argc, argv);
 
-	/* delete instance on main thread end */
+	if (!instance) {
-	delete instance;
+
+		/* out of memory */
+		res = -ENOMEM;
+		warnx("OUT OF MEM");
+	} else {
+		/* this will actually only return once MAVLink exits */
+		res = instance->task_main(argc, argv);
+
+		/* delete instance on main thread end */
+		delete instance;
+	}
 
 	return res;
 }
@@ -2266,7 +2335,7 @@ Mavlink::stream(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
 }
 
 int mavlink_main(int argc, char *argv[])

src/modules/mavlink/mavlink_main.h

@@ -123,27 +123,41 @@ public:
 	/**
 	 * Display the mavlink status.
 	 */
-	void		status();
+	void			status();
 
-	static int stream(int argc, char *argv[]);
+	static int		stream(int argc, char *argv[]);
 
-	static int	instance_count();
+	static int		instance_count();
 
-	static Mavlink *new_instance();
+	static Mavlink		*new_instance();
 
-	static Mavlink *get_instance(unsigned instance);
+	static Mavlink		*get_instance(unsigned instance);
 
-	static Mavlink *get_instance_for_device(const char *device_name);
+	static Mavlink		*get_instance_for_device(const char *device_name);
 
-	static int	destroy_all_instances();
+	static int		destroy_all_instances();
 
-	static bool	instance_exists(const char *device_name, Mavlink *self);
+	static bool		instance_exists(const char *device_name, Mavlink *self);
 
-	static void	forward_message(mavlink_message_t *msg, Mavlink *self);
+	static void		forward_message(mavlink_message_t *msg, Mavlink *self);
 
-	static int get_uart_fd(unsigned index);
+	static int		get_uart_fd(unsigned index);
 
-	int get_uart_fd();
+	int			get_uart_fd();
+
+	/**
+	 * Get the MAVLink system id.
+	 *
+	 * @return		The system ID of this vehicle
+	 */
+	int			get_system_id();
+
+	/**
+	 * Get the MAVLink component id.
+	 *
+	 * @return		The component ID of this vehicle
+	 */
+	int			get_component_id();
 
 	const char *_device_name;
 
@@ -153,30 +167,30 @@ public:
 		MAVLINK_MODE_CAMERA
 	};
 
-	void		set_mode(enum MAVLINK_MODE);
+	void			set_mode(enum MAVLINK_MODE);
-	enum MAVLINK_MODE		get_mode() { return _mode; }
+	enum MAVLINK_MODE	get_mode() { return _mode; }
 
-	bool		get_hil_enabled() { return _hil_enabled; }
+	bool			get_hil_enabled() { return _hil_enabled; }
 
-	bool		get_use_hil_gps() { return _use_hil_gps; }
+	bool			get_use_hil_gps() { return _use_hil_gps; }
 
-	bool		get_flow_control_enabled() { return _flow_control_enabled; }
+	bool			get_flow_control_enabled() { return _flow_control_enabled; }
 
-	bool		get_forwarding_on() { return _forwarding_on; }
+	bool			get_forwarding_on() { return _forwarding_on; }
 
 	/**
 	 * Handle waypoint related messages.
 	 */
-	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
+	void			mavlink_wpm_message_handler(const mavlink_message_t *msg);
 
-	static int start_helper(int argc, char *argv[]);
+	static int		start_helper(int argc, char *argv[]);
 
 	/**
 	 * Handle parameter related messages.
 	 */
-	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+	void			mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
 
-	void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+	void			get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
 
 	/**
 	 * Enable / disable Hardware in the Loop simulation mode.
@@ -186,90 +200,105 @@ public:
 	 *			requested change could not be made or was
 	 *			redundant.
 	 */
-	int		set_hil_enabled(bool hil_enabled);
+	int			set_hil_enabled(bool hil_enabled);
 
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+	MavlinkOrbSubscription	*add_orb_subscription(const orb_id_t topic);
 
-	int get_instance_id();
+	int			get_instance_id();
 
 	/**
 	 * Enable / disable hardware flow control.
 	 *
 	 * @param enabled	True if hardware flow control should be enabled
 	 */
-	int enable_flow_control(bool enabled);
+	int			enable_flow_control(bool enabled);
 
-	mavlink_channel_t get_channel();
+	const mavlink_channel_t	get_channel();
 
-	bool		_task_should_exit;		/**< if true, mavlink task should exit */
+	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
-	int get_mavlink_fd() { return _mavlink_fd; }
+	bool			_task_should_exit;	/**< if true, mavlink task should exit */
+
+	int			get_mavlink_fd() { return _mavlink_fd; }
+
+	MavlinkStream * get_streams() { return _streams; } const
 
 
 	/* Functions for waiting to start transmission until message received. */
-	void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
-	bool get_has_received_messages() { return _received_messages; }
+	bool			get_has_received_messages() { return _received_messages; }
-	void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+	void			set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
-	bool get_wait_to_transmit() { return _wait_to_transmit; }
+	bool			get_wait_to_transmit() { return _wait_to_transmit; }
-	bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+
+	bool			message_buffer_write(void *ptr, int size);
+    
+    void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+    void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
+
+	/**
+	 * Count a transmision error
+	 */
+	void count_txerr();
 
 protected:
-	Mavlink	*next;
+	Mavlink			*next;
 
 private:
-	int _instance_id;
+	int			_instance_id;
 
-	int		_mavlink_fd;
+	int			_mavlink_fd;
-	bool		_task_running;
+	bool			_task_running;
 
 	/* states */
-	bool		_hil_enabled;		/**< Hardware In the Loop mode */
+	bool			_hil_enabled;		/**< Hardware In the Loop mode */
-	bool		_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
-	bool		_is_usb_uart;		/**< Port is USB */
+	bool			_is_usb_uart;		/**< Port is USB */
-	bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
+	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
-	bool        _received_messages; /**< Whether we've received valid mavlink messages. */
+	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
 
-	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
+	unsigned		_main_loop_delay;	/**< mainloop delay, depends on data rate */
 
-	MavlinkOrbSubscription *_subscriptions;
+	MavlinkOrbSubscription	*_subscriptions;
-	MavlinkStream *_streams;
+	MavlinkStream		*_streams;
 
-	orb_advert_t	_mission_pub;
+	orb_advert_t		_mission_pub;
-	struct mission_s mission;
+	struct mission_s	mission;
-	MAVLINK_MODE _mode;
+	MAVLINK_MODE		_mode;
 
-	uint8_t _mavlink_wpm_comp_id;
+	uint8_t			_mavlink_wpm_comp_id;
-	mavlink_channel_t _channel;
+	mavlink_channel_t	_channel;
 
 	struct mavlink_logbuffer _logbuffer;
-	unsigned int _total_counter;
+	unsigned int		_total_counter;
 
-	pthread_t _receive_thread;
+	pthread_t		_receive_thread;
 
 	/* Allocate storage space for waypoints */
-	mavlink_wpm_storage _wpm_s;
+	mavlink_wpm_storage	_wpm_s;
-	mavlink_wpm_storage *_wpm;
+	mavlink_wpm_storage	*_wpm;
 
-	bool _verbose;
+	bool			_verbose;
-	bool _forwarding_on;
+	bool			_forwarding_on;
-	bool _passing_on;
+	bool			_passing_on;
-	int _uart_fd;
+	bool			_ftp_on;
-	int _baudrate;
+	int			_uart_fd;
-	int _datarate;
+	int			_baudrate;
+	int			_datarate;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
 	 * logic will send parameters from the current index
 	 * to len - 1, the end of the param list.
 	 */
-	unsigned int _mavlink_param_queue_index;
+	unsigned int		_mavlink_param_queue_index;
 
-	bool mavlink_link_termination_allowed;
+	bool			mavlink_link_termination_allowed;
 
-	char 	*_subscribe_to_stream;
+	char 			*_subscribe_to_stream;
-	float	_subscribe_to_stream_rate;
+	float			_subscribe_to_stream_rate;
 
-	bool		_flow_control_enabled;
+	bool			_flow_control_enabled;
 
 	struct mavlink_message_buffer {
 		int write_ptr;
@@ -277,11 +306,14 @@ private:
 		int size;
 		char *data;
 	};
+
 	mavlink_message_buffer	_message_buffer;
 
 	pthread_mutex_t		_message_buffer_mutex;
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t		_loop_perf;			/**< loop performance counter */
+	perf_counter_t		_txerr_perf;			/**< TX error counter */
+
 	bool			_param_initialized;
 	param_t			_param_system_id;
 	param_t			_param_component_id;
@@ -294,7 +326,7 @@ private:
 	 * @param param		The parameter id to send.
 	 * @return		zero on success, nonzero on failure.
 	 */
-	int mavlink_pm_send_param(param_t param);
+	int			mavlink_pm_send_param(param_t param);
 
 	/**
 	 * Send one parameter identified by index.
@@ -302,7 +334,7 @@ private:
 	 * @param index		The index of the parameter to send.
 	 * @return		zero on success, nonzero else.
 	 */
-	int mavlink_pm_send_param_for_index(uint16_t index);
+	int			mavlink_pm_send_param_for_index(uint16_t index);
 
 	/**
 	 * Send one parameter identified by name.
@@ -310,14 +342,14 @@ private:
 	 * @param name		The index of the parameter to send.
 	 * @return		zero on success, nonzero else.
 	 */
-	int mavlink_pm_send_param_for_name(const char *name);
+	int			mavlink_pm_send_param_for_name(const char *name);
 
 	/**
 	 * Send a queue of parameters, one parameter per function call.
 	 *
 	 * @return		zero on success, nonzero on failure
 	 */
-	int mavlink_pm_queued_send(void);
+	int			mavlink_pm_queued_send(void);
 
 	/**
 	 * Start sending the parameter queue.
@@ -327,12 +359,12 @@ private:
 	 * mavlink_pm_queued_send().
 	 * @see 		mavlink_pm_queued_send()
 	 */
-	void mavlink_pm_start_queued_send();
+	void			mavlink_pm_start_queued_send();
 
-	void mavlink_update_system();
+	void			mavlink_update_system();
 
-	void mavlink_waypoint_eventloop(uint64_t now);
+	void			mavlink_waypoint_eventloop(uint64_t now);
-	void mavlink_wpm_send_waypoint_reached(uint16_t seq);
+	void 			mavlink_wpm_send_waypoint_reached(uint16_t seq);
 	void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
 	void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
 	void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
@@ -349,7 +381,6 @@ private:
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
 	int configure_stream(const char *stream_name, const float rate);
-	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
 	int message_buffer_init(int size);
 
@@ -359,8 +390,6 @@ private:
 
 	int message_buffer_is_empty();
 
-	bool message_buffer_write(void *ptr, int size);
-
 	int message_buffer_get_ptr(void **ptr, bool *is_part);
 
 	void message_buffer_mark_read(int n);

src/modules/mavlink/mavlink_messages.cpp

@@ -232,6 +232,11 @@ public:
 		return "HEARTBEAT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HEARTBEAT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHeartbeat();
@@ -254,8 +259,15 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		/* always send the heartbeat, independent of the update status of the topics */
-		(void)status_sub->update(&status);
+		if (!status_sub->update(&status)) {
-		(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
+			/* if topic update failed fill it with defaults */
+			memset(&status, 0, sizeof(status));
+		}
+
+		if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			/* if topic update failed fill it with defaults */
+			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+		}
 
 		uint8_t mavlink_state = 0;
 		uint8_t mavlink_base_mode = 0;
@@ -285,6 +297,11 @@ public:
 		return "SYS_STATUS";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SYS_STATUS;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamSysStatus();
@@ -336,6 +353,11 @@ public:
 		return "HIGHRES_IMU";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIGHRES_IMU;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHighresIMU();
@@ -421,6 +443,11 @@ public:
 		return "ATTITUDE";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitude();
@@ -467,6 +494,11 @@ public:
 		return "ATTITUDE_QUATERNION";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeQuaternion();
@@ -519,6 +551,11 @@ public:
 		return "VFR_HUD";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VFR_HUD;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamVFRHUD();
@@ -602,6 +639,11 @@ public:
 		return "GPS_RAW_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_RAW_INT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSRawInt();
@@ -655,6 +697,11 @@ public:
 		return "GLOBAL_POSITION_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionInt();
@@ -716,6 +763,11 @@ public:
 		return "LOCAL_POSITION_NED";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionNED();
@@ -767,6 +819,11 @@ public:
 		return "VICON_POSITION_ESTIMATE";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamViconPositionEstimate();
@@ -817,6 +874,11 @@ public:
 		return "GPS_GLOBAL_ORIGIN";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSGlobalOrigin();
@@ -857,6 +919,11 @@ public:
 		return MavlinkStreamServoOutputRaw<N>::get_name_static();
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
+	}
+
 	static const char *get_name_static()
 	{
 		switch (N) {
@@ -934,6 +1001,11 @@ public:
 		return "HIL_CONTROLS";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIL_CONTROLS;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHILControls();
@@ -1071,6 +1143,11 @@ public:
 		return "GLOBAL_POSITION_SETPOINT_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionSetpointInt();
@@ -1114,6 +1191,11 @@ public:
 		return "LOCAL_POSITION_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionSetpoint();
@@ -1162,6 +1244,11 @@ public:
 		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawThrustSetpoint();
@@ -1210,6 +1297,11 @@ public:
 		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
@@ -1258,6 +1350,11 @@ public:
 		return "RC_CHANNELS_RAW";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRCChannelsRaw();
@@ -1342,6 +1439,11 @@ public:
 		return "MANUAL_CONTROL";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MANUAL_CONTROL;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamManualControl();
@@ -1391,6 +1493,11 @@ public:
 		return "OPTICAL_FLOW";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamOpticalFlow();
@@ -1439,6 +1546,11 @@ public:
 		return "ATTITUDE_CONTROLS";
 	}
 
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeControls();
@@ -1497,6 +1609,11 @@ public:
 		return "NAMED_VALUE_FLOAT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamNamedValueFloat();
@@ -1545,6 +1662,11 @@ public:
 		return "CAMERA_CAPTURE";
 	}
 
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamCameraCapture();
@@ -1590,6 +1712,11 @@ public:
 		return "DISTANCE_SENSOR";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamDistanceSensor();

src/modules/mavlink/mavlink_receiver.cpp

@@ -121,6 +121,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
 	memset(&_control_mode, 0, sizeof(_control_mode));
+
+	// make sure the FTP server is started
+	(void)MavlinkFTP::getServer();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -163,6 +166,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_heartbeat(msg);
 		break;
 
+	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+		handle_message_request_data_stream(msg);
+		break;
+
+	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+		break;
+
 	default:
 		break;
 	}
@@ -468,6 +479,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
+{
+	mavlink_request_data_stream_t req;
+	mavlink_msg_request_data_stream_decode(msg, &req);
+
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
+		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
+
+		MavlinkStream *stream;
+		LL_FOREACH(_mavlink->get_streams(), stream) {
+			if (req.req_stream_id == stream->get_id()) {
+				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
+			}
+		}
+	}
+}
+
 void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -71,6 +71,8 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 
+#include "mavlink_ftp.h"
+
 class Mavlink;
 
 class MavlinkReceiver
@@ -116,6 +118,7 @@ private:
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
+	void handle_message_request_data_stream(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);

src/modules/mavlink/mavlink_stream.h

@@ -67,6 +67,7 @@ public:
 	static const char *get_name_static();
 	virtual void subscribe(Mavlink *mavlink) = 0;
 	virtual const char *get_name() const = 0;
+	virtual uint8_t get_id() = 0;
 
 protected:
 	mavlink_channel_t _channel;

src/modules/mavlink/module.mk

@@ -43,7 +43,8 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_commands.cpp
+			mavlink_commands.cpp \
+			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 

src/modules/sdlog2/sdlog2.c

@@ -976,7 +976,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
 			struct log_TELE_s log_TELE;
-			struct log_ESTM_s log_ESTM;
+			struct log_EST0_s log_EST0;
+			struct log_EST1_s log_EST1;
 			struct log_PWR_s log_PWR;
 			struct log_VICN_s log_VICN;
 			struct log_GS0A_s log_GS0A;
@@ -1489,15 +1490,21 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- ESTIMATOR STATUS --- */
 		if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
-			log_msg.msg_type = LOG_ESTM_MSG;
+			log_msg.msg_type = LOG_EST0_MSG;
-			unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s);
+			unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
-			memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s));
+			memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
-			memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy);
+			memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0);
-			log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states;
+			log_msg.body.log_EST0.n_states = buf.estimator_status.n_states;
-			log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
+			log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags;
-			log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
+			log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags;
-			log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
+			log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags;
-			LOGBUFFER_WRITE_AND_COUNT(ESTM);
+			LOGBUFFER_WRITE_AND_COUNT(EST0);
+
+			log_msg.msg_type = LOG_EST1_MSG;
+			unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s);
+			memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s));
+			memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1);
+			LOGBUFFER_WRITE_AND_COUNT(EST1);
 		}
 
 		/* --- TECS STATUS --- */
@@ -1507,6 +1514,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
+			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
 			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
 			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;

src/modules/sdlog2/sdlog2_messages.h

@@ -288,15 +288,7 @@ struct log_TELE_s {
 	uint8_t txbuf;
 };
 
-/* --- ESTM - ESTIMATOR STATUS --- */
+// ID 23 available
-#define LOG_ESTM_MSG 23
-struct log_ESTM_s {
-	float s[10];
-	uint8_t n_states;
-	uint8_t states_nan;
-	uint8_t covariance_nan;
-	uint8_t kalman_gain_nan;
-};
 
 /* --- PWR - ONBOARD POWER SYSTEM --- */
 #define LOG_PWR_MSG 24
@@ -353,6 +345,7 @@ struct log_TECS_s {
 	float altitude;
 	float flightPathAngleSp;
 	float flightPathAngle;
+	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeed;
 	float airspeedFiltered;
@@ -376,6 +369,22 @@ struct log_WIND_s {
 	float cov_y;
 };
 
+/* --- EST0 - ESTIMATOR STATUS --- */
+#define LOG_EST0_MSG 32
+struct log_EST0_s {
+	float s[12];
+	uint8_t n_states;
+	uint8_t nan_flags;
+	uint8_t health_flags;
+	uint8_t timeout_flags;
+};
+
+/* --- EST1 - ESTIMATOR STATUS --- */
+#define LOG_EST1_MSG 33
+struct log_EST1_s {
+	float s[16];
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -424,14 +433,15 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
 	LOG_FORMAT(TELE, "BBBBHHB",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
-	LOG_FORMAT(ESTM, "ffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
+	LOG_FORMAT(EST0, "ffffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
+	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
 	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */

src/modules/sensors/sensor_params.c

@@ -242,6 +242,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
  */
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
+/**
+ * Board rotation Y (Pitch) offset
+ *
+ * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
+
+/**
+ * Board rotation X (Roll) offset
+ *
+ * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
+
+/**
+ * Board rotation Z (YAW) offset
+ *
+ * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
+
 /**
  * External magnetometer rotation
  *

src/modules/sensors/sensors.cpp

@@ -229,7 +229,7 @@ private:
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
-
+	
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -252,6 +252,8 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
+		
+		float board_offset[3];
 
 		int rc_map_roll;
 		int rc_map_pitch;
@@ -346,6 +348,8 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
+		
+		param_t board_offset[3];
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -594,6 +598,11 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+	
+	/* rotation offsets */
+	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
+	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
+	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -806,6 +815,18 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+	
+	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
+	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
+	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
+	
+	/** fine tune board offset on parameter update **/
+	math::Matrix<3, 3> board_rotation_offset; 
+	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
+	
+	_board_rotation = _board_rotation * board_rotation_offset;
 
 	return OK;
 }

src/modules/systemlib/perf_counter.h

@@ -75,7 +75,7 @@ __EXPORT extern void		perf_free(perf_counter_t handle);
 /**
  * Count a performance event.
  *
- * This call only affects counters that take single events; PC_COUNT etc.
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
  *
  * @param handle		The handle returned from perf_alloc.
  */

src/modules/uORB/topics/estimator_status.h

@@ -64,9 +64,9 @@ struct estimator_status_report {
 	uint64_t timestamp;			/**< Timestamp in microseconds since boot */
 	float states[32];			/**< Internal filter states */
 	float n_states;				/**< Number of states effectively used */
-	bool states_nan;			/**< If set to true, one of the states is NaN */
+	uint8_t nan_flags;			/**< Bitmask to indicate NaN states */
-	bool covariance_nan;			/**< If set to true, the covariance matrix went NaN */
+	uint8_t health_flags;			/**< Bitmask to indicate sensor health states (vel, pos, hgt) */
-	bool kalman_gain_nan;			/**< If set to true, the Kalman gain matrix went NaN */
+	uint8_t timeout_flags;			/**< Bitmask to indicate timeout flags (vel, pos, hgt) */
 
 };
 

src/modules/uORB/topics/tecs_status.h

@@ -68,6 +68,7 @@ struct tecs_status_s {
 	float altitude;
 	float flightPathAngleSp;
 	float flightPathAngle;
+	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeed;
 	float airspeedFiltered;

src/systemcmds/mtd/mtd.c

@@ -193,8 +193,12 @@ ramtron_attach(void)
 		errx(1, "failed to initialize mtd driver");
 
 	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
-        if (ret != OK)
+	if (ret != OK) {
-            warnx(1, "failed to set bus speed");
+		// FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried
+		// that but setting the bug speed does fail all the time. Which was then exiting and the board would
+		// not run correctly. So changed to warnx.
+		warnx("failed to set bus speed");
+	}
 
 	attached = true;
 }

src/systemcmds/tests/test_mixer.cpp

@@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[])
 	}
 
 	warnx("SUCCESS: No errors in mixer test");
+	return 0;
 }
 
 static int