Merge pull request #156 from jgoppert/sensor_hil_rebase

Rebase of changes to sensor_hil_fixedwing branch.

.gitignore

@@ -8,6 +8,7 @@
 apps/namedapp/namedapp_list.h
 apps/namedapp/namedapp_proto.h
 Make.dep
+*.pyc
 *.o
 *.a
 *.d
@@ -43,3 +44,7 @@ cscope.out
 .configX-e
 nuttx-export.zip
 dot.gdbinit
+mavlink/include/mavlink/v0.9/
+.*.swp
+.swp
+core

apps/commander/state_machine_helper.c

@@ -134,7 +134,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 
 	case SYSTEM_STATE_REBOOT:
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
+			|| current_status->flag_hil_enabled) {
 			invalid_state = false;
 			/* set system flags according to state */
 			current_status->flag_system_armed = false;
@@ -708,7 +709,9 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 	case SYSTEM_STATE_REBOOT:
 		printf("try to reboot\n");
 
-		if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
+		if (current_system_state == SYSTEM_STATE_STANDBY
+				|| current_system_state == SYSTEM_STATE_PREFLIGHT
+				|| current_status->flag_hil_enabled) {
 			printf("system will reboot\n");
 			mavlink_log_critical(mavlink_fd, "Rebooting..");
 			usleep(200000);

apps/examples/control_demo/control_demo.cpp

@@ -108,7 +108,7 @@ int control_demo_main(int argc, char *argv[])
 		deamon_task = task_spawn("control_demo",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 10,
-					 4096,
+					 5120,
 					 control_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);

apps/examples/control_demo/params.c

@@ -6,31 +6,31 @@
 // 16 is max name length
 
 // gyro low pass filter
-PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq
-PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq
-PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
 
 // yaw washout
 PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
 
 // stabilization mode
-PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
+PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
-PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
-PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
+PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
 
 //  psi -> phi -> p
-PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f);      // heading 2 roll
+PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f);      // heading 2 roll
-PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f);        // roll to roll rate
+PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f);        // roll to roll rate
-PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f);  // roll limit
+PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f);  // roll limit, 28 deg
 
 // velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
-PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
 
 
 // theta -> q
@@ -41,15 +41,15 @@ PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
 
 //  h -> thr
-PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
 
 // crosstrack
-PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
 
 // speed command
 PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
@@ -58,6 +58,6 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
 
 // trim
 PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
 PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);

apps/examples/kalman_demo/KalmanNav.cpp

@@ -42,10 +42,12 @@
 #include "KalmanNav.hpp"
 
 // constants
+// Titterton pg. 52
 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
-static const float R = 6.371000e6f; // earth radius, m
+static const float R0 = 6378137.0f; // earth radius, m
-static const float RSq = 4.0589641e13f; // radius squared
+
-static const float g = 9.8f; // gravitational accel. m/s^2
+static const float RSq = 4.0680631591e+13; // earth radius squared
+static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
@@ -57,15 +59,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// attitude measurement ekf matrices
 	HAtt(6, 9),
 	RAtt(6, 6),
-	// gps measurement ekf matrices
+	// position measurement ekf matrices
-	HGps(6, 9),
+	HPos(5, 9),
-	RGps(6, 6),
+	RPos(5, 5),
 	// attitude representations
 	C_nb(),
 	q(),
 	// subscriptions
-	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
+	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz
-	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
+	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
 	// publications
 	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
@@ -78,6 +80,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_outTimeStamp(hrt_absolute_time()),
 	// frame count
 	_navFrames(0),
+	// miss counts
+	_missFast(0),
+	_missSlow(0),
 	// state
 	fN(0), fE(0), fD(0),
 	phi(0), theta(0), psi(0),
@@ -87,8 +92,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_vGyro(this, "V_GYRO"),
 	_vAccel(this, "V_ACCEL"),
 	_rMag(this, "R_MAG"),
-	_rGpsV(this, "R_GPS_V"),
+	_rGpsVel(this, "R_GPS_VEL"),
-	_rGpsGeo(this, "R_GPS_GEO"),
+	_rGpsPos(this, "R_GPS_POS"),
 	_rGpsAlt(this, "R_GPS_ALT"),
 	_rAccel(this, "R_ACCEL")
 {
@@ -99,12 +104,21 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 
 	// wait for gps lock
 	while (1) {
-		updateSubscriptions();
+		struct pollfd fds[1];
+		fds[0].fd = _gps.getHandle();
+		fds[0].events = POLLIN;
 
-		if (_gps.fix_type > 2) break;
+		// poll 10 seconds for new data
+		int ret = poll(fds, 1, 10000);
 
-		printf("[kalman_demo] waiting for gps lock\n");
+		if (ret > 0)  {
-		usleep(1000000);
+			updateSubscriptions();
+
+			if (_gps.fix_type > 2) break;
+
+		} else if (ret == 0) {
+			printf("[kalman_demo] waiting for gps lock\n");
+		}
 	}
 
 	// initial state
@@ -124,16 +138,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// initialize dcm
 	C_nb = Dcm(q);
 
-	// initialize F to identity
+	// HPos is constant
-	F = Matrix::identity(9);
+	HPos(0, 3) = 1.0f;
-
+	HPos(1, 4) = 1.0f;
-	// HGps is constant
+	HPos(2, 6) = 1.0f;
-	HGps(0, 3) = 1.0f;
+	HPos(3, 7) = 1.0f;
-	HGps(1, 4) = 1.0f;
+	HPos(4, 8) = 1.0f;
-	HGps(2, 5) = 1.0f;
-	HGps(3, 6) = 1.0f;
-	HGps(4, 7) = 1.0f;
-	HGps(5, 8) = 1.0f;
 
 	// initialize all parameters
 	updateParams();
@@ -143,11 +153,13 @@ void KalmanNav::update()
 {
 	using namespace math;
 
-	struct pollfd fds[2];
+	struct pollfd fds[3];
 	fds[0].fd = _sensors.getHandle();
 	fds[0].events = POLLIN;
 	fds[1].fd = _param_update.getHandle();
 	fds[1].events = POLLIN;
+	fds[2].fd = _gps.getHandle();
+	fds[2].events = POLLIN;
 
 	// poll 20 milliseconds for new data
 	int ret = poll(fds, 2, 20);
@@ -158,53 +170,55 @@ void KalmanNav::update()
 		return;
 
 	} else if (ret == 0) { // timeout
-		// run anyway
+		return;
-	} else if (ret > 0) {
-		// update params when requested
-		if (fds[1].revents & POLLIN) {
-			printf("updating params\n");
-			updateParams();
-		}
-
-		// if no new sensor data, return
-		if (!(fds[0].revents & POLLIN)) return;
 	}
 
 	// get new timestamp
 	uint64_t newTimeStamp = hrt_absolute_time();
 
 	// check updated subscriptions
+	if (_param_update.updated()) updateParams();
+
 	bool gpsUpdate = _gps.updated();
+	bool sensorsUpdate = _sensors.updated();
 
 	// get new information from subscriptions
 	// this clears update flag
 	updateSubscriptions();
 
-	// count fast frames
+	// abort update if no new data
-	_navFrames += 1;
+	if (!(sensorsUpdate || gpsUpdate)) return;
 
 	// fast prediciton step
 	// note, using sensors timestamp so we can account
 	// for packet lag
 	float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
 	_fastTimeStamp = _sensors.timestamp;
-	predictFast(dtFast);
+
+	if (dtFast < 1.0f / 50) {
+		predictFast(dtFast);
+		// count fast frames
+		_navFrames += 1;
+
+	} else _missFast++;
 
 	// slow prediction step
 	float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
 
-	if (dtSlow > 1.0f / 200) { // 200 Hz
+	if (dtSlow > 1.0f / 100) { // 100 Hz
 		_slowTimeStamp = _sensors.timestamp;
-		predictSlow(dtSlow);
+
+		if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
+		else _missSlow ++;
 	}
 
 	// gps correction step
 	if (gpsUpdate) {
-		correctGps();
+		correctPos();
 	}
 
 	// attitude correction step
-	if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
+	if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
 	}
@@ -218,8 +232,10 @@ void KalmanNav::update()
 	// output
 	if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
 		_outTimeStamp = newTimeStamp;
-		printf("nav: %4d Hz\n", _navFrames);
+		printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow);
 		_navFrames = 0;
+		_missFast = 0;
+		_missSlow = 0;
 	}
 }
 
@@ -298,21 +314,28 @@ void KalmanNav::predictFast(float dt)
 	// trig
 	float sinL = sinf(lat);
 	float cosL = cosf(lat);
+	float cosLSing = cosf(lat);
+
+	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
 
 	// position update
 	// neglects angular deflections in local gravity
 	// see Titerton pg. 70
-	float LDot = vN / (R + float(alt));
+	float R = R0 + float(alt);
-	float lDot = vE / (cosL * (R + float(alt)));
+	float LDot = vN / R;
-	float vNDot = fN - vE * (2 * omega +
+	float lDot = vE / (cosLSing * R);
-				 lDot) * sinL +
+	float rotRate = 2 * omega + lDot;
+	float vNDot = fN - vE * rotRate * sinL +
 		      vD * LDot;
-	float vDDot = fD - vE * (2 * omega + lDot) * cosL -
+	float vDDot = fD - vE * rotRate * cosL -
 		      vN * LDot + g;
-	float vEDot = fE + vN * (2 * omega + lDot) * sinL +
+	float vEDot = fE + vN * rotRate * sinL +
-		      vDDot * (2 * omega * cosL);
+		      vDDot * rotRate * cosL;
 
 	// rectangular integration
+	//printf("dt: %8.4f\n", double(dt));
+	//printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
+	//printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
 	vN += vNDot * dt;
 	vE += vEDot * dt;
 	vD += vDDot * dt;
@@ -331,87 +354,88 @@ void KalmanNav::predictSlow(float dt)
 	float cosLSq = cosL * cosL;
 	float tanL = tanf(lat);
 
+	// prepare for matrix
+	float R = R0 + float(alt);
+
 	// F Matrix
 	// Titterton pg. 291
-	//
-	// difference from Jacobian
-	// multiplity by dt for all elements
-	// add 1.0 to diagonal elements
 
-	F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt;
+	F(0, 1) = -(omega * sinL + vE * tanL / R);
-	F(0, 2) = (vN / R) * dt;
+	F(0, 2) = vN / R;
-	F(0, 4) = (1.0f / R) * dt;
+	F(0, 4) = 1.0f / R;
-	F(0, 6) = (-omega * sinL) * dt;
+	F(0, 6) = -omega * sinL;
-	F(0, 8) = (-vE / RSq) * dt;
+	F(0, 8) = -vE / RSq;
 
-	F(1, 0) = (omega * sinL + vE * tanL / R) * dt;
+	F(1, 0) = omega * sinL + vE * tanL / R;
-	F(1, 2) = (omega * cosL + vE / R) * dt;
+	F(1, 2) = omega * cosL + vE / R;
-	F(1, 3) = (-1.0f / R) * dt;
+	F(1, 3) = -1.0f / R;
-	F(1, 8) = (vN / RSq) * dt;
+	F(1, 8) = vN / RSq;
 
-	F(2, 0) = (-vN / R) * dt;
+	F(2, 0) = -vN / R;
-	F(2, 1) = (-omega * cosL - vE / R) * dt;
+	F(2, 1) = -omega * cosL - vE / R;
-	F(2, 4) = (-tanL / R) * dt;
+	F(2, 4) = -tanL / R;
-	F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt;
+	F(2, 6) = -omega * cosL - vE / (R * cosLSq);
-	F(2, 8) = (vE * tanL / RSq) * dt;
+	F(2, 8) = vE * tanL / RSq;
 
-	F(3, 1) = (-fD) * dt;
+	F(3, 1) = -fD;
-	F(3, 2) = (fE) * dt;
+	F(3, 2) = fE;
-	F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal
+	F(3, 3) = vD / R;
-	F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt;
+	F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
-	F(3, 5) = (vN / R) * dt;
+	F(3, 5) = vN / R;
-	F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt;
+	F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
-	F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt;
+	F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
 
-	F(4, 0) = (fD) * dt;
+	F(4, 0) = fD;
-	F(4, 2) = (-fN) * dt;
+	F(4, 2) = -fN;
-	F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt;
+	F(4, 3) = 2 * omega * sinL + vE * tanL / R;
-	F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal
+	F(4, 4) = (vN * tanL + vD) / R;
-	F(4, 5) = (2 * omega * cosL + vE / R) * dt;
+	F(4, 5) = 2 * omega * cosL + vE / R;
-	F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) +
+	F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
-		   vN * vE / (R * cosLSq)) * dt;
+		  vN * vE / (R * cosLSq);
-	F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt;
+	F(4, 8) = -vE * (vN * tanL + vD) / RSq;
 
-	F(5, 0) = (-fE) * dt;
+	F(5, 0) = -fE;
-	F(5, 1) = (fN) * dt;
+	F(5, 1) = fN;
-	F(5, 3) = (-2 * vN / R) * dt;
+	F(5, 3) = -2 * vN / R;
-	F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt;
+	F(5, 4) = -2 * (omega * cosL + vE / R);
-	F(5, 6) = (2 * omega * vE * sinL) * dt;
+	F(5, 6) = 2 * omega * vE * sinL;
-	F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt;
+	F(5, 8) = (vN * vN + vE * vE) / RSq;
 
-	F(6, 3) = (1 / R) * dt;
+	F(6, 3) = 1 / R;
-	F(6, 8) = (-vN / RSq) * dt;
+	F(6, 8) = -vN / RSq;
 
-	F(7, 4) = (1 / (R * cosL)) * dt;
+	F(7, 4) = 1 / (R * cosL);
-	F(7, 6) = (vE * tanL / (R * cosL)) * dt;
+	F(7, 6) = vE * tanL / (R * cosL);
-	F(7, 8) = (-vE / (cosL * RSq)) * dt;
+	F(7, 8) = -vE / (cosL * RSq);
 
-	F(8, 5) = (-1) * dt;
+	F(8, 5) = -1;
 
 	// G Matrix
 	// Titterton pg. 291
-	G(0, 0) = -C_nb(0, 0) * dt;
+	G(0, 0) = -C_nb(0, 0);
-	G(0, 1) = -C_nb(0, 1) * dt;
+	G(0, 1) = -C_nb(0, 1);
-	G(0, 2) = -C_nb(0, 2) * dt;
+	G(0, 2) = -C_nb(0, 2);
-	G(1, 0) = -C_nb(1, 0) * dt;
+	G(1, 0) = -C_nb(1, 0);
-	G(1, 1) = -C_nb(1, 1) * dt;
+	G(1, 1) = -C_nb(1, 1);
-	G(1, 2) = -C_nb(1, 2) * dt;
+	G(1, 2) = -C_nb(1, 2);
-	G(2, 0) = -C_nb(2, 0) * dt;
+	G(2, 0) = -C_nb(2, 0);
-	G(2, 1) = -C_nb(2, 1) * dt;
+	G(2, 1) = -C_nb(2, 1);
-	G(2, 2) = -C_nb(2, 2) * dt;
+	G(2, 2) = -C_nb(2, 2);
 
-	G(3, 3) = C_nb(0, 0) * dt;
+	G(3, 3) = C_nb(0, 0);
-	G(3, 4) = C_nb(0, 1) * dt;
+	G(3, 4) = C_nb(0, 1);
-	G(3, 5) = C_nb(0, 2) * dt;
+	G(3, 5) = C_nb(0, 2);
-	G(4, 3) = C_nb(1, 0) * dt;
+	G(4, 3) = C_nb(1, 0);
-	G(4, 4) = C_nb(1, 1) * dt;
+	G(4, 4) = C_nb(1, 1);
-	G(4, 5) = C_nb(1, 2) * dt;
+	G(4, 5) = C_nb(1, 2);
-	G(5, 3) = C_nb(2, 0) * dt;
+	G(5, 3) = C_nb(2, 0);
-	G(5, 4) = C_nb(2, 1) * dt;
+	G(5, 4) = C_nb(2, 1);
-	G(5, 5) = C_nb(2, 2) * dt;
+	G(5, 5) = C_nb(2, 2);
 
-	// predict equations for kalman filter
+	// continuous predictioon equations
-	P = F * P * F.transpose() + G * V * G.transpose();
+	// for discrte time EKF
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
 }
 
 void KalmanNav::correctAtt()
@@ -428,12 +452,11 @@ void KalmanNav::correctAtt()
 
 	// mag measurement
 	Vector3 zMag(_sensors.magnetometer_ga);
-	zMag = zMag.unit();
 
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
 	//  TODO dip/dec depend on lat/ lon/ time
-	static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level
+	static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level
 	static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
 	float bN = cosf(dip) * cosf(dec);
 	float bE = cosf(dip) * sinf(dec);
@@ -443,10 +466,29 @@ void KalmanNav::correctAtt()
 
 	// accel measurement
 	Vector3 zAccel(_sensors.accelerometer_m_s2);
+	float accelMag = zAccel.norm();
 	zAccel = zAccel.unit();
 
+	// ignore accel correction when accel mag not close to g
+	Matrix RAttAdjust = RAtt;
+
+	bool ignoreAccel = fabsf(accelMag - g) > 1.1f;
+
+	if (ignoreAccel) {
+		RAttAdjust(3, 3) = 1.0e10;
+		RAttAdjust(4, 4) = 1.0e10;
+		RAttAdjust(5, 5) = 1.0e10;
+
+	} else {
+		//printf("correcting attitude with accel\n");
+	}
+
+	// account for banked turn
+	// this would only work for fixed wing, so try to avoid
+	//Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi);
+
 	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -1)).unit();
+	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -g) /*+ zCentrip*/).unit();
 
 	// combined measurement
 	Vector zAtt(6);
@@ -498,8 +540,9 @@ void KalmanNav::correctAtt()
 	HAtt(5, 1) = cosPhi * sinTheta;
 
 	// compute correction
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	Vector y = zAtt - zAttHat; // residual
-	Matrix S = HAtt * P * HAtt.transpose() + RAtt; // residual covariance
+	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
 	Matrix K = P * HAtt.transpose() * S.inverse();
 	Vector xCorrect = K * y;
 
@@ -510,24 +553,34 @@ void KalmanNav::correctAtt()
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in att correction\n");
+			// reset P matrix to identity
+			P = Matrix::identity(9) * 1.0f;
 			return;
 		}
 	}
 
 	// correct state
-	phi += xCorrect(PHI);
+	if (!ignoreAccel) {
-	theta += xCorrect(THETA);
+		phi += xCorrect(PHI);
+		theta += xCorrect(THETA);
+	}
+
 	psi += xCorrect(PSI);
 
+	// attitude also affects nav velocities
+	vN += xCorrect(VN);
+	vE += xCorrect(VE);
+	vD += xCorrect(VD);
+
 	// update state covariance
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	P = P - K * HAtt * P;
 
 	// fault detection
 	float beta = y.dot(S.inverse() * y);
-	printf("attitude: beta = %8.4f\n", (double)beta);
 
-	if (beta > 10.0f) {
+	if (beta > 1000.0f) {
-		//printf("fault in attitude: beta = %8.4f\n", (double)beta);
+		printf("fault in attitude: beta = %8.4f\n", (double)beta);
 		//printf("y:\n"); y.print();
 	}
 
@@ -536,20 +589,32 @@ void KalmanNav::correctAtt()
 	q = Quaternion(EulerAngles(phi, theta, psi));
 }
 
-void KalmanNav::correctGps()
+void KalmanNav::correctPos()
 {
 	using namespace math;
 	Vector y(6);
 	y(0) = _gps.vel_n - vN;
 	y(1) = _gps.vel_e - vE;
-	y(2) = _gps.vel_d - vD;
+	y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
-	y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
+	y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
-	y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
+	y(4) = double(_gps.alt) / 1.0e3 - alt;
-	y(5) = double(_gps.alt) / 1.0e3 - alt;
+
+	// update gps noise
+	float cosLSing = cosf(lat);
+	float R = R0 + float(alt);
+
+	// prevent singularity
+	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+
+	float noiseLat = _rGpsPos.get() / R;
+	float noiseLon = _rGpsPos.get() / (R * cosLSing);
+	RPos(2, 2) = noiseLat * noiseLat;
+	RPos(3, 3) = noiseLon * noiseLon;
 
 	// compute correction
-	Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix K = P * HGps.transpose() * S.inverse();
+	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
+	Matrix K = P * HPos.transpose() * S.inverse();
 	Vector xCorrect = K * y;
 
 	// check correction is sane
@@ -566,6 +631,8 @@ void KalmanNav::correctGps()
 			setLatDegE7(_gps.lat);
 			setLonDegE7(_gps.lon);
 			setAltE3(_gps.alt);
+			// reset P matrix to identity
+			P = Matrix::identity(9) * 1.0f;
 			return;
 		}
 	}
@@ -579,14 +646,14 @@ void KalmanNav::correctGps()
 	alt += double(xCorrect(ALT));
 
 	// update state covariance
-	P = P - K * HGps * P;
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	P = P - K * HPos * P;
 
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
-	printf("gps: beta = %8.4f\n", (double)beta);
 
-	if (beta > 100.0f) {
+	if (beta > 1000.0f) {
-		//printf("fault in gps: beta = %8.4f\n", (double)beta);
+		printf("fault in gps: beta = %8.4f\n", (double)beta);
 		//printf("y:\n"); y.print();
 	}
 }
@@ -597,6 +664,7 @@ void KalmanNav::updateParams()
 	using namespace control;
 	SuperBlock::updateParams();
 
+
 	// gyro noise
 	V(0, 0) = _vGyro.get();   // gyro x, rad/s
 	V(1, 1) = _vGyro.get();   // gyro y
@@ -608,20 +676,31 @@ void KalmanNav::updateParams()
 	V(5, 5) = _vAccel.get();   // accel z
 
 	// magnetometer noise
-	RAtt(0, 0) = _rMag.get(); // normalized direction
+	float noiseMagSq = _rMag.get() * _rMag.get();
-	RAtt(1, 1) = _rMag.get();
+	RAtt(0, 0) = noiseMagSq; // normalized direction
-	RAtt(2, 2) = _rMag.get();
+	RAtt(1, 1) = noiseMagSq;
+	RAtt(2, 2) = noiseMagSq;
 
 	// accelerometer noise
-	RAtt(3, 3) = _rAccel.get(); // normalized direction
+	float noiseAccelSq = _rAccel.get() * _rAccel.get();
-	RAtt(4, 4) = _rAccel.get();
+	RAtt(3, 3) = noiseAccelSq; // normalized direction
-	RAtt(5, 5) = _rAccel.get();
+	RAtt(4, 4) = noiseAccelSq;
+	RAtt(5, 5) = noiseAccelSq;
 
 	// gps noise
-	RGps(0, 0) = _rGpsV.get(); // vn, m/s
+	float cosLSing = cosf(lat);
-	RGps(1, 1) = _rGpsV.get(); // ve
+	float R = R0 + float(alt);
-	RGps(2, 2) = _rGpsV.get(); // vd
+
-	RGps(3, 3) = _rGpsGeo.get(); // L, rad
+	// prevent singularity
-	RGps(4, 4) = _rGpsGeo.get(); // l, rad
+	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
-	RGps(5, 5) = _rGpsAlt.get(); // h, m
+
+	float noiseVel = _rGpsVel.get();
+	float noiseLat = _rGpsPos.get() / R;
+	float noiseLon = _rGpsPos.get() / (R * cosLSing);
+	float noiseAlt = _rGpsAlt.get();
+	RPos(0, 0) = noiseVel * noiseVel; // vn
+	RPos(1, 1) = noiseVel * noiseVel; // ve
+	RPos(2, 2) = noiseLat * noiseLat; // lat
+	RPos(3, 3) = noiseLon * noiseLon; // lon
+	RPos(4, 4) = noiseAlt * noiseAlt; // h
 }

apps/examples/kalman_demo/KalmanNav.hpp

@@ -60,6 +60,11 @@
 #include <poll.h>
 #include <unistd.h>
 
+/**
+ * Kalman filter navigation class
+ * http://en.wikipedia.org/wiki/Extended_Kalman_filter
+ * Discrete-time extended Kalman filter
+ */
 class KalmanNav : public control::SuperBlock
 {
 public:
@@ -70,7 +75,7 @@ public:
 	void predictFast(float dt);
 	void predictSlow(float dt);
 	void correctAtt();
-	void correctGps();
+	void correctPos();
 	virtual void updateParams();
 protected:
 	math::Matrix F;
@@ -79,8 +84,8 @@ protected:
 	math::Matrix V;
 	math::Matrix HAtt;
 	math::Matrix RAtt;
-	math::Matrix HGps;
+	math::Matrix HPos;
-	math::Matrix RGps;
+	math::Matrix RPos;
 	math::Dcm C_nb;
 	math::Quaternion q;
 	control::UOrbSubscription<sensor_combined_s> _sensors;
@@ -94,6 +99,8 @@ protected:
 	uint64_t _attTimeStamp;
 	uint64_t _outTimeStamp;
 	uint16_t _navFrames;
+	uint16_t _missFast;
+	uint16_t _missSlow;
 	float fN, fE, fD;
 	// states
 	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
@@ -103,8 +110,8 @@ protected:
 	control::BlockParam<float> _vGyro;
 	control::BlockParam<float> _vAccel;
 	control::BlockParam<float> _rMag;
-	control::BlockParam<float> _rGpsV;
+	control::BlockParam<float> _rGpsVel;
-	control::BlockParam<float> _rGpsGeo;
+	control::BlockParam<float> _rGpsPos;
 	control::BlockParam<float> _rGpsAlt;
 	control::BlockParam<float> _rAccel;
 	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }

apps/examples/kalman_demo/params.c

@@ -3,8 +3,8 @@
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
 PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
 PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);

apps/mathlib/math/Dcm.cpp

@@ -52,6 +52,23 @@ Dcm::Dcm() :
 {
 }
 
+Dcm::Dcm(float c00, float c01, float c02,
+	 float c10, float c11, float c12,
+	 float c20, float c21, float c22) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	dcm(0, 0) = c00;
+	dcm(0, 1) = c01;
+	dcm(0, 2) = c02;
+	dcm(1, 0) = c10;
+	dcm(1, 1) = c11;
+	dcm(1, 2) = c12;
+	dcm(2, 0) = c20;
+	dcm(2, 1) = c21;
+	dcm(2, 2) = c22;
+}
+
 Dcm::Dcm(const float *data) :
 	Matrix(3, 3, data)
 {
@@ -61,22 +78,22 @@ Dcm::Dcm(const Quaternion &q) :
 	Matrix(3, 3)
 {
 	Dcm &dcm = *this;
-	float a = q.getA();
+	double a = q.getA();
-	float b = q.getB();
+	double b = q.getB();
-	float c = q.getC();
+	double c = q.getC();
-	float d = q.getD();
+	double d = q.getD();
-	float aSq = a * a;
+	double aSq = a * a;
-	float bSq = b * b;
+	double bSq = b * b;
-	float cSq = c * c;
+	double cSq = c * c;
-	float dSq = d * d;
+	double dSq = d * d;
 	dcm(0, 0) = aSq + bSq - cSq - dSq;
-	dcm(0, 1) = 2 * (b * c - a * d);
+	dcm(0, 1) = 2.0 * (b * c - a * d);
-	dcm(0, 2) = 2 * (a * c + b * d);
+	dcm(0, 2) = 2.0 * (a * c + b * d);
-	dcm(1, 0) = 2 * (b * c + a * d);
+	dcm(1, 0) = 2.0 * (b * c + a * d);
 	dcm(1, 1) = aSq - bSq + cSq - dSq;
-	dcm(1, 2) = 2 * (c * d - a * b);
+	dcm(1, 2) = 2.0 * (c * d - a * b);
-	dcm(2, 0) = 2 * (b * d - a * c);
+	dcm(2, 0) = 2.0 * (b * d - a * c);
-	dcm(2, 1) = 2 * (a * b + c * d);
+	dcm(2, 1) = 2.0 * (a * b + c * d);
 	dcm(2, 2) = aSq - bSq - cSq + dSq;
 }
 
@@ -84,12 +101,12 @@ Dcm::Dcm(const EulerAngles &euler) :
 	Matrix(3, 3)
 {
 	Dcm &dcm = *this;
-	float cosPhi = cosf(euler.getPhi());
+	double cosPhi = cos(euler.getPhi());
-	float sinPhi = sinf(euler.getPhi());
+	double sinPhi = sin(euler.getPhi());
-	float cosThe = cosf(euler.getTheta());
+	double cosThe = cos(euler.getTheta());
-	float sinThe = sinf(euler.getTheta());
+	double sinThe = sin(euler.getTheta());
-	float cosPsi = cosf(euler.getPsi());
+	double cosPsi = cos(euler.getPsi());
-	float sinPsi = sinf(euler.getPsi());
+	double sinPsi = sin(euler.getPsi());
 
 	dcm(0, 0) = cosThe * cosPsi;
 	dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
@@ -116,18 +133,30 @@ Dcm::~Dcm()
 int __EXPORT dcmTest()
 {
 	printf("Test DCM\t\t: ");
+	// default ctor
+	ASSERT(matrixEqual(Dcm(),
+			   Matrix::identity(3)));
+	// quaternion ctor
+	ASSERT(matrixEqual(
+		       Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// euler angle ctor
+	ASSERT(matrixEqual(
+		       Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// rotations
 	Vector3 vB(1, 2, 3);
-	ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
+	ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
-			   Matrix::identity(3)));
+			   Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
-	ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Matrix::identity(3)));
+			   Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(-2, 1, 3),
+	ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
-			   Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
+			   Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(3, 2, -1),
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Dcm(EulerAngles(0, M_PI_2_F, 0))*vB));
-	ASSERT(vectorEqual(Vector3(1, -3, 2),
-			   Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB));
-	ASSERT(vectorEqual(Vector3(3, 2, -1),
 			   Dcm(EulerAngles(
 				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
 	printf("PASS\n");

apps/mathlib/math/Dcm.hpp

@@ -64,6 +64,13 @@ public:
 	 */
 	Dcm();
 
+	/**
+	 * scalar ctor
+	 */
+	Dcm(float c00, float c01, float c02,
+	    float c10, float c11, float c12,
+	    float c20, float c21, float c22);
+
 	/**
 	 * data ctor
 	 */

apps/mathlib/math/EulerAngles.cpp

@@ -97,23 +97,27 @@ EulerAngles::~EulerAngles()
 int __EXPORT eulerAnglesTest()
 {
 	printf("Test EulerAngles\t: ");
-	EulerAngles euler(1, 2, 3);
+	EulerAngles euler(0.1f, 0.2f, 0.3f);
 
 	// test ctor
-	ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-	ASSERT(equal(euler.getPhi(), 1));
+	ASSERT(equal(euler.getPhi(), 0.1f));
-	ASSERT(equal(euler.getTheta(), 2));
+	ASSERT(equal(euler.getTheta(), 0.2f));
-	ASSERT(equal(euler.getPsi(), 3));
+	ASSERT(equal(euler.getPsi(), 0.3f));
 
 	// test dcm ctor
+	euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+
+	// test quat ctor
+	euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
 
 	// test assignment
-	euler.setPhi(4);
+	euler.setPhi(0.4f);
-	ASSERT(equal(euler.getPhi(), 4));
+	euler.setTheta(0.5f);
-	euler.setTheta(5);
+	euler.setPsi(0.6f);
-	ASSERT(equal(euler.getTheta(), 5));
+	ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
-	euler.setPsi(6);
-	ASSERT(equal(euler.getPsi(), 6));
 
 	printf("PASS\n");
 	return 0;

apps/mathlib/math/Quaternion.cpp

@@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) :
 Quaternion::Quaternion(const Dcm &dcm) :
 	Vector(4)
 {
-	setA(0.5f * sqrtf(1 + dcm(0, 0) +
+	// avoiding singularities by not using
-			  dcm(1, 1) + dcm(2, 2)));
+	// division equations
-	setB((dcm(2, 1) - dcm(1, 2)) /
+	setA(0.5 * sqrt(1.0 +
-	     (4 * getA()));
+			double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
-	setC((dcm(0, 2) - dcm(2, 0)) /
+	setB(0.5 * sqrt(1.0 +
-	     (4 * getA()));
+			double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
-	setD((dcm(1, 0) - dcm(0, 1)) /
+	setC(0.5 * sqrt(1.0 +
-	     (4 * getA()));
+			double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
+	setD(0.5 * sqrt(1.0 +
+			double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
 }
 
 Quaternion::Quaternion(const EulerAngles &euler) :
 	Vector(4)
 {
-	float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
+	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
-	float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
+	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
-	float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
+	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
-	float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
+	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
-	float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
+	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
-	float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
+	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
 	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
 	     sinPhi_2 * sinTheta_2 * sinPsi_2);
 	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
 	     cosPhi_2 * sinTheta_2 * sinPsi_2);
 	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
 	     sinPhi_2 * cosTheta_2 * sinPsi_2);
-	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
+	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
 	     sinPhi_2 * sinTheta_2 * cosPsi_2);
 }
 
@@ -142,38 +144,29 @@ int __EXPORT quaternionTest()
 	printf("Test Quaternion\t\t: ");
 	// test default ctor
 	Quaternion q;
-	ASSERT(equal(q.getA(), 1));
+	ASSERT(equal(q.getA(), 1.0f));
-	ASSERT(equal(q.getB(), 0));
+	ASSERT(equal(q.getB(), 0.0f));
-	ASSERT(equal(q.getC(), 0));
+	ASSERT(equal(q.getC(), 0.0f));
-	ASSERT(equal(q.getD(), 0));
+	ASSERT(equal(q.getD(), 0.0f));
 	// test float ctor
-	q = Quaternion(0, 1, 0, 0);
+	q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
-	ASSERT(equal(q.getA(), 0));
+	ASSERT(equal(q.getA(), 0.1825742f));
-	ASSERT(equal(q.getB(), 1));
+	ASSERT(equal(q.getB(), 0.3651484f));
-	ASSERT(equal(q.getC(), 0));
+	ASSERT(equal(q.getC(), 0.5477226f));
-	ASSERT(equal(q.getD(), 0));
+	ASSERT(equal(q.getD(), 0.7302967f));
 	// test euler ctor
-	q = Quaternion(EulerAngles(0, 0, 0));
+	q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(equal(q.getA(), 1));
+	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
-	ASSERT(equal(q.getB(), 0));
-	ASSERT(equal(q.getC(), 0));
-	ASSERT(equal(q.getD(), 0));
 	// test dcm ctor
 	q = Quaternion(Dcm());
-	ASSERT(equal(q.getA(), 1));
+	ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
-	ASSERT(equal(q.getB(), 0));
-	ASSERT(equal(q.getC(), 0));
-	ASSERT(equal(q.getD(), 0));
 	// TODO test derivative
 	// test accessors
-	q.setA(0.1);
+	q.setA(0.1f);
-	q.setB(0.2);
+	q.setB(0.2f);
-	q.setC(0.3);
+	q.setC(0.3f);
-	q.setD(0.4);
+	q.setD(0.4f);
-	ASSERT(equal(q.getA(), 0.1));
+	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
-	ASSERT(equal(q.getB(), 0.2));
-	ASSERT(equal(q.getC(), 0.3));
-	ASSERT(equal(q.getD(), 0.4));
 	printf("PASS\n");
 	return 0;
 }

apps/mathlib/math/test_math.sce

@@ -0,0 +1,63 @@
+clc
+clear
+function out = float_truncate(in, digits)
+    out = round(in*10^digits)
+    out = out/10^digits
+endfunction
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+
+cosPhi = cos(phi)
+cosPhi_2 = cos(phi/2)
+sinPhi = sin(phi)
+sinPhi_2 = sin(phi/2)
+
+cosTheta = cos(theta)
+cosTheta_2 = cos(theta/2)
+sinTheta = sin(theta)
+sinTheta_2 = sin(theta/2)
+
+cosPsi = cos(psi)
+cosPsi_2 = cos(psi/2)
+sinPsi = sin(psi)
+sinPsi_2 = sin(psi/2)
+
+C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
+        cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
+        -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
+    
+disp(C_nb)
+//C_nb = float_truncate(C_nb,3)
+//disp(C_nb)
+
+theta = asin(-C_nb(3,1))
+phi = atan(C_nb(3,2), C_nb(3,3))
+psi = atan(C_nb(2,1), C_nb(1,1))
+printf('phi %f\n', phi)
+printf('theta %f\n', theta)
+printf('psi %f\n', psi)
+
+q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
+     sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
+     cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
+     cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
+     
+//q = float_truncate(q,3)
+
+a = q(1)
+b = q(2)
+c = q(3)
+d = q(4)
+printf('q: %f %f %f %f\n', a, b, c, d)
+a2 = a*a
+b2 = b*b
+c2 = c*c
+d2 = d*d
+
+C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
+         2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
+         2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
+
+disp(C2_nb)

apps/mavlink/mavlink.c

@@ -77,7 +77,7 @@
 /* define MAVLink specific parameters */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
-PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_QUADROTOR);
+PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
 
 __EXPORT int mavlink_main(int argc, char *argv[]);
 
@@ -98,7 +98,7 @@ static bool mavlink_link_termination_allowed = false;
 mavlink_system_t mavlink_system = {
 	100,
 	50,
-	MAV_TYPE_QUADROTOR,
+	MAV_TYPE_FIXED_WING,
 	0,
 	0,
 	0
@@ -148,6 +148,10 @@ set_hil_on_off(bool hil_enabled)
 		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 
+		/* sensore level hil */
+		pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+		pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+
 		mavlink_hil_enabled = true;
 
 		/* ramp up some HIL-related subscriptions */

apps/mavlink/mavlink_hil.h

@@ -43,8 +43,12 @@ extern bool mavlink_hil_enabled;
 
 extern struct vehicle_global_position_s hil_global_pos;
 extern struct vehicle_attitude_s hil_attitude;
+extern struct sensor_combined_s hil_sensors;
+extern struct vehicle_gps_position_s hil_gps;
 extern orb_advert_t pub_hil_global_pos;
 extern orb_advert_t pub_hil_attitude;
+extern orb_advert_t pub_hil_sensors;
+extern orb_advert_t pub_hil_gps;
 
 /**
  * Enable / disable Hardware in the Loop simulation mode.
@@ -54,4 +58,4 @@ extern orb_advert_t pub_hil_attitude;
  *			requested change could not be made or was
  *			redundant.
  */
-extern int set_hil_on_off(bool hil_enabled);
+extern int set_hil_on_off(bool hil_enabled);

apps/mavlink/mavlink_receiver.c

@@ -86,8 +86,12 @@ static struct offboard_control_setpoint_s offboard_control_sp;
 
 struct vehicle_global_position_s hil_global_pos;
 struct vehicle_attitude_s hil_attitude;
+struct vehicle_gps_position_s hil_gps;
+struct sensor_combined_s hil_sensors;
 orb_advert_t pub_hil_global_pos = -1;
 orb_advert_t pub_hil_attitude = -1;
+orb_advert_t pub_hil_gps = -1;
+orb_advert_t pub_hil_sensors = -1;
 
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -302,6 +306,161 @@ handle_message(mavlink_message_t *msg)
 
 	if (mavlink_hil_enabled) {
 
+		uint64_t timestamp = hrt_absolute_time();
+
+		if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
+
+			mavlink_raw_imu_t imu;
+			mavlink_msg_raw_imu_decode(msg, &imu);
+
+			/* packet counter */
+			static uint16_t hil_counter = 0;
+			static uint16_t hil_frames = 0;
+			static uint64_t old_timestamp = 0;
+
+			/* hil gyro */
+			static const float mrad2rad = 1.0e-3f;
+			hil_sensors.timestamp = timestamp;
+			hil_sensors.gyro_counter = hil_counter;
+			hil_sensors.gyro_raw[0] = imu.xgyro;
+			hil_sensors.gyro_raw[1] = imu.ygyro;
+			hil_sensors.gyro_raw[2] = imu.zgyro;
+			hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
+			hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
+			hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
+
+			/* accelerometer */
+			hil_sensors.accelerometer_counter = hil_counter;
+			static const float mg2ms2 = 9.8f / 1000.0f;
+			hil_sensors.accelerometer_raw[0] = imu.xacc;
+			hil_sensors.accelerometer_raw[1] = imu.yacc;
+			hil_sensors.accelerometer_raw[2] = imu.zacc;
+			hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
+			hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
+			hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
+			hil_sensors.accelerometer_mode = 0; // TODO what is this?
+			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
+
+			/* adc */
+			hil_sensors.adc_voltage_v[0] = 0;
+			hil_sensors.adc_voltage_v[1] = 0;
+			hil_sensors.adc_voltage_v[2] = 0;
+
+			/* magnetometer */
+			float mga2ga = 1.0e-3f;
+			hil_sensors.magnetometer_counter = hil_counter;
+			hil_sensors.magnetometer_raw[0] = imu.xmag;
+			hil_sensors.magnetometer_raw[1] = imu.ymag;
+			hil_sensors.magnetometer_raw[2] = imu.zmag;
+			hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
+			hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
+			hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
+			hil_sensors.magnetometer_range_ga = 32.7f; // int16
+			hil_sensors.magnetometer_mode = 0; // TODO what is this
+			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+
+			/* publish */
+			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
+			// increment counters
+			hil_counter += 1 ;
+			hil_frames += 1 ;
+
+			// output
+			if ((timestamp - old_timestamp) > 1000000) {
+				printf("receiving hil imu at %d hz\n", hil_frames);
+				old_timestamp = timestamp;
+				hil_frames = 0;
+			}
+		}
+
+		if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
+
+			mavlink_gps_raw_int_t gps;
+			mavlink_msg_gps_raw_int_decode(msg, &gps);
+
+			/* packet counter */
+			static uint16_t hil_counter = 0;
+			static uint16_t hil_frames = 0;
+			static uint64_t old_timestamp = 0;
+
+			/* gps */
+			hil_gps.timestamp = gps.time_usec;
+			hil_gps.counter = hil_counter++;
+			hil_gps.time_gps_usec = gps.time_usec;
+			hil_gps.lat = gps.lat;
+			hil_gps.lon = gps.lon;
+			hil_gps.alt = gps.alt;
+			hil_gps.counter_pos_valid = hil_counter++;
+			hil_gps.eph = gps.eph;
+			hil_gps.epv = gps.epv;
+			hil_gps.s_variance = 100;
+			hil_gps.p_variance = 100;
+			hil_gps.vel = gps.vel;
+			hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
+			hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
+			hil_gps.vel_d = 0.0f;
+			hil_gps.cog = gps.cog;
+			hil_gps.fix_type = gps.fix_type;
+			hil_gps.satellites_visible = gps.satellites_visible;
+
+			/* publish */
+			orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
+
+			// increment counters
+			hil_counter += 1 ;
+			hil_frames += 1 ;
+
+			// output
+			if ((timestamp - old_timestamp) > 1000000) {
+				printf("receiving hil gps at %d hz\n", hil_frames);
+				old_timestamp = timestamp;
+				hil_frames = 0;
+			}
+		}
+
+		if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
+
+			mavlink_raw_pressure_t press;
+			mavlink_msg_raw_pressure_decode(msg, &press);
+
+			/* packet counter */
+			static uint16_t hil_counter = 0;
+			static uint16_t hil_frames = 0;
+			static uint64_t old_timestamp = 0;
+
+			/* baro */
+			/* TODO, set ground_press/ temp during calib */
+			static const float ground_press = 1013.25f; // mbar
+			static const float ground_tempC = 21.0f;
+			static const float ground_alt = 0.0f;
+			static const float T0 = 273.15;
+			static const float R = 287.05f;
+			static const float g = 9.806f;
+
+			float tempC =  press.temperature / 100.0f;
+			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
+			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
+			hil_sensors.baro_counter = hil_counter;
+			hil_sensors.baro_pres_mbar = press.press_abs;
+			hil_sensors.baro_alt_meter = h;
+			hil_sensors.baro_temp_celcius = tempC;
+
+			/* publish */
+			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
+			// increment counters
+			hil_counter += 1 ;
+			hil_frames += 1 ;
+
+			// output
+			if ((timestamp - old_timestamp) > 1000000) {
+				printf("receiving hil pressure at %d hz\n", hil_frames);
+				old_timestamp = timestamp;
+				hil_frames = 0;
+			}
+		}
+
 		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
 
 			mavlink_hil_state_t hil_state;
@@ -412,7 +571,7 @@ receive_thread(void *arg)
 	int uart_fd = *((int *)arg);
 
 	const int timeout = 1000;
-	uint8_t ch;
+	uint8_t buf[32];
 
 	mavlink_message_t msg;
 
@@ -423,13 +582,12 @@ receive_thread(void *arg)
 		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
 
 		if (poll(fds, 1, timeout) > 0) {
-			/* non-blocking read until buffer is empty */
+			/* non-blocking read */
-			int nread = 0;
+			size_t nread = read(uart_fd, buf, sizeof(buf));
+			ASSERT(nread > 0)
 
-			do {
+			for (size_t i = 0; i < nread; i++) {
-				nread = read(uart_fd, &ch, 1);
+				if (mavlink_parse_char(chan, buf[i], &msg, &status)) { //parse the char
-
-				if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
@@ -439,7 +597,7 @@ receive_thread(void *arg)
 					/* Handle packet with parameter component */
 					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
 				}
-			} while (nread > 0);
+			}
 		}
 	}
 
@@ -452,6 +610,10 @@ receive_start(int uart)
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
+	// set to non-blocking read
+	int flags = fcntl(uart, F_GETFL, 0);
+	fcntl(uart, F_SETFL, flags | O_NONBLOCK);
+
 	struct sched_param param;
 	param.sched_priority = SCHED_PRIORITY_MAX - 40;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);