Merge branch 'master' into inav_sonar_indep

ROMFS/px4fmu_common/init.d/10_dji_f330

@@ -10,12 +10,12 @@ then
 	# Set all params here, then disable autoconfig
 	param set SYS_AUTOCONFIG 0
 
-	param set MC_ATTRATE_D 0.002
+	param set MC_ATTRATE_D 0.004
 	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_P 0.09
+	param set MC_ATTRATE_P 0.12
 	param set MC_ATT_D 0.0
 	param set MC_ATT_I 0.0
-	param set MC_ATT_P 6.8
+	param set MC_ATT_P 7.0
 	param set MC_YAWPOS_D 0.0
 	param set MC_YAWPOS_I 0.0
 	param set MC_YAWPOS_P 2.0

ROMFS/px4fmu_common/init.d/16_3dr_iris

@@ -10,12 +10,12 @@ then
 	# Set all params here, then disable autoconfig
 	param set SYS_AUTOCONFIG 0
 
-	param set MC_ATTRATE_D 0.006
+	param set MC_ATTRATE_D 0.004
 	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_P 0.17
+	param set MC_ATTRATE_P 0.13
 	param set MC_ATT_D 0.0
 	param set MC_ATT_I 0.0
-	param set MC_ATT_P 5.0
+	param set MC_ATT_P 9.0
 	param set MC_YAWPOS_D 0.0
 	param set MC_YAWPOS_I 0.15
 	param set MC_YAWPOS_P 0.5

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/drivers/px4io/px4io_uploader.cpp

@@ -237,7 +237,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	fds[0].fd = _io_fd;
 	fds[0].events = POLLIN;
 
-	/* wait 100 ms for a character */
+	/* wait <timout> ms for a character */
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {

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

@@ -1194,31 +1194,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

@@ -513,7 +513,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,8 +646,8 @@ 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;
 
 			/* set quaternion */
@@ -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);
 		}
@@ -219,7 +223,7 @@ handle_message(mavlink_message_t *msg)
 			/* switch to a receiving link mode */
 			gcs_link = false;
 
-			/* 
+			/*
 			 * rate control mode - defined by MAVLink
 			 */
 
@@ -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/multirotor_att_control/multirotor_att_control_main.c

@@ -89,18 +89,18 @@ static int
 mc_thread_main(int argc, char *argv[])
 {
 	/* declare and safely initialize all structs */
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
 	memset(&att_sp, 0, sizeof(att_sp));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
 	struct offboard_control_setpoint_s offboard_sp;
 	memset(&offboard_sp, 0, sizeof(offboard_sp));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
 	struct vehicle_rates_setpoint_s rates_sp;
 	memset(&rates_sp, 0, sizeof(rates_sp));
 	struct vehicle_status_s status;
@@ -108,29 +108,16 @@ mc_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
-	/* subscribe to attitude, motor setpoints and system state */
+	/* subscribe */
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/*
-	 * Do not rate-limit the loop to prevent aliasing
-	 * if rate-limiting would be desired later, the line below would
-	 * enable it.
-	 *
-	 * rate-limit the attitude subscription to 200Hz to pace our loop
-	 * orb_set_interval(att_sub, 5);
-	 */
-	struct pollfd fds[2] = {
-		{ .fd = att_sub, .events = POLLIN },
-		{ .fd = param_sub, .events = POLLIN }
-	};
 
 	/* publish actuator controls */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@@ -146,233 +133,246 @@ mc_thread_main(int argc, char *argv[])
 	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
 	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
 
-	/* welcome user */
 	warnx("starting");
 
 	/* store last control mode to detect mode switches */
 	bool control_yaw_position = true;
 	bool reset_yaw_sp = true;
 
+	struct pollfd fds[1] = {
+		{ .fd = vehicle_attitude_sub, .events = POLLIN },
+	};
+
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
-		int ret = poll(fds, 2, 500);
+		int ret = poll(fds, 1, 500);
 
 		if (ret < 0) {
 			/* poll error, count it in perf */
 			perf_count(mc_err_perf);
 
-		} else if (ret == 0) {
+		} else if (ret > 0) {
-			/* no return value, ignore */
+			/* only run controller if attitude changed */
-		} else {
+			perf_begin(mc_loop_perf);
 
-			/* only update parameters if they changed */
+			/* attitude */
-			if (fds[1].revents & POLLIN) {
+			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-				/* read from param to clear updated flag */
+
+			bool updated;
+
+			/* parameters */
+			orb_check(parameter_update_sub, &updated);
+
+			if (updated) {
 				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), param_sub, &update);
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
-
 				/* update parameters */
 			}
 
-			/* only run controller if attitude changed */
+			/* control mode */
-			if (fds[0].revents & POLLIN) {
+			orb_check(vehicle_control_mode_sub, &updated);
 
-				perf_begin(mc_loop_perf);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode);
+			}
 
-				/* get a local copy of system state */
+			/* manual control setpoint */
-				bool updated;
+			orb_check(manual_control_setpoint_sub, &updated);
-				orb_check(control_mode_sub, &updated);
 
-				if (updated) {
+			if (updated) {
-					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+				orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
+			}
+
+			/* attitude setpoint */
+			orb_check(vehicle_attitude_setpoint_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
+			}
+
+			/* offboard control setpoint */
+			orb_check(offboard_control_setpoint_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp);
+			}
+
+			/* vehicle status */
+			orb_check(vehicle_status_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
+			}
+
+			/* sensors */
+			orb_check(sensor_combined_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			}
+
+			/* set flag to safe value */
+			control_yaw_position = true;
+
+			/* reset yaw setpoint if not armed */
+			if (!control_mode.flag_armed) {
+				reset_yaw_sp = true;
+			}
+
+			/* define which input is the dominating control input */
+			if (control_mode.flag_control_offboard_enabled) {
+				/* offboard inputs */
+				if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
+					rates_sp.roll = offboard_sp.p1;
+					rates_sp.pitch = offboard_sp.p2;
+					rates_sp.yaw = offboard_sp.p3;
+					rates_sp.thrust = offboard_sp.p4;
+					rates_sp.timestamp = hrt_absolute_time();
+					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+
+				} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
+					att_sp.roll_body = offboard_sp.p1;
+					att_sp.pitch_body = offboard_sp.p2;
+					att_sp.yaw_body = offboard_sp.p3;
+					att_sp.thrust = offboard_sp.p4;
+					att_sp.timestamp = hrt_absolute_time();
+					/* publish the result to the vehicle actuators */
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 				}
 
-				/* get a local copy of manual setpoint */
+				/* reset yaw setpoint after offboard control */
-				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+				reset_yaw_sp = true;
-				/* get a local copy of attitude */
-				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-				/* get a local copy of attitude setpoint */
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
-				/* get a local copy of rates setpoint */
-				orb_check(setpoint_sub, &updated);
 
-				if (updated) {
+			} else if (control_mode.flag_control_manual_enabled) {
-					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
+				/* manual input */
-				}
+				if (control_mode.flag_control_attitude_enabled) {
+					/* control attitude, update attitude setpoint depending on mode */
+					if (att_sp.thrust < 0.1f) {
+						/* no thrust, don't try to control yaw */
+						rates_sp.yaw = 0.0f;
+						control_yaw_position = false;
 
-				/* get a local copy of status */
+						if (status.condition_landed) {
-				orb_check(status_sub, &updated);
+							/* reset yaw setpoint if on ground */
-
+							reset_yaw_sp = true;
-				if (updated) {
-					orb_copy(ORB_ID(vehicle_status), status_sub, &status);
-				}
-
-				/* get a local copy of the current sensor values */
-				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-
-				/* set flag to safe value */
-				control_yaw_position = true;
-
-				/* reset yaw setpoint if not armed */
-				if (!control_mode.flag_armed) {
-					reset_yaw_sp = true;
-				}
-
-				/* define which input is the dominating control input */
-				if (control_mode.flag_control_offboard_enabled) {
-					/* offboard inputs */
-					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-						rates_sp.roll = offboard_sp.p1;
-						rates_sp.pitch = offboard_sp.p2;
-						rates_sp.yaw = offboard_sp.p3;
-						rates_sp.thrust = offboard_sp.p4;
-						rates_sp.timestamp = hrt_absolute_time();
-						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-
-					} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
-						att_sp.roll_body = offboard_sp.p1;
-						att_sp.pitch_body = offboard_sp.p2;
-						att_sp.yaw_body = offboard_sp.p3;
-						att_sp.thrust = offboard_sp.p4;
-						att_sp.timestamp = hrt_absolute_time();
-						/* publish the result to the vehicle actuators */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-					}
-
-					/* reset yaw setpoint after offboard control */
-					reset_yaw_sp = true;
-
-				} else if (control_mode.flag_control_manual_enabled) {
-					/* manual input */
-					if (control_mode.flag_control_attitude_enabled) {
-						/* control attitude, update attitude setpoint depending on mode */
-						if (att_sp.thrust < 0.1f) {
-							/* no thrust, don't try to control yaw */
-							rates_sp.yaw = 0.0f;
-							control_yaw_position = false;
-
-							if (status.condition_landed) {
-								/* reset yaw setpoint if on ground */
-								reset_yaw_sp = true;
-							}
-
-						} else {
-							/* only move yaw setpoint if manual input is != 0 */
-							if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
-								/* control yaw rate */
-								control_yaw_position = false;
-								rates_sp.yaw = manual.yaw;
-								reset_yaw_sp = true;	// has no effect on control, just for beautiful log
-
-							} else {
-								control_yaw_position = true;
-							}
 						}
 
-						if (!control_mode.flag_control_velocity_enabled) {
-							/* update attitude setpoint if not in position control mode */
-							att_sp.roll_body = manual.roll;
-							att_sp.pitch_body = manual.pitch;
-
-							if (!control_mode.flag_control_climb_rate_enabled) {
-								/* pass throttle directly if not in altitude control mode */
-								att_sp.thrust = manual.throttle;
-							}
-						}
-
-						/* reset yaw setpint to current position if needed */
-						if (reset_yaw_sp) {
-							att_sp.yaw_body = att.yaw;
-							reset_yaw_sp = false;
-						}
-
-						if (motor_test_mode) {
-							printf("testmode");
-							att_sp.roll_body = 0.0f;
-							att_sp.pitch_body = 0.0f;
-							att_sp.yaw_body = 0.0f;
-							att_sp.thrust = 0.1f;
-						}
-
-						att_sp.timestamp = hrt_absolute_time();
-
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
 					} else {
-						/* manual rate inputs (ACRO), from RC control or joystick */
+						/* only move yaw setpoint if manual input is != 0 */
-						if (control_mode.flag_control_rates_enabled) {
+						if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
-							rates_sp.roll = manual.roll;
+							/* control yaw rate */
-							rates_sp.pitch = manual.pitch;
+							control_yaw_position = false;
 							rates_sp.yaw = manual.yaw;
-							rates_sp.thrust = manual.throttle;
+							reset_yaw_sp = true;	// has no effect on control, just for beautiful log
-							rates_sp.timestamp = hrt_absolute_time();
-						}
 
-						/* reset yaw setpoint after ACRO */
+						} else {
-						reset_yaw_sp = true;
+							control_yaw_position = true;
+						}
 					}
 
+					if (!control_mode.flag_control_velocity_enabled) {
+						/* update attitude setpoint if not in position control mode */
+						att_sp.roll_body = manual.roll;
+						att_sp.pitch_body = manual.pitch;
+
+						if (!control_mode.flag_control_climb_rate_enabled) {
+							/* pass throttle directly if not in altitude control mode */
+							att_sp.thrust = manual.throttle;
+						}
+					}
+
+					/* reset yaw setpint to current position if needed */
+					if (reset_yaw_sp) {
+						att_sp.yaw_body = att.yaw;
+						reset_yaw_sp = false;
+					}
+
+					if (motor_test_mode) {
+						printf("testmode");
+						att_sp.roll_body = 0.0f;
+						att_sp.pitch_body = 0.0f;
+						att_sp.yaw_body = 0.0f;
+						att_sp.thrust = 0.1f;
+					}
+
+					att_sp.timestamp = hrt_absolute_time();
+
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
 				} else {
-					if (!control_mode.flag_control_auto_enabled) {
+					/* manual rate inputs (ACRO), from RC control or joystick */
-						/* no control, try to stay on place */
+					if (control_mode.flag_control_rates_enabled) {
-						if (!control_mode.flag_control_velocity_enabled) {
+						rates_sp.roll = manual.roll;
-							/* no velocity control, reset attitude setpoint */
+						rates_sp.pitch = manual.pitch;
-							att_sp.roll_body = 0.0f;
+						rates_sp.yaw = manual.yaw;
-							att_sp.pitch_body = 0.0f;
+						rates_sp.thrust = manual.throttle;
-							att_sp.timestamp = hrt_absolute_time();
+						rates_sp.timestamp = hrt_absolute_time();
-							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-						}
 					}
 
-					/* reset yaw setpoint after non-manual control */
+					/* reset yaw setpoint after ACRO */
 					reset_yaw_sp = true;
 				}
 
-				/* check if we should we reset integrals */
+			} else {
-				bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
+				if (!control_mode.flag_control_auto_enabled) {
-
+					/* no control, try to stay on place */
-				/* run attitude controller if needed */
+					if (!control_mode.flag_control_velocity_enabled) {
-				if (control_mode.flag_control_attitude_enabled) {
+						/* no velocity control, reset attitude setpoint */
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
+						att_sp.roll_body = 0.0f;
-					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+						att_sp.pitch_body = 0.0f;
-				}
+						att_sp.timestamp = hrt_absolute_time();
-
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-				/* measure in what intervals the controller runs */
-				perf_count(mc_interval_perf);
-
-				/* run rates controller if needed */
-				if (control_mode.flag_control_rates_enabled) {
-					/* get current rate setpoint */
-					bool rates_sp_updated = false;
-					orb_check(rates_sp_sub, &rates_sp_updated);
-
-					if (rates_sp_updated) {
-						orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
 					}
-
-					/* apply controller */
-					float rates[3];
-					rates[0] = att.rollspeed;
-					rates[1] = att.pitchspeed;
-					rates[2] = att.yawspeed;
-					multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
-
-				} else {
-					/* rates controller disabled, set actuators to zero for safety */
-					actuators.control[0] = 0.0f;
-					actuators.control[1] = 0.0f;
-					actuators.control[2] = 0.0f;
-					actuators.control[3] = 0.0f;
 				}
 
-				actuators.timestamp = hrt_absolute_time();
+				/* reset yaw setpoint after non-manual control */
-				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+				reset_yaw_sp = true;
+			}
 
-				perf_end(mc_loop_perf);
+			/* check if we should we reset integrals */
-			} /* end of poll call for attitude updates */
+			bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
-		} /* end of poll return value check */
+
+			/* run attitude controller if needed */
+			if (control_mode.flag_control_attitude_enabled) {
+				multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
+				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+			}
+
+			/* measure in what intervals the controller runs */
+			perf_count(mc_interval_perf);
+
+			/* run rates controller if needed */
+			if (control_mode.flag_control_rates_enabled) {
+				/* get current rate setpoint */
+				bool rates_sp_updated = false;
+				orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated);
+
+				if (rates_sp_updated) {
+					orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
+				}
+
+				/* apply controller */
+				float rates[3];
+				rates[0] = att.rollspeed;
+				rates[1] = att.pitchspeed;
+				rates[2] = att.yawspeed;
+				multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
+
+			} else {
+				/* rates controller disabled, set actuators to zero for safety */
+				actuators.control[0] = 0.0f;
+				actuators.control[1] = 0.0f;
+				actuators.control[2] = 0.0f;
+				actuators.control[3] = 0.0f;
+			}
+
+			actuators.timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+
+			perf_end(mc_loop_perf);
+		}
 	}
 
 	warnx("stopping, disarming motors");
@@ -383,10 +383,9 @@ mc_thread_main(int argc, char *argv[])
 
 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-
+	close(vehicle_attitude_sub);
-	close(att_sub);
+	close(vehicle_control_mode_sub);
-	close(control_mode_sub);
+	close(manual_control_setpoint_sub);
-	close(manual_sub);
 	close(actuator_pub);
 	close(att_sp_pub);
 

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>
@@ -105,22 +104,22 @@
  * IN13 - aux1
  * IN14 - aux2
  * IN15 - pressure sensor
- * 
+ *
  * IO:
  * IN4 - servo supply rail
  * IN5 - analog RSSI
  */
 
 #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 */
@@ -503,7 +498,7 @@ Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
-	_ppm_last_valid(0),
+	_rc_last_valid(0),
 
 	_fd_adc(-1),
 	_last_adc(0),
@@ -534,8 +529,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)
 {
 
@@ -663,9 +658,9 @@ 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++) {
 
@@ -677,19 +672,19 @@ Sensors::parameters_update()
 
 		tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
-        
+
 		/* 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 */
@@ -820,7 +815,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;
@@ -831,7 +826,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
@@ -849,7 +844,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);
@@ -857,7 +852,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);
@@ -865,10 +860,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);
@@ -890,7 +885,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)
@@ -900,7 +895,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);
@@ -908,7 +903,7 @@ Sensors::gyro_init()
 		/* set the driver to poll at 760Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 760);
 
-		#endif
+#endif
 
 		warnx("using system gyro");
 		close(fd);
@@ -932,23 +927,28 @@ 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");
 		}
 	}
 
-	
+
 
 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+
 	if (ret < 0)
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
 	else if (ret == 1)
@@ -1001,7 +1001,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);
@@ -1027,7 +1027,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);
@@ -1055,9 +1055,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);
@@ -1102,8 +1102,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) {
@@ -1244,12 +1244,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 		int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
 
 		for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-			
+
 			if (ret >= (int)sizeof(buf_adc[0])) {
 
 				/* 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 */
@@ -1277,12 +1277,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 						} else {
 							_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
 						}
-					} 
+					}
 
 				} 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
@@ -1314,17 +1314,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);
@@ -1362,7 +1358,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++) {
@@ -1372,6 +1368,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];
 
@@ -1639,7 +1636,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);
 	}
@@ -1657,11 +1654,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;