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

Tools/px_uploader.py

@@ -430,7 +430,7 @@ while True:
                                 # Windows, don't open POSIX ports
                                 if not "/" in port:
                                         up = uploader(port, args.baud)
-                except:
+                except Exception:
                         # open failed, rate-limit our attempts
                         time.sleep(0.05)
 
@@ -443,7 +443,7 @@ while True:
                         up.identify()
                         print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
 
-                except:
+                except Exception:
                         # most probably a timeout talking to the port, no bootloader, try to reboot the board
                         print("attempting reboot on %s..." % port)
                         up.send_reboot()

makefiles/config_px4fmu-v1_default.mk

@@ -134,7 +134,10 @@ MODULES		+= lib/geo
 
 # Tutorial code from
 # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
-MODULES			+= examples/fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
 
 #
 # Transitional support - add commands from the NuttX export archive.

makefiles/config_px4fmu-v2_default.mk

@@ -128,8 +128,12 @@ MODULES		+= lib/geo
 # https://pixhawk.ethz.ch/px4/dev/debug_values
 #MODULES		+= examples/px4_mavlink_debug
 
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
 # Hardware test
-MODULES			+= examples/hwtest
+#MODULES			+= examples/hwtest
 
 #
 # Transitional support - add commands from the NuttX export archive.

src/drivers/drv_pwm_output.h

@@ -120,6 +120,7 @@ ORB_DECLARE(output_pwm);
 
 #define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
 #define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
+#define DSMX8_BIND_PULSES 10	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
 
 /** Power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)

src/modules/att_pos_estimator_ekf/KalmanNav.hpp

@@ -166,19 +166,19 @@ protected:
 	double lat, lon;                   	/**< lat, lon radians */
 	float alt;                   		/**< altitude, meters */
 	// parameters
-	control::BlockParam<float> _vGyro;      /**< gyro process noise */
+	control::BlockParamFloat _vGyro;      /**< gyro process noise */
-	control::BlockParam<float> _vAccel;     /**< accelerometer process noise  */
+	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */
-	control::BlockParam<float> _rMag;       /**< magnetometer measurement noise  */
+	control::BlockParamFloat _rMag;       /**< magnetometer measurement noise  */
-	control::BlockParam<float> _rGpsVel;    /**< gps velocity measurement noise */
+	control::BlockParamFloat _rGpsVel;    /**< gps velocity measurement noise */
-	control::BlockParam<float> _rGpsPos;    /**< gps position measurement noise */
+	control::BlockParamFloat _rGpsPos;    /**< gps position measurement noise */
-	control::BlockParam<float> _rGpsAlt;    /**< gps altitude measurement noise */
+	control::BlockParamFloat _rGpsAlt;    /**< gps altitude measurement noise */
-	control::BlockParam<float> _rPressAlt;  /**< press altitude measurement noise */
+	control::BlockParamFloat _rPressAlt;  /**< press altitude measurement noise */
-	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
+	control::BlockParamFloat _rAccel;     /**< accelerometer measurement noise */
-	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
+	control::BlockParamFloat _magDip;     /**< magnetic inclination with level */
-	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
+	control::BlockParamFloat _magDec;     /**< magnetic declination, clockwise rotation */
-	control::BlockParam<float> _g;          /**< gravitational constant */
+	control::BlockParamFloat _g;          /**< gravitational constant */
-	control::BlockParam<float> _faultPos;   /**< fault detection threshold for position */
+	control::BlockParamFloat _faultPos;   /**< fault detection threshold for position */
-	control::BlockParam<float> _faultAtt;   /**< fault detection threshold for attitude */
+	control::BlockParamFloat _faultAtt;   /**< fault detection threshold for attitude */
 	// status
 	bool _attitudeInitialized;
 	bool _positionInitialized;

src/modules/commander/commander.cpp

@@ -1193,31 +1193,19 @@ int commander_thread_main(int argc, char *argv[])
 		bool main_state_changed = check_main_state_changed();
 		bool navigation_state_changed = check_navigation_state_changed();
 
+		hrt_abstime t1 = hrt_absolute_time();
+
+		if (navigation_state_changed || arming_state_changed) {
+			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
+		}
+
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
 		}
 
-		hrt_abstime t1 = hrt_absolute_time();
-
-		/* publish arming state */
-		if (arming_state_changed) {
-			armed.timestamp = t1;
-			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-		}
-
-		/* publish control mode */
-		if (navigation_state_changed || arming_state_changed) {
-			/* publish new navigation state */
-			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
-			control_mode.counter++;
-			control_mode.timestamp = t1;
-			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
-		}
-
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
-			status.counter++;
 			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
 			control_mode.timestamp = t1;

src/modules/commander/state_machine_helper.cpp

@@ -504,7 +504,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 	if (valid_transition) {
 		current_status->hil_state = new_state;
 
-		current_status->counter++;
 		current_status->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 

src/modules/controllib/block/BlockParam.hpp

@@ -69,22 +69,39 @@ protected:
 /**
  * Parameters that are tied to blocks for updating and nameing.
  */
-template<class T>
+
-class __EXPORT BlockParam : public BlockParamBase
+class __EXPORT BlockParamFloat : public BlockParamBase
 {
 public:
-	BlockParam(Block *block, const char *name, bool parent_prefix=true) :
+	BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) :
 		BlockParamBase(block, name, parent_prefix),
 		_val() {
 		update();
 	}
-	T get() { return _val; }
+	float get() { return _val; }
-	void set(T val) { _val = val; }
+	void set(float val) { _val = val; }
 	void update() {
 		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
 	}
 protected:
-	T _val;
+	float _val;
+};
+
+class __EXPORT BlockParamInt : public BlockParamBase
+{
+public:
+	BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
+		BlockParamBase(block, name, parent_prefix),
+		_val() {
+		update();
+	}
+	int get() { return _val; }
+	void set(int val) { _val = val; }
+	void update() {
+		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+	}
+protected:
+	int _val;
 };
 
 } // namespace control

src/modules/controllib/blocks.hpp

@@ -74,8 +74,8 @@ public:
 	float getMax() { return _max.get(); }
 protected:
 // attributes
-	BlockParam<float> _min;
+	control::BlockParamFloat _min;
-	BlockParam<float> _max;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockLimitTest();
@@ -99,7 +99,7 @@ public:
 	float getMax() { return _max.get(); }
 protected:
 // attributes
-	BlockParam<float> _max;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockLimitSymTest();
@@ -126,7 +126,7 @@ public:
 protected:
 // attributes
 	float _state;
-	BlockParam<float> _fCut;
+	control::BlockParamFloat _fCut;
 };
 
 int __EXPORT blockLowPassTest();
@@ -157,7 +157,7 @@ protected:
 // attributes
 	float _u; /**< previous input */
 	float _y; /**< previous output */
-	BlockParam<float> _fCut; /**< cut-off frequency, Hz */
+	control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */
 };
 
 int __EXPORT blockHighPassTest();
@@ -273,7 +273,7 @@ public:
 // accessors
 	float getKP() { return _kP.get(); }
 protected:
-	BlockParam<float> _kP;
+	control::BlockParamFloat _kP;
 };
 
 int __EXPORT blockPTest();
@@ -303,8 +303,8 @@ public:
 	BlockIntegral &getIntegral() { return _integral; }
 private:
 	BlockIntegral _integral;
-	BlockParam<float> _kP;
+	control::BlockParamFloat _kP;
-	BlockParam<float> _kI;
+	control::BlockParamFloat _kI;
 };
 
 int __EXPORT blockPITest();
@@ -334,8 +334,8 @@ public:
 	BlockDerivative &getDerivative() { return _derivative; }
 private:
 	BlockDerivative _derivative;
-	BlockParam<float> _kP;
+	control::BlockParamFloat _kP;
-	BlockParam<float> _kD;
+	control::BlockParamFloat _kD;
 };
 
 int __EXPORT blockPDTest();
@@ -372,9 +372,9 @@ private:
 // attributes
 	BlockIntegral _integral;
 	BlockDerivative _derivative;
-	BlockParam<float> _kP;
+	control::BlockParamFloat _kP;
-	BlockParam<float> _kI;
+	control::BlockParamFloat _kI;
-	BlockParam<float> _kD;
+	control::BlockParamFloat _kD;
 };
 
 int __EXPORT blockPIDTest();
@@ -404,7 +404,7 @@ public:
 	float get() { return _val; }
 private:
 // attributes
-	BlockParam<float> _trim;
+	control::BlockParamFloat _trim;
 	BlockLimit _limit;
 	float _val;
 };
@@ -439,8 +439,8 @@ public:
 	float getMax() { return _max.get(); }
 private:
 // attributes
-	BlockParam<float> _min;
+	control::BlockParamFloat _min;
-	BlockParam<float> _max;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockRandUniformTest();
@@ -486,8 +486,8 @@ public:
 	float getStdDev() { return _stdDev.get(); }
 private:
 // attributes
-	BlockParam<float> _mean;
+	control::BlockParamFloat _mean;
-	BlockParam<float> _stdDev;
+	control::BlockParamFloat _stdDev;
 };
 
 int __EXPORT blockRandGaussTest();

src/modules/mavlink/mavlink_receiver.cpp

@@ -167,6 +167,7 @@ handle_message(mavlink_message_t *msg)
 				/* check if topic is advertised */
 				if (cmd_pub <= 0) {
 					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
 				} else {
 					/* publish */
 					orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
@@ -437,9 +438,11 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_airspeed < 0) {
 				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
+
 			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
 			/* individual sensor publications */
@@ -455,49 +458,72 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_gyro < 0) {
 				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+
 			} else {
 				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
 			}
 
 			struct accel_report accel;
+
 			accel.x_raw = imu.xacc / mg2ms2;
+
 			accel.y_raw = imu.yacc / mg2ms2;
+
 			accel.z_raw = imu.zacc / mg2ms2;
+
 			accel.x = imu.xacc;
+
 			accel.y = imu.yacc;
+
 			accel.z = imu.zacc;
+
 			accel.temperature = imu.temperature;
+
 			accel.timestamp = hrt_absolute_time();
 
 			if (pub_hil_accel < 0) {
 				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
 			} else {
 				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
 			}
 
 			struct mag_report mag;
+
 			mag.x_raw = imu.xmag / mga2ga;
+
 			mag.y_raw = imu.ymag / mga2ga;
+
 			mag.z_raw = imu.zmag / mga2ga;
+
 			mag.x = imu.xmag;
+
 			mag.y = imu.ymag;
+
 			mag.z = imu.zmag;
+
 			mag.timestamp = hrt_absolute_time();
 
 			if (pub_hil_mag < 0) {
 				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+
 			} else {
 				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
 			}
 
 			struct baro_report baro;
+
 			baro.pressure = imu.abs_pressure;
+
 			baro.altitude = imu.pressure_alt;
+
 			baro.temperature = imu.temperature;
+
 			baro.timestamp = hrt_absolute_time();
 
 			if (pub_hil_baro < 0) {
 				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+
 			} else {
 				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
 			}
@@ -505,6 +531,7 @@ handle_message(mavlink_message_t *msg)
 			/* publish combined sensor topic */
 			if (pub_hil_sensors > 0) {
 				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
 			} else {
 				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 			}
@@ -517,6 +544,7 @@ handle_message(mavlink_message_t *msg)
 			/* lazily publish the battery voltage */
 			if (pub_hil_battery > 0) {
 				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
 			} else {
 				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 			}
@@ -527,7 +555,7 @@ handle_message(mavlink_message_t *msg)
 
 			// output
 			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil sensor at %d hz\n", hil_frames/10);
+				printf("receiving hil sensor at %d hz\n", hil_frames / 10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
@@ -552,9 +580,11 @@ handle_message(mavlink_message_t *msg)
 
 			/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
 			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
+
 			/* go back to -PI..PI */
 			if (heading_rad > M_PI_F)
 				heading_rad -= 2.0f * M_PI_F;
+
 			hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
 			hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
 			hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
@@ -567,6 +597,7 @@ handle_message(mavlink_message_t *msg)
 			/* publish GPS measurement data */
 			if (pub_hil_gps > 0) {
 				orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
+
 			} else {
 				pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 			}
@@ -585,6 +616,7 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_airspeed < 0) {
 				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
@@ -602,6 +634,7 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_global_pos > 0) {
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+
 			} else {
 				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 			}
@@ -613,7 +646,7 @@ handle_message(mavlink_message_t *msg)
 
 			/* set rotation matrix */
 			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-				hil_attitude.R[i][j] = C_nb(i, j);
+					hil_attitude.R[i][j] = C_nb(i, j);
 
 			hil_attitude.R_valid = true;
 
@@ -636,22 +669,32 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_attitude > 0) {
 				orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+
 			} else {
 				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 			}
 
 			struct accel_report accel;
+
 			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+
 			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+
 			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+
 			accel.x = hil_state.xacc;
+
 			accel.y = hil_state.yacc;
+
 			accel.z = hil_state.zacc;
+
 			accel.temperature = 25.0f;
+
 			accel.timestamp = hrt_absolute_time();
 
 			if (pub_hil_accel < 0) {
 				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
 			} else {
 				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
 			}
@@ -664,6 +707,7 @@ handle_message(mavlink_message_t *msg)
 			/* lazily publish the battery voltage */
 			if (pub_hil_battery > 0) {
 				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
 			} else {
 				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 			}
@@ -733,17 +777,23 @@ receive_thread(void *arg)
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink uart rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
+
+	struct pollfd fds[1];
+	fds[0].fd = uart_fd;
+	fds[0].events = POLLIN;
+
+	ssize_t nread = 0;
 
 	while (!thread_should_exit) {
-
-		struct pollfd fds[1];
-		fds[0].fd = uart_fd;
-		fds[0].events = POLLIN;
-
 		if (poll(fds, 1, timeout) > 0) {
+			if (nread < sizeof(buf)) {
+				/* to avoid reading very small chunks wait for data before reading */
+				usleep(1000);
+			}
+
 			/* non-blocking read. read may return negative values */
-			ssize_t nread = read(uart_fd, buf, sizeof(buf));
+			nread = read(uart_fd, buf, sizeof(buf));
 
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
@@ -751,10 +801,10 @@ receive_thread(void *arg)
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
-					/* Handle packet with waypoint component */
+					/* handle packet with waypoint component */
 					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
 
-					/* Handle packet with parameter component */
+					/* handle packet with parameter component */
 					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
 				}
 			}

src/modules/mavlink/orb_listener.c

@@ -711,7 +711,7 @@ static void *
 uorb_receive_thread(void *arg)
 {
 	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink orb rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
 
 	/*
 	 * set up poll to block for new data,

src/modules/mavlink_onboard/mavlink.c

@@ -167,12 +167,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	case 921600: speed = B921600; break;
 
 	default:
-		fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud);
 		return -EINVAL;
 	}
 
 	/* open uart */
-	printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
+	warnx("UART is %s, baudrate is %d", uart_name, baud);
 	uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -183,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
 		/* Back up the original uart configuration to restore it after exit */
 		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+			warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state);
 			close(uart);
 			return -1;
 		}
@@ -196,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 
 		/* Set baud rate */
 		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state);
 			close(uart);
 			return -1;
 		}
 
 
 		if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+			warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name);
 			close(uart);
 			return -1;
 		}
@@ -481,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[])
 static void
 usage()
 {
-	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
+	fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n"
-			"       mavlink stop\n"
+			"       mavlink_onboard stop\n"
-			"       mavlink status\n");
+			"       mavlink_onboard status\n");
 	exit(1);
 }
 
@@ -499,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[])
 
 		/* this is not an error */
 		if (thread_running)
-			errx(0, "mavlink already running\n");
+			errx(0, "already running");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn_cmd("mavlink_onboard",
@@ -516,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[])
 		while (thread_running) {
 			usleep(200000);
 		}
-		warnx("terminated.");
+		warnx("terminated");
 		exit(0);
 	}
 

src/modules/mavlink_onboard/mavlink_receiver.c

@@ -100,11 +100,11 @@ handle_message(mavlink_message_t *msg)
 		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
 
 		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
-			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+				|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
 			//check for MAVLINK terminate command
 			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 				/* This is the link shutdown command, terminate mavlink */
-				printf("[mavlink] Terminating .. \n");
+				warnx("terminating...");
 				fflush(stdout);
 				usleep(50000);
 
@@ -132,6 +132,7 @@ handle_message(mavlink_message_t *msg)
 				if (cmd_pub <= 0) {
 					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 				}
+
 				/* publish */
 				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 			}
@@ -156,6 +157,7 @@ handle_message(mavlink_message_t *msg)
 		/* check if topic is advertised */
 		if (flow_pub <= 0) {
 			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+
 		} else {
 			/* publish */
 			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
@@ -186,6 +188,7 @@ handle_message(mavlink_message_t *msg)
 		/* check if topic is advertised */
 		if (cmd_pub <= 0) {
 			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
 		} else {
 			/* create command */
 			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
@@ -203,6 +206,7 @@ handle_message(mavlink_message_t *msg)
 
 		if (vicon_position_pub <= 0) {
 			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+
 		} else {
 			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
 		}
@@ -227,33 +231,37 @@ handle_message(mavlink_message_t *msg)
 			bool ml_armed = false;
 
 			switch (quad_motors_setpoint.mode) {
-				case 0:
+			case 0:
-					ml_armed = false;
+				ml_armed = false;
-					break;
+				break;
-				case 1:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-					ml_armed = true;
 
-					break;
+			case 1:
-				case 2:
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
+				ml_armed = true;
-					ml_armed = true;
 
-					break;
+				break;
-				case 3:
+
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
+			case 2:
-					break;
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-				case 4:
+				ml_armed = true;
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
+
-					break;
+				break;
+
+			case 3:
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
+				break;
+
+			case 4:
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
+				break;
 			}
 
-			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1]   / (float)INT16_MAX;
+			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1]  / (float)INT16_MAX;
+			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-			offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1]    / (float)INT16_MAX;
+			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
+			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
 
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
+			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
 				ml_armed = false;
 			}
 
@@ -265,6 +273,7 @@ handle_message(mavlink_message_t *msg)
 			/* check if topic has to be advertised */
 			if (offboard_control_sp_pub <= 0) {
 				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
 			} else {
 				/* Publish */
 				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
@@ -281,31 +290,36 @@ handle_message(mavlink_message_t *msg)
 static void *
 receive_thread(void *arg)
 {
-	int uart_fd = *((int*)arg);
+	int uart_fd = *((int *)arg);
 
 	const int timeout = 1000;
-	uint8_t ch;
+	uint8_t buf[32];
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink offb rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid());
+
+	struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
+
+	ssize_t nread = 0;
 
 	while (!thread_should_exit) {
-
-		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
-
 		if (poll(fds, 1, timeout) > 0) {
-			/* non-blocking read until buffer is empty */
+			if (nread < sizeof(buf)) {
-			int nread = 0;
+				/* to avoid reading very small chunks wait for data before reading */
+				usleep(1000);
+			}
 
-			do {
+			/* non-blocking read. read may return negative values */
-				nread = read(uart_fd, &ch, 1);
+			nread = read(uart_fd, buf, sizeof(buf));
 
-				if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
+			/* if read failed, this loop won't execute */
+			for (ssize_t i = 0; i < nread; i++) {
+				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
 					/* handle generic messages and commands */
 					handle_message(&msg);
 				}
-			} while (nread > 0);
+			}
 		}
 	}
 
@@ -319,8 +333,8 @@ receive_start(int uart)
 	pthread_attr_init(&receiveloop_attr);
 
 	struct sched_param param;
-  	param.sched_priority = SCHED_PRIORITY_MAX - 40;
+	param.sched_priority = SCHED_PRIORITY_MAX - 40;
-  	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
+	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
 	pthread_attr_setstacksize(&receiveloop_attr, 2048);
 

src/modules/px4iofirmware/dsm.c

@@ -285,10 +285,10 @@ dsm_bind(uint16_t cmd, int pulses)
 
 		/*Pulse RX pin a number of times*/
 		for (int i = 0; i < pulses; i++) {
+			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, false);
 			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(25);
 		}
 		break;
 

src/modules/sensors/sensors.cpp

@@ -68,7 +68,6 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 
-#include <systemlib/ppm_decode.h>
 #include <systemlib/airspeed.h>
 
 #include <uORB/uORB.h>
@@ -112,15 +111,15 @@
  */
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-  #define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_BATTERY_VOLTAGE_CHANNEL	10
-  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
 #endif
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-  #define ADC_BATTERY_VOLTAGE_CHANNEL	2
+#define ADC_BATTERY_VOLTAGE_CHANNEL	2
-  #define ADC_BATTERY_CURRENT_CHANNEL	3
+#define ADC_BATTERY_CURRENT_CHANNEL	3
-  #define ADC_5V_RAIL_SENSE		4
+#define ADC_5V_RAIL_SENSE		4
-  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
 #endif
 
 #define BAT_VOL_INITIAL 0.f
@@ -134,8 +133,6 @@
  */
 #define PCB_TEMP_ESTIMATE_DEG 5.0f
 
-#define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
-
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
@@ -143,70 +140,68 @@
  * This enum maps from board attitude to airframe attitude.
  */
 enum Rotation {
-    ROTATION_NONE                = 0,
+	ROTATION_NONE                = 0,
-    ROTATION_YAW_45              = 1,
+	ROTATION_YAW_45              = 1,
-    ROTATION_YAW_90              = 2,
+	ROTATION_YAW_90              = 2,
-    ROTATION_YAW_135             = 3,
+	ROTATION_YAW_135             = 3,
-    ROTATION_YAW_180             = 4,
+	ROTATION_YAW_180             = 4,
-    ROTATION_YAW_225             = 5,
+	ROTATION_YAW_225             = 5,
-    ROTATION_YAW_270             = 6,
+	ROTATION_YAW_270             = 6,
-    ROTATION_YAW_315             = 7,
+	ROTATION_YAW_315             = 7,
-    ROTATION_ROLL_180            = 8,
+	ROTATION_ROLL_180            = 8,
-    ROTATION_ROLL_180_YAW_45     = 9,
+	ROTATION_ROLL_180_YAW_45     = 9,
-    ROTATION_ROLL_180_YAW_90     = 10,
+	ROTATION_ROLL_180_YAW_90     = 10,
-    ROTATION_ROLL_180_YAW_135    = 11,
+	ROTATION_ROLL_180_YAW_135    = 11,
-    ROTATION_PITCH_180           = 12,
+	ROTATION_PITCH_180           = 12,
-    ROTATION_ROLL_180_YAW_225    = 13,
+	ROTATION_ROLL_180_YAW_225    = 13,
-    ROTATION_ROLL_180_YAW_270    = 14,
+	ROTATION_ROLL_180_YAW_270    = 14,
-    ROTATION_ROLL_180_YAW_315    = 15,
+	ROTATION_ROLL_180_YAW_315    = 15,
-    ROTATION_ROLL_90             = 16,
+	ROTATION_ROLL_90             = 16,
-    ROTATION_ROLL_90_YAW_45      = 17,
+	ROTATION_ROLL_90_YAW_45      = 17,
-    ROTATION_ROLL_90_YAW_90      = 18,
+	ROTATION_ROLL_90_YAW_90      = 18,
-    ROTATION_ROLL_90_YAW_135     = 19,
+	ROTATION_ROLL_90_YAW_135     = 19,
-    ROTATION_ROLL_270            = 20,
+	ROTATION_ROLL_270            = 20,
-    ROTATION_ROLL_270_YAW_45     = 21,
+	ROTATION_ROLL_270_YAW_45     = 21,
-    ROTATION_ROLL_270_YAW_90     = 22,
+	ROTATION_ROLL_270_YAW_90     = 22,
-    ROTATION_ROLL_270_YAW_135    = 23,
+	ROTATION_ROLL_270_YAW_135    = 23,
-    ROTATION_PITCH_90            = 24,
+	ROTATION_PITCH_90            = 24,
-    ROTATION_PITCH_270           = 25,
+	ROTATION_PITCH_270           = 25,
-    ROTATION_MAX
+	ROTATION_MAX
 };
 
-typedef struct
+typedef struct {
-{
+	uint16_t roll;
-    uint16_t roll;
+	uint16_t pitch;
-    uint16_t pitch;
+	uint16_t yaw;
-    uint16_t yaw;
 } rot_lookup_t;
 
-const rot_lookup_t rot_lookup[] =
+const rot_lookup_t rot_lookup[] = {
-{
+	{  0,   0,   0 },
-    {  0,   0,   0 },
+	{  0,   0,  45 },
-    {  0,   0,  45 },
+	{  0,   0,  90 },
-    {  0,   0,  90 },
+	{  0,   0, 135 },
-    {  0,   0, 135 },
+	{  0,   0, 180 },
-    {  0,   0, 180 },
+	{  0,   0, 225 },
-    {  0,   0, 225 },
+	{  0,   0, 270 },
-    {  0,   0, 270 },
+	{  0,   0, 315 },
-    {  0,   0, 315 },
+	{180,   0,   0 },
-    {180,   0,   0 },
+	{180,   0,  45 },
-    {180,   0,  45 },
+	{180,   0,  90 },
-    {180,   0,  90 },
+	{180,   0, 135 },
-    {180,   0, 135 },
+	{  0, 180,   0 },
-    {  0, 180,   0 },
+	{180,   0, 225 },
-    {180,   0, 225 },
+	{180,   0, 270 },
-    {180,   0, 270 },
+	{180,   0, 315 },
-    {180,   0, 315 },
+	{ 90,   0,   0 },
-    { 90,   0,   0 },
+	{ 90,   0,  45 },
-    { 90,   0,  45 },
+	{ 90,   0,  90 },
-    { 90,   0,  90 },
+	{ 90,   0, 135 },
-    { 90,   0, 135 },
+	{270,   0,   0 },
-    {270,   0,   0 },
+	{270,   0,  45 },
-    {270,   0,  45 },
+	{270,   0,  90 },
-    {270,   0,  90 },
+	{270,   0, 135 },
-    {270,   0, 135 },
+	{  0,  90,   0 },
-    {  0,  90,   0 },
+	{  0, 270,   0 }
-    {  0, 270,   0 }
 };
 
 /**
@@ -239,12 +234,12 @@ public:
 private:
 	static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX;	/**< maximum number of r/c channels we handle */
 
-	hrt_abstime	_ppm_last_valid;		/**< last time we got a valid ppm signal */
+	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
 
 	/**
-	 * Gather and publish PPM input data.
+	 * Gather and publish RC input data.
 	 */
-	void		ppm_poll();
+	void		rc_poll();
 
 	/* XXX should not be here - should be own driver */
 	int 		_fd_adc;			/**< ADC driver handle */
@@ -501,7 +496,7 @@ Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
-	_ppm_last_valid(0),
+	_rc_last_valid(0),
 
 	_fd_adc(-1),
 	_last_adc(0),
@@ -532,8 +527,8 @@ Sensors::Sensors() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
-	_board_rotation(3,3),
+	_board_rotation(3, 3),
-	_external_mag_rotation(3,3),
+	_external_mag_rotation(3, 3),
 	_mag_is_external(false)
 {
 
@@ -660,8 +655,8 @@ int
 Sensors::parameters_update()
 {
 	bool rc_valid = true;
-    float tmpScaleFactor = 0.0f;
+	float tmpScaleFactor = 0.0f;
-    float tmpRevFactor = 0.0f;
+	float tmpRevFactor = 0.0f;
 
 	/* rc values */
 	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
@@ -678,15 +673,15 @@ Sensors::parameters_update()
 		/* handle blowup in the scaling factor calculation */
 		if (!isfinite(tmpScaleFactor) ||
 		    (tmpRevFactor < 0.000001f) ||
-		    (tmpRevFactor > 0.2f) ) {
+		    (tmpRevFactor > 0.2f)) {
 			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
 			/* scaling factors do not make sense, lock them down */
 			_parameters.scaling_factor[i] = 0.0f;
 			rc_valid = false;
+
+		} else {
+			_parameters.scaling_factor[i] = tmpScaleFactor;
 		}
-        else {
-            _parameters.scaling_factor[i] = tmpScaleFactor;
-        }
 	}
 
 	/* handle wrong values */
@@ -812,7 +807,7 @@ void
 Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
 {
 	/* first set to zero */
-	rot_matrix->Matrix::zero(3,3);
+	rot_matrix->Matrix::zero(3, 3);
 
 	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
 	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
@@ -823,7 +818,7 @@ Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
 	math::Dcm R(euler);
 
 	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-		(*rot_matrix)(i,j) = R(i, j);
+			(*rot_matrix)(i, j) = R(i, j);
 }
 
 void
@@ -841,7 +836,7 @@ Sensors::accel_init()
 
 		// XXX do the check more elegantly
 
-		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 		/* set the accel internal sampling rate up to at leat 1000Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
@@ -849,7 +844,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -857,10 +852,10 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
-		#else
+#else
-			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
 
-		#endif
+#endif
 
 		warnx("using system accel");
 		close(fd);
@@ -882,7 +877,7 @@ Sensors::gyro_init()
 
 		// XXX do the check more elegantly
 
-		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 		/* set the gyro internal sampling rate up to at least 1000Hz */
 		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
@@ -892,7 +887,7 @@ Sensors::gyro_init()
 		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
 			ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
-		#else
+#else
 
 		/* set the gyro internal sampling rate up to at least 760Hz */
 		ioctl(fd, GYROIOCSSAMPLERATE, 760);
@@ -900,7 +895,7 @@ Sensors::gyro_init()
 		/* set the driver to poll at 760Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 760);
 
-		#endif
+#endif
 
 		warnx("using system gyro");
 		close(fd);
@@ -924,15 +919,19 @@ Sensors::mag_init()
 
 
 	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+
 	if (ret == OK) {
 		/* set the pollrate accordingly */
 		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
 	} else {
 		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+
 		/* if the slower sampling rate still fails, something is wrong */
 		if (ret == OK) {
 			/* set the driver to poll also at the slower rate */
 			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+
 		} else {
 			errx(1, "FATAL: mag sampling rate could not be set");
 		}
@@ -941,6 +940,7 @@ Sensors::mag_init()
 
 
 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+
 	if (ret < 0)
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
 	else if (ret == 1)
@@ -993,7 +993,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
 		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
-		vect = _board_rotation*vect;
+		vect = _board_rotation * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
 		raw.accelerometer_m_s2[1] = vect(1);
@@ -1019,7 +1019,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
 		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
-		vect = _board_rotation*vect;
+		vect = _board_rotation * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
 		raw.gyro_rad_s[1] = vect(1);
@@ -1047,9 +1047,9 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
 
 		if (_mag_is_external)
-			vect = _external_mag_rotation*vect;
+			vect = _external_mag_rotation * vect;
 		else
-			vect = _board_rotation*vect;
+			vect = _board_rotation * vect;
 
 		raw.magnetometer_ga[0] = vect(0);
 		raw.magnetometer_ga[1] = vect(1);
@@ -1094,8 +1094,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_counter++;
 
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, 
+		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
-											 			 raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+					      raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {
@@ -1241,7 +1241,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 				/* Save raw voltage values */
 				if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
-					 raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
+					raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
 				}
 
 				/* look for specific channels and process the raw voltage to measurement data */
@@ -1274,7 +1274,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 
 					/* calculate airspeed, raw is the difference from */
-					float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
+					float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f;  //V_ref/4096 * (voltage divider factor)
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
@@ -1306,17 +1306,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 }
 
 void
-Sensors::ppm_poll()
+Sensors::rc_poll()
 {
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
 
-	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	if (rc_updated) {
-	struct pollfd fds[1];
+		/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
-	fds[0].fd = _rc_sub;
-	fds[0].events = POLLIN;
-	/* check non-blocking for new data */
-	int poll_ret = poll(fds, 1, 0);
-
-	if (poll_ret > 0) {
 		struct rc_input_values	rc_input;
 
 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
@@ -1352,7 +1348,7 @@ Sensors::ppm_poll()
 			channel_limit = _rc_max_chan_count;
 
 		/* we are accepting this message */
-		_ppm_last_valid = rc_input.timestamp;
+		_rc_last_valid = rc_input.timestamp;
 
 		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1362,6 +1358,7 @@ Sensors::ppm_poll()
 			 */
 			if (rc_input.values[i] < _parameters.min[i])
 				rc_input.values[i] = _parameters.min[i];
+
 			if (rc_input.values[i] > _parameters.max[i])
 				rc_input.values[i] = _parameters.max[i];
 
@@ -1622,7 +1619,7 @@ Sensors::task_main()
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
 
 		/* Look for new r/c input data */
-		ppm_poll();
+		rc_poll();
 
 		perf_end(_loop_perf);
 	}
@@ -1640,11 +1637,11 @@ Sensors::start()
 
 	/* start the task */
 	_sensors_task = task_spawn_cmd("sensors_task",
-				   SCHED_DEFAULT,
+				       SCHED_DEFAULT,
-				   SCHED_PRIORITY_MAX - 5,
+				       SCHED_PRIORITY_MAX - 5,
-				   2048,
+				       2048,
-				   (main_t)&Sensors::task_main_trampoline,
+				       (main_t)&Sensors::task_main_trampoline,
-				   nullptr);
+				       nullptr);
 
 	if (_sensors_task < 0) {
 		warn("task start failed");

src/modules/uORB/topics/vehicle_control_mode.h

@@ -61,7 +61,6 @@
  */
 struct vehicle_control_mode_s
 {
-	uint16_t counter;   /**< incremented by the writing thread every time new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	bool flag_armed;