Merge branch 'master' of https://github.com/PX4/Firmware

apps/drivers/drv_rc_input.h

@@ -57,15 +57,23 @@
 #define RC_INPUT_DEVICE_PATH	"/dev/input_rc"
 
 /**
- * Maximum number of R/C input channels in the system.
+ * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
  */
-#define RC_INPUT_MAX_CHANNELS	16
+#define RC_INPUT_MAX_CHANNELS	18
 
 /**
  * Input signal type, value is a control position from zero to 100
  * percent.
  */
-typedef uint8_t		rc_input_t;
+typedef uint16_t		rc_input_t;
+
+enum RC_INPUT_SOURCE {
+	RC_INPUT_SOURCE_UNKNOWN = 0,
+	RC_INPUT_SOURCE_PX4FMU_PPM,
+	RC_INPUT_SOURCE_PX4IO_PPM,
+	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
+	RC_INPUT_SOURCE_PX4IO_SBUS
+};
 
 /**
  * R/C input status structure.
@@ -74,10 +82,16 @@ typedef uint8_t		rc_input_t;
  * on the board involved.
  */
 struct rc_input_values {
+	/** decoding time */
+	uint64_t		timestamp;
+
 	/** number of channels actually being seen */
 	uint32_t		channel_count;
 
-	/** desired pulse widths for each of the supported channels */
+	/** Input source */
+	enum RC_INPUT_SOURCE 	input_source;
+
+	/** measured pulse widths for each of the supported channels */
 	rc_input_t		values[RC_INPUT_MAX_CHANNELS];
 };
 

apps/drivers/px4io/px4io.cpp

@@ -61,9 +61,10 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
-#include <systemlib/mixer/mixer.h>
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
 
+#include <systemlib/mixer/mixer.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/hx_stream.h>
 #include <systemlib/err.h>
@@ -87,6 +88,8 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
+	void			set_rx_mode(unsigned mode);
+
 private:
 	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
 
@@ -103,6 +106,9 @@ private:
 	int			_t_armed;	///< system armed control topic
 	actuator_armed_s	_armed;		///< system armed state
 
+	orb_advert_t 		_to_input_rc;	///< rc inputs from io
+	rc_input_values		_input_rc;	///< rc input values
+
 	orb_advert_t		_t_outputs;	///< mixed outputs topic
 	actuator_outputs_s	_outputs;	///< mixed outputs
 
@@ -114,6 +120,9 @@ private:
 	// XXX how should this work?
 
 	bool			_send_needed;	///< If true, we need to send a packet to IO
+	bool			_config_needed;	///< if true, we need to set a config update to IO
+
+	uint8_t			_rx_mode;	///< the current RX mode on IO
 
 	/**
 	 * Trampoline to the worker task
@@ -145,6 +154,11 @@ private:
 	 */
 	void			io_send();
 
+	/**
+	 * Send a config packet to PX4IO
+	 */
+	void			config_send();
+
 	/**
 	 * Mixer control callback; invoked to fetch a control from a specific
 	 * group/index during mixing.
@@ -176,7 +190,9 @@ PX4IO::PX4IO() :
 	_mixers(nullptr),
 	_primary_pwm_device(false),
 	_switch_armed(false),
-	_send_needed(false)
+	_send_needed(false),
+	_config_needed(false),
+	_rx_mode(RX_MODE_PPM_ONLY)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -305,9 +321,14 @@ PX4IO::task_main()
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
+	memset(&_outputs, 0, sizeof(_outputs));
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &_outputs);
 
+	/* advertise the rc inputs */
+	memset(&_input_rc, 0, sizeof(_input_rc));
+	_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
+
 	/* poll descriptor */
 	pollfd fds[3];
 	fds[0].fd = _serial_fd;
@@ -373,6 +394,12 @@ PX4IO::task_main()
 			_send_needed = false;
 			io_send();
 		}
+
+		/* send a config packet to IO if required */
+		if (_config_needed) {
+			_config_needed = false;
+			config_send();
+		}
 	}
 
 out:
@@ -438,7 +465,14 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 	}
 	_connected = true;
 
-	/* XXX handle R/C inputs here ... needs code sharing/library */
+	/* publish raw rc channel values from IO */
+	_input_rc.timestamp = hrt_absolute_time();
+	for (int i = 0; i < rep->channel_count; i++)
+	{
+		_input_rc.values[i] = rep->rc_channel[i];
+	}
+
+	orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
 
 	/* remember the latched arming switch state */
 	_switch_armed = rep->armed;
@@ -473,6 +507,20 @@ PX4IO::io_send()
 		debug("send error %d", ret);
 }
 
+void
+PX4IO::config_send()
+{
+	px4io_config	cfg;
+	int		ret;
+
+	cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
+	cfg.serial_rx_mode = _rx_mode;
+
+	ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
+	if (ret)
+		debug("config error %d", ret);
+}
+
 int
 PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
@@ -586,6 +634,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 	return ret;
 }
 
+void
+PX4IO::set_rx_mode(unsigned mode)
+{
+	if (mode != _rx_mode) {
+		_rx_mode = mode;
+		_config_needed = true;
+	}
+}
+
 extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 
 namespace
@@ -642,9 +699,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "test"))
-		test();
-
 	/* note, stop not currently implemented */
 
 	if (!strcmp(argv[1], "update")) {
@@ -690,5 +744,25 @@ px4io_main(int argc, char *argv[])
 		return ret;
 	}
 
-	errx(1, "need a verb, only support 'start' and 'update'");
+	if (!strcmp(argv[1], "rx_spektrum6")) {
+		if (g_dev == nullptr)
+			errx(1, "not started");
+		g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6);
+	}
+	if (!strcmp(argv[1], "rx_spektrum7")) {
+		if (g_dev == nullptr)
+			errx(1, "not started");
+		g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7);
+	}
+	if (!strcmp(argv[1], "rx_sbus")) {
+		if (g_dev == nullptr)
+			errx(1, "not started");
+		g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS);
+	}
+
+	if (!strcmp(argv[1], "test"))
+		test();
+
+
+	errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'");
 }

apps/drivers/stm32/drv_hrt.c

@@ -338,7 +338,7 @@ static void		hrt_call_invoke(void);
 /* decoded PPM buffer */
 #define PPM_MAX_CHANNELS	12
 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
-__EXPORT unsigned ppm_decoded_channels;
+__EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;
 
 /* PPM edge history */

apps/mavlink/mavlink.c

@@ -589,16 +589,15 @@ int mavlink_thread_main(int argc, char *argv[])
 		/* 2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
 	} else if (baudrate >= 115200) {
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
 		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
-		/* 10 Hz / 100 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
+		/* 5 Hz / 100 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
-		/* 1 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
+		/* 2 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 	} else if (baudrate >= 57600) {
 		/* 10 Hz / 100 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);

apps/mavlink/mavlink_receiver.c

@@ -397,10 +397,6 @@ handle_message(mavlink_message_t *msg)
 
 			rc_hil.timestamp = hrt_absolute_time();
 			rc_hil.chan_count = 4;
-			rc_hil.chan[0].raw = 1500 + man.x / 2;
-			rc_hil.chan[1].raw = 1500 + man.y / 2;
-			rc_hil.chan[2].raw = 1500 + man.r / 2;
-			rc_hil.chan[3].raw = 1500 + man.z / 2;
 
 			rc_hil.chan[0].scaled = man.x / 1000.0f;
 			rc_hil.chan[1].scaled = man.y / 1000.0f;

apps/mavlink/orb_listener.c

@@ -67,6 +67,7 @@ struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
 struct vehicle_status_s v_status;
 struct rc_channels_s rc;
+struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
 
 struct mavlink_subscriptions mavlink_subs;
@@ -99,6 +100,7 @@ static void	l_vehicle_attitude(struct listener *l);
 static void	l_vehicle_gps_position(struct listener *l);
 static void	l_vehicle_status(struct listener *l);
 static void	l_rc_channels(struct listener *l);
+static void	l_input_rc(struct listener *l);
 static void	l_global_position(struct listener *l);
 static void	l_local_position(struct listener *l);
 static void	l_global_position_setpoint(struct listener *l);
@@ -116,6 +118,7 @@ struct listener listeners[] = {
 	{l_vehicle_gps_position,	&mavlink_subs.gps_sub,		0},
 	{l_vehicle_status,		&status_sub,			0},
 	{l_rc_channels,			&rc_sub,			0},
+	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
 	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
 	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
 	{l_global_position_setpoint,	&mavlink_subs.spg_sub,		0},
@@ -274,21 +277,29 @@ l_rc_channels(struct listener *l)
 {
 	/* copy rc channels into local buffer */
 	orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
+	// XXX Add RC channels scaled message here
+}
+
+void
+l_input_rc(struct listener *l)
+{
+	/* copy rc channels into local buffer */
+	orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
 	
 	if (gcs_link)
 		/* Channels are sent in MAVLink main loop at a fixed interval */
 		mavlink_msg_rc_channels_raw_send(chan,
-					 rc.timestamp / 1000,
+					 rc_raw.timestamp / 1000,
 					 0,
-					 rc.chan[0].raw,
-					 rc.chan[1].raw,
-					 rc.chan[2].raw,
-					 rc.chan[3].raw,
-					 rc.chan[4].raw,
-					 rc.chan[5].raw,
-					 rc.chan[6].raw,
-					 rc.chan[7].raw,
-					 rc.rssi);
+					 (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
+					 (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
+					 (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
+					 (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
+					 (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
+					 (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
+					 (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
+					 (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
+					 255);
 }
 
 void
@@ -584,6 +595,10 @@ uorb_receive_start(void)
 	rc_sub = orb_subscribe(ORB_ID(rc_channels));
 	orb_set_interval(rc_sub, 100);			/* 10Hz updates */
 
+	/* --- RC RAW VALUE --- */
+	mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
+	orb_set_interval(mavlink_subs.input_rc_sub, 100);
+
 	/* --- GLOBAL POS VALUE --- */
 	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	orb_set_interval(mavlink_subs.global_pos_sub, 1000);	/* 1Hz active updates */

apps/mavlink/orb_topics.h

@@ -57,6 +57,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_rc_input.h>
 
 struct mavlink_subscriptions {
 	int sensor_sub;
@@ -75,6 +76,7 @@ struct mavlink_subscriptions {
 	int spl_sub;
 	int spg_sub;
 	int debug_key_value;
+	int input_rc_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;

apps/mavlink/waypoints.c

@@ -360,7 +360,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 		float dist = -1.0f;
 
 		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->alt);
+			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
@@ -376,14 +376,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 		if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
 			wpm->pos_reached = true;
 
-			if (counter % 10 == 0)
+			if (counter % 100 == 0)
 				printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
 		}
-
 //		else
 //		{
 //			if(counter % 100 == 0)
-//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f\n",dist,orbit);
+//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
 //		}
 	}
 

apps/multirotor_pos_control/multirotor_pos_control.c

@@ -196,12 +196,14 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
 
 			float x_setpoint = 0.0f;
 
+			// XXX enable switching between Vicon and local position estimate
 			/* local pos is the Vicon position */
 
-			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT;
+			// XXX just an example, lacks rotation around world-body transformation
+			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
 			att_sp.roll_body = 0.0f;
 			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.4f;
+			att_sp.thrust = 0.3f;
 			att_sp.timestamp = hrt_absolute_time();
 
 			/* publish new attitude setpoint */

apps/px4io/comms.c

@@ -116,18 +116,32 @@ int frame_rx;
 int frame_bad;
 
 static void
-comms_handle_frame(void *arg, const void *buffer, size_t length)
+comms_handle_config(const void *buffer, size_t length)
 {
-	struct px4io_command *cmd = (struct px4io_command *)buffer;
+	const struct px4io_config *cfg = (struct px4io_config *)buffer;
 
-	/* make sure it's what we are expecting */
-	if ((length != sizeof(struct px4io_command)) || 
-	    (cmd->f2i_magic != F2I_MAGIC)) {
-	    	frame_bad++;
+	if (length != sizeof(*cfg)) {
+		frame_bad++;
 		return;
 	}
+
 	frame_rx++;
 
+	mixer_set_serial_mode(cfg->serial_rx_mode);
+
+}
+
+static void
+comms_handle_command(const void *buffer, size_t length)
+{
+	const struct px4io_command *cmd = (struct px4io_command *)buffer;
+
+	if (length != sizeof(*cmd)) {
+		frame_bad++;
+		return;
+	}
+
+	frame_rx++;
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
@@ -146,6 +160,29 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
 		system_state.relays[i] = cmd->relay_state[i];
 
-out:
 	irqrestore(flags);
 }
+
+
+static void
+comms_handle_frame(void *arg, const void *buffer, size_t length)
+{
+	const uint16_t *type = (const uint16_t *)buffer;
+
+
+	/* make sure it's what we are expecting */
+	if (length > 2) {
+		switch (*type) {
+		case F2I_MAGIC:
+			comms_handle_command(buffer, length);
+			break;
+		case F2I_CONFIG_MAGIC:
+			comms_handle_config(buffer, length);
+			break;
+		default:
+			break;
+		}
+	}
+    	frame_bad++;
+}
+

apps/px4io/mixer.c

@@ -40,10 +40,13 @@
 
 #include <sys/types.h>
 #include <stdbool.h>
-
+#include <string.h>
 #include <assert.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -64,6 +67,11 @@ static unsigned mixer_input_drops;
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
 
+/*
+ * Serial port fd for serial RX protocols
+ */
+static int rx_port = -1;
+
 /*
  * HRT periodic call used to check for control input data.
  */
@@ -98,12 +106,47 @@ struct mixer {
 int
 mixer_init(void)
 {
+	/* open the serial port */
+	rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
+
 	/* look for control data at 50Hz */
 	hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL);
 
 	return 0;
 }
 
+void
+mixer_set_serial_mode(uint8_t serial_mode)
+{
+
+	if (serial_mode == system_state.serial_rx_mode)
+		return;
+
+	struct termios t;
+	tcgetattr(rx_port, &t);
+
+	switch (serial_mode) {
+	case RX_MODE_PPM_ONLY:
+		break;
+	case RX_MODE_SPEKTRUM_6:
+	case RX_MODE_SPEKTRUM_7:
+		/* 115200, no parity, one stop bit */
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		break;
+	case RX_MODE_FUTABA_SBUS:
+		/* 100000, even parity, two stop bits */
+		cfsetspeed(&t, 100000);
+		t.c_cflag |= (CSTOPB | PARENB);
+		break;
+	default:
+		return;
+	}
+
+	tcsetattr(rx_port, TCSANOW, &t);
+	system_state.serial_rx_mode = serial_mode;
+}
+
 static void
 mixer_tick(void *arg)
 {
@@ -180,23 +223,146 @@ mixer_tick(void *arg)
 	}
 }
 
+static void
+mixer_update(int mixer, uint16_t *inputs, int input_count)
+{
+	/* simple passthrough for now */
+	if (mixer < input_count) {
+		mixers[mixer].current_value = inputs[mixer];
+	} else {
+		mixers[mixer].current_value = 0;
+	}
+}
+
+static bool
+mixer_get_spektrum_input(void)
+{
+	static uint8_t buf[16];
+	static unsigned count;
+
+	/* always read as much data as we can into the buffer */
+	if (count >= sizeof(buf))
+		count = 0;
+	ssize_t result = read(rx_port, buf, sizeof(buf) - count);
+	/* no data or an error */
+	if (result <= 0)
+		return false;
+	count += result;
+
+	/* if there are more than two bytes in the buffer, check for sync */
+	if (count >= 2) {
+		if ((buf[0] != 0x3) || (buf[1] != 0x1)) {
+			/* not in sync; look for a possible sync marker */
+			for (unsigned i = 1; i < count; i++) {
+				if (buf[i] == 0x3) {
+					/* could be a frame marker; move buffer bytes */
+					count -= i;
+					memmove(buf, buf + i, count);
+					break;
+				}
+			}
+		}
+	}
+	if (count < sizeof(buf))
+		return false;
+
+	/* we got a frame; decode it */
+	const uint16_t *channels = (const uint16_t *)&buf[2];
+
+	/*
+	 * Channel assignment for DX6i vs. DX7 is different.
+	 *
+	 * DX7 etc. is:
+	 *
+	 * 0: Aileron
+	 * 1: Flaps
+	 * 2: Gear
+	 * 3: Elevator
+	 * 4: Aux2
+	 * 5: Throttle
+	 * 6: Rudder
+	 *
+	 * DX6i is:
+	 *
+	 * 0: Aileron	
+	 * 1: Flaps
+	 * 2: Elevator
+	 * 3: Rudder
+	 * 4: Throttle
+	 * 5: Gear
+	 * 6: <notused>
+	 *
+	 * We convert these to our standard Futaba-style assignment:
+	 *
+	 * 0: Throttle  (Throttle)
+	 * 1: Roll      (Aileron)
+	 * 2: Pitch     (Elevator)
+	 * 3: Yaw       (Rudder)
+	 * 4: Override  (Flaps)
+	 * 5: FUNC_0    (Gear)
+	 * 6: FUNC_1    (Aux2)
+	 */
+	if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) {
+		system_state.rc_channel_data[0] = channels[5]; /* Throttle */
+		system_state.rc_channel_data[1] = channels[0]; /* Roll */
+		system_state.rc_channel_data[2] = channels[3]; /* Pitch */
+		system_state.rc_channel_data[3] = channels[6]; /* Yaw */
+		system_state.rc_channel_data[4] = channels[1]; /* Override */
+		system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */
+		system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */
+		system_state.rc_channels = 7;
+	} else {
+		system_state.rc_channel_data[0] = channels[4]; /* Throttle */
+		system_state.rc_channel_data[1] = channels[0]; /* Roll */
+		system_state.rc_channel_data[2] = channels[2]; /* Pitch */
+		system_state.rc_channel_data[3] = channels[3]; /* Yaw */
+		system_state.rc_channel_data[4] = channels[1]; /* Override */
+		system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */
+		system_state.rc_channels = 6;
+	}
+	count = 0;
+	return true;
+}
+
+static bool
+mixer_get_sbus_input(void)
+{
+	/* XXX not implemented yet */
+	return false;
+}
+
 static void
 mixer_get_rc_input(void)
 {
-	/*
-	 * XXX currently only supporting PPM
-	 *
-	 * XXX check timestamp to ensure current
-	 */
-	if (ppm_decoded_channels > 0) {
+	bool got_input = false;
+
+	switch (system_state.serial_rx_mode) {
+	case RX_MODE_PPM_ONLY:
+		if (ppm_decoded_channels > 0) {
+			/* copy channel data */
+			system_state.rc_channels = ppm_decoded_channels;
+			for (unsigned i = 0; i < ppm_decoded_channels; i++)
+				system_state.rc_channel_data[i] = ppm_buffer[i];
+			got_input = true;
+		}
+		break;
+
+	case RX_MODE_SPEKTRUM_6:
+	case RX_MODE_SPEKTRUM_7:
+		got_input = mixer_get_spektrum_input();
+		break;
+
+	case RX_MODE_FUTABA_SBUS:
+		got_input = mixer_get_sbus_input();
+		break;
+
+	default:
+		break;
+	}
+
+	if (got_input) {
 		mixer_input_drops = 0;
 		system_state.fmu_report_due = true;
-
-		/* copy channel data */
-		system_state.rc_channels = ppm_decoded_channels;
-		for (unsigned i = 0; i < ppm_decoded_channels; i++)
-			system_state.rc_channel_data[i] = ppm_buffer[i];
-
 	} else {
 		/*
 		 * No data; count the 'frame drops' and once we hit the limit
@@ -213,14 +379,3 @@ mixer_get_rc_input(void)
 		}
 	}
 }
-
-static void
-mixer_update(int mixer, uint16_t *inputs, int input_count)
-{
-	/* simple passthrough for now */
-	if (mixer < input_count) {
-		mixers[mixer].current_value = inputs[mixer];
-	} else {
-		mixers[mixer].current_value = 0;
-	}
-}

apps/px4io/protocol.h

@@ -62,6 +62,18 @@ struct px4io_command {
 	bool		arm_ok;
 };
 
+/* config message from FMU to IO */
+struct px4io_config {
+	uint16_t f2i_config_magic;
+#define F2I_CONFIG_MAGIC 0x6366
+
+	uint8_t		serial_rx_mode;
+#define RX_MODE_PPM_ONLY	0
+#define RX_MODE_SPEKTRUM_6	1
+#define RX_MODE_SPEKTRUM_7	2
+#define RX_MODE_FUTABA_SBUS	3
+};
+
 /* report from IO to FMU */
 struct px4io_report {
 	uint16_t	i2f_magic;

apps/px4io/px4io.c

@@ -102,7 +102,7 @@ int user_start(int argc, char *argv[])
 	safety_init();
 
 	/* set up some timers for the main loop */
-	timers[TIMER_BLINK_AMBER] = 250;	/* heartbeat blink @ 2Hz */
+	timers[TIMER_BLINK_BLUE] = 250;		/* heartbeat blink @ 2Hz */
 	timers[TIMER_STATUS_PRINT] = 1000;	/* print status message @ 1Hz */
 
 	/*
@@ -114,21 +114,21 @@ int user_start(int argc, char *argv[])
 		comms_check();
 
 		/* blink the heartbeat LED */
-		if (timers[TIMER_BLINK_AMBER] == 0) {
-			timers[TIMER_BLINK_AMBER] = 250;
-			LED_AMBER(heartbeat = !heartbeat);
+		if (timers[TIMER_BLINK_BLUE] == 0) {
+			timers[TIMER_BLINK_BLUE] = 250;
+			LED_BLUE(heartbeat = !heartbeat);
 		}
 
 		/* blink the failsafe LED if we don't have FMU input */
 		if (!system_state.mixer_use_fmu) {
-			if (timers[TIMER_BLINK_BLUE] == 0) {
-				timers[TIMER_BLINK_BLUE] = 125;
+			if (timers[TIMER_BLINK_AMBER] == 0) {
+				timers[TIMER_BLINK_AMBER] = 125;
 				failsafe = !failsafe;
 			}
 		} else {
 			failsafe = false;
 		}
-		LED_BLUE(failsafe);
+		LED_AMBER(failsafe);
 		
 		/* print some simple status */
 		if (timers[TIMER_STATUS_PRINT] == 0) {

apps/px4io/px4io.h

@@ -101,6 +101,12 @@ struct sys_state_s
 	 * If true, new control data from the FMU has been received.
 	 */
 	bool		fmu_data_received;
+
+	/*
+	 * Current serial interface mode, per the serial_rx_mode parameter
+	 * in the config packet.
+	 */
+	uint8_t		serial_rx_mode;
 };
 
 extern struct sys_state_s system_state;
@@ -123,8 +129,8 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
 /*
  * GPIO handling.
  */
-#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
 #define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
 
 #define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
@@ -141,6 +147,7 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
  * Mixer
  */
 extern int	mixer_init(void);
+extern void	mixer_set_serial_mode(uint8_t newmode);
 
 /*
  * Safety switch/LED.

apps/sensors/sensors.cpp

@@ -58,11 +58,13 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_rc_input.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
+
 #include <systemlib/ppm_decode.h>
 
 #include <uORB/uORB.h>
@@ -143,6 +145,7 @@ private:
 	int		_gyro_sub;			/**< raw gyro data subscription */
 	int		_accel_sub;			/**< raw accel data subscription */
 	int		_mag_sub;			/**< raw mag data subscription */
+	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -162,6 +165,7 @@ private:
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
 		float ex[_rc_max_chan_count];
+		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
 		float mag_offset[3];
@@ -331,6 +335,7 @@ Sensors::Sensors() :
 	_gyro_sub(-1),
 	_accel_sub(-1),
 	_mag_sub(-1),
+	_rc_sub(-1),
 	_baro_sub(-1),
 	_vstatus_sub(-1),
 	_params_sub(-1),
@@ -464,14 +469,13 @@ Sensors::parameters_update()
 			warnx("Failed getting exponential gain for chan %d", i);
 		}
 
-		_rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 
 		/* handle blowup in the scaling factor calculation */
-		if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) {
-			_rc.chan[i].scaling_factor = 0;
+		if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) {
+			_parameters.scaling_factor[i] = 0;
 		}
 
-		_rc.chan[i].mid = _parameters.trim[i];
 	}
 
 	/* update RC function mappings */
@@ -856,99 +860,126 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 void
 Sensors::ppm_poll()
 {
-	struct manual_control_setpoint_s manual_control;
+	/* fake low-level driver, directly pulling from driver variables */
+	static orb_advert_t rc_input_pub = -1;
+	struct rc_input_values raw;
 
-	/* check to see whether a new frame has been decoded */
-	if (_ppm_last_valid == ppm_last_valid_decode)
-		return;
-	/* require at least two chanels to consider the signal valid */
-	if (ppm_decoded_channels < 4)
-		return;
+	raw.timestamp = ppm_last_valid_decode;
 
-	unsigned channel_limit = ppm_decoded_channels;
-	if (channel_limit > _rc_max_chan_count)
-		channel_limit = _rc_max_chan_count;
+	if (ppm_decoded_channels > 1) {
 
-	/* we are accepting this decode */
-	_ppm_last_valid = ppm_last_valid_decode;
-
-	/* Read out values from HRT */
-	for (unsigned int i = 0; i < channel_limit; i++) {
-		_rc.chan[i].raw = ppm_buffer[i];
-
-		/* scale around the mid point differently for lower and upper range */
-		if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) {
-			_rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
-		} else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) {
-			/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
-			_rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
-			
-		} else {
-			/* in the configured dead zone, output zero */
-			_rc.chan[i].scaled = 0.0f;
+		for (int i = 0; i < ppm_decoded_channels; i++) {
+			raw.values[i] = ppm_buffer[i];
 		}
 
-		/* reverse channel if required */
-		if (i == _rc.function[THROTTLE]) {
-			if ((int)_parameters.rev[i] == -1) {
-				_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
+		raw.channel_count = ppm_decoded_channels;
+
+		/* publish to object request broker */
+		if (rc_input_pub <= 0) {
+			rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw);
+		} else {
+			orb_publish(ORB_ID(input_rc), rc_input_pub, &raw);
+		}
+	}
+
+
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
+
+	if (rc_updated) {
+		struct rc_input_values	rc_input;
+
+		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+		struct manual_control_setpoint_s manual_control;
+
+		/* require at least two chanels to consider the signal valid */
+		if (rc_input.channel_count < 2)
+			return;
+
+		unsigned channel_limit = rc_input.channel_count;
+		if (channel_limit > _rc_max_chan_count)
+			channel_limit = _rc_max_chan_count;
+
+		/* we are accepting this message */
+		_ppm_last_valid = rc_input.timestamp;
+
+		/* Read out values from raw message */
+		for (unsigned int i = 0; i < channel_limit; i++) {
+
+			/* scale around the mid point differently for lower and upper range */
+			if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
+				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
+			} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
+				/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
+				_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
+				
+			} else {
+				/* in the configured dead zone, output zero */
+				_rc.chan[i].scaled = 0.0f;
 			}
-		} else {
-			_rc.chan[i].scaled *= _parameters.rev[i];
+
+			/* reverse channel if required */
+			if (i == _rc.function[THROTTLE]) {
+				if ((int)_parameters.rev[i] == -1) {
+					_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
+				}
+			} else {
+				_rc.chan[i].scaled *= _parameters.rev[i];
+			}
+
+			/* handle any parameter-induced blowups */
+			if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
+				_rc.chan[i].scaled = 0.0f;
 		}
 
-		/* handle any parameter-induced blowups */
-		if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
-			_rc.chan[i].scaled = 0.0f;
+		_rc.chan_count = rc_input.channel_count;
+		_rc.timestamp = rc_input.timestamp;
 
-		//_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor;
+		manual_control.timestamp = rc_input.timestamp;
+
+		/* roll input - rolling right is stick-wise and rotation-wise positive */
+		manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
+		if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
+		if (manual_control.roll >  1.0f) manual_control.roll =  1.0f;
+		if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
+			manual_control.roll *= _parameters.rc_scale_roll;
+		}
+
+		/*
+		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
+		 * so reverse sign.
+		 */
+		manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
+		if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
+		if (manual_control.pitch >  1.0f) manual_control.pitch =  1.0f;
+		if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
+			manual_control.pitch *= _parameters.rc_scale_pitch;
+		}
+
+		/* yaw input - stick right is positive and positive rotation */
+		manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
+		if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
+		if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
+		if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
+			manual_control.yaw *= _parameters.rc_scale_yaw;
+		}
+		
+		/* throttle input */
+		manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
+		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
+		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
+
+		/* mode switch input */
+		manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
+		if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
+		if (manual_control.override_mode_switch >  1.0f) manual_control.override_mode_switch =  1.0f;
+
+		orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
+		orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
 	}
 
-	_rc.chan_count = ppm_decoded_channels;
-	_rc.timestamp = ppm_last_valid_decode;
-
-	manual_control.timestamp = ppm_last_valid_decode;
-
-	/* roll input - rolling right is stick-wise and rotation-wise positive */
-	manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
-	if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
-	if (manual_control.roll >  1.0f) manual_control.roll =  1.0f;
-	if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
-		manual_control.roll *= _parameters.rc_scale_roll;
-	}
-
-	/*
-	 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-	 * so reverse sign.
-	 */
-	manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
-	if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
-	if (manual_control.pitch >  1.0f) manual_control.pitch =  1.0f;
-	if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
-		manual_control.pitch *= _parameters.rc_scale_pitch;
-	}
-
-	/* yaw input - stick right is positive and positive rotation */
-	manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
-	if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
-	if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
-	if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
-		manual_control.yaw *= _parameters.rc_scale_yaw;
-	}
-	
-	/* throttle input */
-	manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-	if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
-	if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
-
-	/* mode switch input */
-	manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
-	if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
-	if (manual_control.override_mode_switch >  1.0f) manual_control.override_mode_switch =  1.0f;
-
-	orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
-	orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
-
 }
 #endif
 
@@ -979,6 +1010,7 @@ Sensors::task_main()
 	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
 	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
 	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));

apps/systemlib/geo/geo.c

@@ -47,6 +47,150 @@
 #include <systemlib/geo/geo.h>
 #include <math.h>
 
+
+/* values for map projection */
+static double phi_1;
+static double sin_phi_1;
+static double cos_phi_1;
+static double lambda_0;
+static double scale;
+
+/**
+ * Initializes the map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	phi_1 = lat_0 / 180.0 * M_PI;
+	lambda_0 = lon_0 / 180.0 * M_PI;
+
+	sin_phi_1 = sin(phi_1);
+	cos_phi_1 = cos(phi_1);
+
+	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
+
+	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
+	const double r_earth = 6371000;
+
+	double lat1 = phi_1;
+	double lon1 = lambda_0;
+
+	double lat2 = phi_1 + 0.5 / 180 * M_PI;
+	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
+	double sin_lat_2 = sin(lat2);
+	double cos_lat_2 = cos(lat2);
+	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
+
+	/* 2) calculate distance rho on plane */
+	double k_bar = 0;
+	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
+	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
+	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
+
+	scale = d / rho;
+
+}
+
+/**
+ * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	double phi = lat / 180.0 * M_PI;
+	double lambda = lon / 180.0 * M_PI;
+
+	double sin_phi = sin(phi);
+	double cos_phi = cos(phi);
+
+	double k_bar = 0;
+	/* using small angle approximation (formula in comment is without aproximation) */
+	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	/* using small angle approximation (formula in comment is without aproximation) */
+	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
+	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
+
+//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
+}
+
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+
+	double x_descaled = x / scale;
+	double y_descaled = y / scale;
+
+	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
+	double sin_c = sin(c);
+	double cos_c = cos(c);
+
+	double lat_sphere = 0;
+
+	if (c != 0)
+		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
+	else
+		lat_sphere = asin(cos_c * sin_phi_1);
+
+//	printf("lat_sphere = %.10f\n",lat_sphere);
+
+	double lon_sphere = 0;
+
+	if (phi_1  == M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
+
+	} else if (phi_1 == -M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
+
+	} else {
+
+		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
+		//using small angle approximation
+//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
+//    	if(denominator != 0)
+//    	{
+//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
+//    	}
+//    	else
+//    	{
+//    	...
+//    	}
+	}
+
+//	printf("lon_sphere = %.10f\n",lon_sphere);
+
+	*lat = lat_sphere * 180.0 / M_PI;
+	*lon = lon_sphere * 180.0 / M_PI;
+
+}
+
+
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
 	double lat_now_rad = lat_now / 180.0d * M_PI;

apps/systemlib/geo/geo.h

@@ -55,6 +55,12 @@ typedef struct {
 	float bearing;		// Bearing in radians to closest point on line/arc
 } crosstrack_error_s;
 
+__EXPORT static void map_projection_init(double lat_0, double lon_0);
+
+__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y);
+
+__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon);
+
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
@@ -69,4 +75,4 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
 float _wrap180(float bearing);
 float _wrap360(float bearing);
 float _wrapPI(float bearing);
-float _wrap2PI(float bearing);
+float _wrap2PI(float bearing);

apps/systemlib/pid/pid.c

@@ -172,9 +172,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculate the output.  Limit output magnitude to pid->limit
 	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
-	//if (output > pid->limit) output = pid->limit;
+	if (output > pid->limit) output = pid->limit;
 
-	//if (output < -pid->limit) output = -pid->limit;
+	if (output < -pid->limit) output = -pid->limit;
 
 	if (isfinite(output)) {
 		pid->last_output = output;

apps/uORB/topics/rc_channels.h

@@ -50,14 +50,6 @@
  * @{
  */
 
-enum RC_CHANNELS_STATUS
-{
-	UNKNOWN = 0,
-	KNOWN = 1,
-	SIGNAL = 2,
-	TIMEOUT = 3
-};
-
 /** 
  * This defines the mapping of the RC functions.
  * The value assigned to the specific function corresponds to the entry of
@@ -85,12 +77,7 @@ struct rc_channels_s {
 	uint64_t timestamp;                 /**< In microseconds since boot time. */
   uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
   struct {
-    uint16_t mid;                     /**< midpoint (0). */
-    float scaling_factor;             /**< scaling factor from raw counts to -1..+1 */
-    uint16_t raw;                     /**< current raw value */
     float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-    uint16_t override;
-    enum RC_CHANNELS_STATUS status;   /**< status of the channel */
   } chan[RC_CHANNELS_FUNCTION_MAX];
   uint8_t chan_count;                 /**< maximum number of valid channels */
 
@@ -98,6 +85,7 @@ struct rc_channels_s {
   char function_name[RC_CHANNELS_FUNCTION_MAX][20];
   uint8_t function[RC_CHANNELS_FUNCTION_MAX];
   uint8_t rssi;                       /**< Overall receive signal strength */
+  bool is_valid;                      /**< Inputs are valid, no timeout */
 }; /**< radio control channels. */
 
 /**

nuttx/configs/px4io/io/defconfig

@@ -155,6 +155,8 @@ CONFIG_STM32_ADC3=n
 # CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
 # CONFIG_USARTn_2STOP - Two stop bits
 #
+CONFIG_SERIAL_TERMIOS=y
+
 CONFIG_USART1_SERIAL_CONSOLE=y
 CONFIG_USART2_SERIAL_CONSOLE=n
 CONFIG_USART3_SERIAL_CONSOLE=n
@@ -163,11 +165,11 @@ CONFIG_USART1_TXBUFSIZE=32
 CONFIG_USART2_TXBUFSIZE=32
 CONFIG_USART3_TXBUFSIZE=32
 
-CONFIG_USART1_RXBUFSIZE=32
+CONFIG_USART1_RXBUFSIZE=64
 CONFIG_USART2_RXBUFSIZE=128
 CONFIG_USART3_RXBUFSIZE=32
 
-CONFIG_USART1_BAUD=57600
+CONFIG_USART1_BAUD=115200
 CONFIG_USART2_BAUD=115200
 CONFIG_USART3_BAUD=115200