Merge branch 'master' into sdlog2

src/modules/att_pos_estimator_ekf/KalmanNav.cpp

@@ -683,7 +683,8 @@ int KalmanNav::correctPos()
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
 
-	if (beta > _faultPos.get()) {
+	static int counter = 0;
+	if (beta > _faultPos.get() && (counter % 10 == 0)) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
 		printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
 		       double(y(0) / sqrtf(RPos(0, 0))),
@@ -693,6 +694,7 @@ int KalmanNav::correctPos()
 		       double(y(4) / sqrtf(RPos(4, 4))),
 		       double(y(5) / sqrtf(RPos(5, 5))));
 	}
+	counter++;
 
 	return ret_ok;
 }

src/modules/px4iofirmware/mixer.cpp

@@ -85,6 +85,9 @@ static int	mixer_callback(uintptr_t handle,
 
 static MixerGroup mixer_group(mixer_callback, 0);
 
+/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
+static void mixer_set_failsafe();
+
 void
 mixer_tick(void)
 {
@@ -102,13 +105,16 @@ mixer_tick(void)
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 	}
 
+	/* default to failsafe mixing */
 	source = MIX_FAILSAFE;
 
 	/*
 	 * Decide which set of controls we're using.
 	 */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
-		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
+	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 		/* don't actually mix anything - we already have raw PWM values or
 		 not a valid mixer. */
@@ -117,6 +123,7 @@ mixer_tick(void)
 	} else {
 
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 			/* mix from FMU controls */
@@ -132,15 +139,29 @@ mixer_tick(void)
 		}
 	}
 
+	/*
+	 * Set failsafe status flag depending on mixing source
+	 */
+	if (source == MIX_FAILSAFE) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
+	} else {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
+	}
+
 	/*
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
 
 		/* copy failsafe values to the servo outputs */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
+			/* safe actuators for FMU feedback */
+			r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
+		}
+
+
 	} else if (source != MIX_NONE) {
 
 		float	outputs[IO_SERVO_COUNT];
@@ -156,7 +177,7 @@ mixer_tick(void)
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
 			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
+			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
 
 		}
 		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
@@ -175,7 +196,7 @@ mixer_tick(void)
 	bool should_arm = (
 	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
+		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
 		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
 		/* FMU is available or FMU is not available but override is an option */
 		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
@@ -225,7 +246,7 @@ mixer_callback(uintptr_t handle,
 
 	case MIX_FAILSAFE:
 	case MIX_NONE:
-		/* XXX we could allow for configuration of per-output failsafe values */
+		control = 0.0f;
 		return -1;
 	}
 
@@ -303,8 +324,41 @@ mixer_handle_text(const void *buffer, size_t length)
 				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
 
 			mixer_text_length = resid;
+
+			/* update failsafe values */
+			mixer_set_failsafe();
 		}
 
 		break;
 	}
 }
+
+static void
+mixer_set_failsafe()
+{
+	/* 
+	 * Check if a custom failsafe value has been written,
+	 * else use the opportunity to set decent defaults.
+	 */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+		return;
+
+	float	outputs[IO_SERVO_COUNT];
+	unsigned mixed;
+
+	/* mix */
+	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+	/* scale to PWM and update the servo outputs as required */
+	for (unsigned i = 0; i < mixed; i++) {
+
+		/* scale to servo output */
+		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
+
+	}
+
+	/* disable the rest of the outputs */
+	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		r_page_servo_failsafe[i] = 0;
+
+}

src/modules/px4iofirmware/protocol.h

@@ -85,7 +85,7 @@
 #define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
 #define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
-#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* harcoded # of relay outputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT		8	/* hardcoded # of relay outputs */
 
 /* dynamic status page */
 #define PX4IO_PAGE_STATUS		1
@@ -104,6 +104,7 @@
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
+#define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
@@ -148,6 +149,7 @@
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
 #define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */

src/modules/px4iofirmware/registers.c

@@ -41,6 +41,7 @@
 
 #include <stdbool.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -178,8 +179,10 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  * PAGE 105
  *
  * Failsafe servo PWM values
+ * 
+ * Disable pulses as default.
  */
-uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT];
+uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
 
 void
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@@ -230,11 +233,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_FAILSAFE_PWM:
 
 		/* copy channel data */
-		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_servo_failsafe[offset] = *values;
 
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
+
 			offset++;
 			num_values--;
 			values++;

src/modules/systemlib/geo/geo.c

@@ -185,12 +185,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
 
-	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
+	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
 	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
 
 	const double radius_earth = 6371000.0d;
-
-	return radius_earth * c;
+	return radius_earth * c; 
 }
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)

src/modules/systemlib/geo/geo.h

@@ -82,8 +82,24 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
  */
 __EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
 
+/**
+ * Returns the distance to the next waypoint in meters.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
+/**
+ * Returns the bearing to the next waypoint in radians.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);