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

.gitignore

@@ -38,3 +38,4 @@ tags
 .tags_sorted_by_file
 .pydevproject
 .ropeproject
+*.orig

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -32,9 +32,9 @@ then
 	param set MPC_Z_VEL_D 0.0
 	param set MPC_Z_VEL_MAX 3
 	param set MPC_Z_FF 0.5
-	param set MPC_TILT_MAX 1.0
+	param set MPC_TILTMAX_AIR 45.0
+	param set MPC_TILTMAX_LND 15.0
 	param set MPC_LAND_SPEED 1.0
-	param set MPC_LAND_TILT 0.3
 fi
 
 set PWM_RATE 400

nuttx-configs/px4io-v2/include/board.h

@@ -103,8 +103,6 @@
 #define GPIO_USART2_RTS	0xffffffff
 #undef GPIO_USART2_CK
 #define GPIO_USART2_CK 	0xffffffff
-#undef GPIO_USART3_TX
-#define GPIO_USART3_TX 	0xffffffff
 #undef GPIO_USART3_CK
 #define GPIO_USART3_CK 	0xffffffff
 #undef GPIO_USART3_CTS

nuttx-configs/px4io-v2/nsh/defconfig

@@ -104,9 +104,9 @@ CONFIG_ARMV7M_CMNVECTOR=y
 #   CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
 #
 CONFIG_STM32_DFU=n
-CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_FULL_ENABLE=n
 CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
-CONFIG_STM32_JTAG_SW_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=y
 
 #
 # Individual subsystems can be enabled:

src/drivers/px4io/px4io.cpp

@@ -683,6 +683,25 @@ PX4IO::init()
 
 		/* send command to arm system via command API */
 		vehicle_command_s cmd;
+		/* send this to itself */
+		param_t sys_id_param = param_find("MAV_SYS_ID");
+		param_t comp_id_param = param_find("MAV_COMP_ID");
+
+		int32_t sys_id;
+		int32_t comp_id;
+
+		if (param_get(sys_id_param, &sys_id)) {
+			errx(1, "PRM SYSID");
+		}
+
+		if (param_get(comp_id_param, &comp_id)) {
+			errx(1, "PRM CMPID");
+		}
+
+		cmd.target_system = sys_id;
+		cmd.target_component = comp_id;
+		cmd.source_system = sys_id;
+		cmd.source_component = comp_id;
 		/* request arming */
 		cmd.param1 = 1.0f;
 		cmd.param2 = 0;
@@ -692,10 +711,7 @@ PX4IO::init()
 		cmd.param6 = 0;
 		cmd.param7 = 0;
 		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
-		// cmd.target_system = status.system_id;
+
-		// cmd.target_component = status.component_id;
-		// cmd.source_system = status.system_id;
-		// cmd.source_component = status.component_id;
 		/* ask to confirm command */
 		cmd.confirmation =  1;
 

src/modules/commander/commander.cpp

@@ -484,6 +484,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
             if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
 				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
             } else {
+
+		// Flick to inair restore first if this comes from an onboard system
+		if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
+			status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
+		}
                 transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
                 if (arming_res == TRANSITION_DENIED) {
                     mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");

src/modules/mavlink/mavlink_messages.cpp

@@ -819,11 +819,11 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = status_sub->update(t);
+		bool updated = act_sub->update(t);
-		updated |= pos_sp_triplet_sub->update(t);
+		(void)pos_sp_triplet_sub->update(t);
-		updated |= act_sub->update(t);
+		(void)status_sub->update(t);
 
-		if (updated) {
+		if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
@@ -1339,22 +1339,23 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		(void)range_sub->update(t);
+		if (range_sub->update(t)) {
 
-		uint8_t type;
+			uint8_t type;
 
-		switch (range->type) {
+			switch (range->type) {
-			case RANGE_FINDER_TYPE_LASER:
+				case RANGE_FINDER_TYPE_LASER:
-			type = MAV_DISTANCE_SENSOR_LASER;
+				type = MAV_DISTANCE_SENSOR_LASER;
-			break;
+				break;
+			}
+
+			uint8_t id = 0;
+			uint8_t orientation = 0;
+			uint8_t covariance = 20;
+
+			mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
+				range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
 		}
-
-		uint8_t id = 0;
-		uint8_t orientation = 0;
-		uint8_t covariance = 20;
-
-		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, 
-			range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
 	}
 };
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -217,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			_mavlink->_task_should_exit = true;
 
 		} else {
+
+			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+				warnx("ignoring CMD spoofed with same SYS/COMP ID");
+				return;
+			}
+
 			struct vehicle_command_s vcmd;
 			memset(&vcmd, 0, sizeof(vcmd));
 

src/modules/px4iofirmware/mixer.cpp

@@ -254,10 +254,25 @@ mixer_tick(void)
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
 
+		/* set S.BUS1 or S.BUS2 outputs */
+
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+		}
+
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
 		/* set the disarmed servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
+
+		/* set S.BUS1 or S.BUS2 outputs */
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
+			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
 	}
 }
 

src/modules/px4iofirmware/px4io.h

@@ -219,6 +219,8 @@ extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
+extern bool	sbus1_output(uint16_t *values, uint16_t num_values);
+extern bool	sbus2_output(uint16_t *values, uint16_t num_values);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;

src/modules/px4iofirmware/registers.c

@@ -463,9 +463,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			#ifdef ENABLE_SBUS_OUT
 			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
 
-			/* disable the conflicting options */
+			/* disable the conflicting options with SBUS 1 */
-			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
-				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with SBUS 2 */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
 			}
 			#endif
 

src/modules/px4iofirmware/sbus.c

@@ -93,7 +93,7 @@ int
 sbus_init(const char *device)
 {
 	if (sbus_fd < 0)
-		sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
+		sbus_fd = open(device, O_RDWR | O_NONBLOCK);
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -113,10 +113,21 @@ sbus_init(const char *device)
 	} else {
 		debug("S.Bus: open failed");
 	}
-
 	return sbus_fd;
 }
 
+bool
+sbus1_output(uint16_t *values, uint16_t num_values)
+{
+	write(sbus_fd, 'A', 1);
+}
+
+bool
+sbus2_output(uint16_t *values, uint16_t num_values)
+{
+	write(sbus_fd, 'B', 1);
+}
+
 bool
 sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
 {