Merge remote-tracking branch 'upstream/io_fixes' into new_state_machine

Conflicts:
	src/drivers/px4io/px4io.cpp
	src/modules/commander/commander.c
	src/modules/commander/state_machine_helper.c
	src/modules/commander/state_machine_helper.h
	src/modules/px4iofirmware/mixer.cpp
	src/modules/uORB/topics/actuator_controls.h
	src/modules/uORB/topics/vehicle_status.h

src/drivers/blinkm/blinkm.cpp

@@ -505,15 +505,15 @@ BlinkM::led()
 
 		/* get number of used satellites in navigation */
 		num_of_used_sats = 0;
-		//for(int satloop=0; satloop<20; satloop++) {
+
-		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
+		for (unsigned satloop = 0; satloop < (sizeof(vehicle_gps_position_raw.satellite_used) / sizeof(vehicle_gps_position_raw.satellite_used[0])); satloop++) {
-			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
+			if (vehicle_gps_position_raw.satellite_used[satloop] == 1) {
 				num_of_used_sats++;
 			}
 		}
 
-		if(new_data_vehicle_status || no_data_vehicle_status < 3){
+		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
-			if(num_of_cells == 0) {
+			if (num_of_cells == 0) {
 				/* looking for lipo cells that are connected */
 				printf("<blinkm> checking cells\n");
 				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {

src/drivers/px4io/px4io.cpp

@@ -81,6 +81,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/safety.h>
 #include <debug.h>
 
 #include <mavlink/mavlink_log.h>
@@ -172,6 +173,7 @@ private:
 	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
 	orb_advert_t		_to_outputs;	///< mixed servo outputs topic
 	orb_advert_t		_to_battery;	///< battery status / voltage
+	orb_advert_t		_to_safety;	///< status of safety
 
 	actuator_outputs_s	_outputs;	///< mixed outputs
 	actuator_controls_effective_s _controls_effective; ///< effective controls
@@ -345,6 +347,7 @@ PX4IO::PX4IO() :
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_to_safety(0),
 	_primary_pwm_device(false),
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
@@ -398,6 +401,40 @@ PX4IO::init()
 	 */
 	_retries = 2;
 
+	/* get IO's last seen FMU state */
+	int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
+	if (val == _io_reg_get_error) {
+		mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE");
+	}
+	uint16_t arming = val;
+
+	/* get basic software version */
+	/* basic configuration */
+	usleep(5000);
+
+	unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+	unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION);
+
+	if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) {
+		mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n",
+			proto_version,
+			PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC);
+
+		mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware.");
+		log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC);
+		return 1;
+	}
+
+	if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) {
+		mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n",
+			proto_version,
+			PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC);
+
+		mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware.");
+		log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC);
+		return 1;
+	}
+
 	/* get some parameters */
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
 	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
@@ -423,21 +460,24 @@ PX4IO::init()
 	 * in this case.
 	 */
 
-	uint16_t reg;
+	printf("arming 0x%04x%s%s%s%s\n",
+		arming,
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)    ? " FAILSAFE CUSTOM" : ""));
 
-	/* get IO's last seen FMU state */
-	ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
-	if (ret != OK)
-		return ret;
 
 	/*
 	 * in-air restart is only tried if the IO board reports it is
 	 * already armed, and has been configured for in-air restart
 	 */
-	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
+	if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
-	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+	    (arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+	    	log("INAIR RESTART RECOVERY (needs commander app running)");
 
 		/* WARNING: COMMANDER app/vehicle status must be initialized.
 		 * If this fails (or the app is not started), worst-case IO
@@ -446,7 +486,7 @@ PX4IO::init()
 
 		int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
 		/* fill with initial values, clear updated flag */
-		struct actuator_safety_s armed;
+		struct actuator_safety_s safety;
 		uint64_t try_start_time = hrt_absolute_time();
 		bool updated = false;
 		
@@ -457,7 +497,7 @@ PX4IO::init()
 
 			if (updated) {
 				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
+				orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
 				break;
 			}
 
@@ -491,27 +531,33 @@ PX4IO::init()
 		cmd.confirmation =  1;
 
 		/* send command once */
-		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
 
 		/* spin here until IO's state has propagated into the system */
 		do {
 			orb_check(safety_sub, &updated);
 
 			if (updated) {
-				orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
+				orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
 			}
 
-			/* wait 10 ms */
+			/* wait 50 ms */
-			usleep(10000);
+			usleep(50000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time)/1000 > 2000) {
 				log("failed to recover from in-air restart (2), aborting IO driver init.");
 				return 1;
 			}
 
-		/* keep waiting for state change for 10 s */
+			/* re-send if necessary */
-		} while (!armed.armed);
+			if (!safety.armed) {
+				orb_publish(ORB_ID(vehicle_command), pub, &cmd);
+				log("re-sending arm cmd");
+			}
+
+		/* keep waiting for state change for 2 s */
+		} while (!safety.armed);
 
 	/* regular boot, no in-air restart, init IO */
 	} else {
@@ -549,7 +595,7 @@ PX4IO::init()
 		return -errno;
 	}
 
-	mavlink_log_info(_mavlink_fd, "[IO] init ok");
+	mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version);
 
 	return OK;
 }
@@ -899,14 +945,14 @@ PX4IO::io_handle_status(uint16_t status)
 	 */
 
 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
 		&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
 
 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 
 		/* set the sync flag */
@@ -921,6 +967,25 @@ PX4IO::io_handle_status(uint16_t status)
 		_status = status;
 	}
 
+	/**
+	 * Get and handle the safety status
+	 */
+	struct safety_s safety;
+	safety.timestamp = hrt_absolute_time();
+
+	if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
+		safety.status = SAFETY_STATUS_UNLOCKED;
+	} else {
+		safety.status = SAFETY_STATUS_SAFE;
+	}
+
+	/* lazily publish the safety status */
+	if (_to_safety > 0) {
+		orb_publish(ORB_ID(safety), _to_safety, &safety);
+	} else {
+		_to_safety = orb_advertise(ORB_ID(safety), &safety);
+	}
+
 	return ret;
 }
 
@@ -950,7 +1015,7 @@ PX4IO::io_get_status()
 
 	io_handle_status(regs[0]);
 	io_handle_alarms(regs[1]);
-
+	
 	/* only publish if battery has a valid minimum voltage */
 	if (regs[2] > 3300) {
 		battery_status_s	battery_status;
@@ -984,6 +1049,7 @@ PX4IO::io_get_status()
 			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
 		}
 	}
+
 	return ret;
 }
 
@@ -1221,6 +1287,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 	uint16_t value;
 
 	ret = io_reg_get(page, offset, &value, 1);
+
 	if (ret)
 		return ret;
 	value &= ~clearbits;
@@ -1311,7 +1378,8 @@ PX4IO::print_status()
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
 	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
-		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
@@ -1672,6 +1740,11 @@ test(void)
 	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
 		err(1, "failed to get servo count");
 
+	/* tell IO that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+
 	if (ioctl(fd, PWM_SERVO_ARM, 0))
 		err(1, "failed to arm servos");
 
@@ -1720,7 +1793,7 @@ test(void)
 		/* Check if user wants to quit */
 		char c;
 		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
+			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
 				close(console);
 				exit(0);

src/modules/commander/commander.c

@@ -79,6 +79,7 @@
 #include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/safety.h>
 #include <mavlink/mavlink_log.h>
 
 #include <drivers/drv_led.h>
@@ -1229,6 +1230,10 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* set battery warning flag */
 	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+	/* set safety device detection flag */
+	
+	/* XXX do we need this? */
+	//current_status.flag_safety_present = false;
 
 	// XXX for now just set sensors as initialized
 	current_status.condition_system_sensors_initialized = true;
@@ -1352,7 +1357,6 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&battery, 0, sizeof(battery));
 	battery.voltage_v = 0.0f;
 
-
 	// uint8_t vehicle_state_previous = current_status.state_machine;
 	float voltage_previous = 0.0f;
 
@@ -1374,6 +1378,39 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* Get current values */
 		bool new_data;
+
+		/* update parameters */
+		orb_check(param_changed_sub, &new_data);
+
+		if (new_data || param_init_forced) {
+			param_init_forced = false;
+			/* parameters changed */
+			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
+
+			/* update parameters */
+			if (!safety.armed) {
+				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
+					warnx("failed setting new system type");
+				}
+
+				/* disable manual override for all systems that rely on electronic stabilization */
+				if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
+				    current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
+				    current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+					current_status.flag_external_manual_override_ok = false;
+
+				} else {
+					current_status.flag_external_manual_override_ok = true;
+				}
+
+				/* check and update system / component ID */
+				param_get(_param_system_id, &(current_status.system_id));
+				param_get(_param_component_id, &(current_status.component_id));
+
+			}
+		}
+
 		orb_check(sp_man_sub, &new_data);
 
 		if (new_data) {
@@ -1408,7 +1445,6 @@ int commander_thread_main(int argc, char *argv[])
 			/* handle it */
 			handle_command(status_pub, &current_status, &cmd, safety_pub, &safety);
 		}
-
 		
 
 		/* update parameters */
@@ -1686,7 +1722,9 @@ int commander_thread_main(int argc, char *argv[])
 		// 	state_changed = true;
 		// }
 
-		if (orb_check(gps_sub, &new_data)) {
+		orb_check(ORB_ID(vehicle_gps_position), &new_data);
+		if (new_data) {
+
 
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 

src/modules/commander/state_machine_helper.c

@@ -54,9 +54,25 @@
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
+#include "commander.h"
+
+bool is_multirotor(const struct vehicle_status_s *current_status)
+{
+	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+}
+
+bool is_rotary_wing(const struct vehicle_status_s *current_status)
+{
+	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
+	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+}
 
 int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) {
-	
+
+
 	int ret = ERROR;
 
 	/* only check transition if the new state is actually different from the current one */
@@ -717,7 +733,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 // }
 
 
-
 ///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
 //
 //int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)

src/modules/commander/state_machine_helper.h

@@ -49,8 +49,13 @@
 #include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/vehicle_control_mode.h>
 
+
 void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
+bool is_multirotor(const struct vehicle_status_s *current_status);
+
+bool is_rotary_wing(const struct vehicle_status_s *current_status);
+
 //int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
 
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
@@ -59,4 +64,4 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 
 int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state);
 
-#endif /* STATE_MACHINE_HELPER_H_ */
+#endif /* STATE_MACHINE_HELPER_H_ */

src/modules/mavlink/orb_listener.c

@@ -659,7 +659,7 @@ uorb_receive_thread(void *arg)
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+			/* silent */
 
 		} else if (poll_ret < 0) {
 			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");

src/modules/px4iofirmware/mixer.cpp

@@ -62,7 +62,7 @@ extern "C" {
 /*
  * Time that the ESCs need to initialize
  */
- #define ESC_INIT_TIME_US		2000000
+ #define ESC_INIT_TIME_US		1000000
 
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
@@ -74,8 +74,9 @@ extern "C" {
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
 
-static uint64_t time_armed;
+static uint64_t esc_init_time;
-static bool init_complete = false;
+static bool esc_init_active = false;
+static bool esc_init_done = false;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -106,7 +107,7 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			isr_debug(1, "AP RX timeout");
 		}
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
 	} else {
@@ -120,12 +121,11 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
-	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	/* do not mix if RAW_PWM mode is on and FMU is good */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
-	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values or
+		/* don't actually mix anything - we already have raw PWM values */
-		 not a valid mixer. */
 		source = MIX_NONE;
 
 	} else {
@@ -175,28 +175,42 @@ mixer_tick(void)
 		float	outputs[IO_SERVO_COUNT];
 		unsigned mixed;
 
+		/* after arming, some ESCs need an initalization period, count the time from here */
+		if (mixer_servos_armed && !esc_init_done && !esc_init_active) {
+			esc_init_time = hrt_absolute_time();
+			esc_init_active = true;
+			isr_debug(1, "start counting now");
+		}
+
+		/* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */
+		if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) {
+			esc_init_active = false;
+			esc_init_done = true;
+			isr_debug(1, "time is up");
+		}
+
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
 
-		if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) {
-			init_complete = true;
-		}
-
 		/* scale to PWM and update the servo outputs as required */
 		for (unsigned i = 0; i < mixed; i++) {
 
 			/* save actuator values for FMU readback */
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
-			/* scale to control range after init time */
+			/* XXX maybe this check for an armed FMU could be achieved a little less messy */
-			if (init_complete) {
+			if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) {
+				r_page_servos[i] = r_page_servo_failsafe[i];
+			}
+			/* during ESC initialization, use low PWM */
+			else if (esc_init_active) {
+				r_page_servos[i] = (outputs[i] * 600 + 1500);
+			
+			/* afterwards use min and max values */
+			} else {
 				r_page_servos[i] = (outputs[i]
 					* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
 					+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
-
-			/* but use init range from 900 to 2100 right after arming */
-			} else {
-				r_page_servos[i] = (outputs[i] * 600 + 1500);
 			}
 
 		}
@@ -214,26 +228,31 @@ mixer_tick(void)
 	 * here.
 	 */
 	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
 		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
-		/* FMU is available or FMU is not available but override is an option */
+		/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && 
-		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+			/* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && 
+				/* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) ||
+			
+			/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) )
 	);
 
+
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
-		time_armed = hrt_absolute_time();
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
-		init_complete = false;
+		isr_debug(5, "> armed");
 
 	} else if (!should_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
-		init_complete = false;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+		isr_debug(5, "> disarmed");
+
+		esc_init_active = false;
+		isr_debug(1, "disarming, and init aborted");
 	}
 
 	if (mixer_servos_armed) {
@@ -288,9 +307,8 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while fully armed */
+	/* do not allow a mixer change while outputs armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
-	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 		return;
 	}
 
@@ -367,6 +385,7 @@ mixer_set_failsafe()
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
+
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
 		return;

src/modules/px4iofirmware/protocol.h

@@ -75,10 +75,13 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC	2
+#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC	2
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
-#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers */
-#define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
 #define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
@@ -93,7 +96,7 @@
 #define PX4IO_P_STATUS_CPULOAD			1
 
 #define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
 #define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
@@ -105,6 +108,7 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */

src/modules/px4iofirmware/registers.c

@@ -57,8 +57,8 @@ static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
  * Static configuration parameters.
  */
 static const uint16_t	r_page_config[] = {
-	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC,
-	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC,
 	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= 3,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
@@ -146,7 +146,8 @@ volatile uint16_t	r_page_setup[] =
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
-					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
+					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM
 #define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -377,9 +378,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			// XXX do not reset IO's safety state by FMU for now
-			}
+			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			// }
 
 			r_setup_arming = value;
 
@@ -427,9 +430,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 	case PX4IO_PAGE_RC_CONFIG: {
 
-		/* do not allow a RC config change while fully armed */
+		/* do not allow a RC config change while outputs armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
-		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 			break;
 		}
 

src/modules/px4iofirmware/safety.c

@@ -110,7 +110,7 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* switch to armed state */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
-	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 

src/modules/uORB/objects_common.cpp

@@ -168,3 +168,7 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
+
+/* status of the system safety device */
+#include "topics/safety.h"
+ORB_DEFINE(safety, struct safety_s);

src/modules/uORB/topics/safety.h

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file safety.h
+ *
+ * Status of an attached safety device
+ */
+
+#ifndef TOPIC_SAFETY_H
+#define TOPIC_SAFETY_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+enum SAFETY_STATUS {
+	SAFETY_STATUS_NOT_PRESENT,
+	SAFETY_STATUS_SAFE,
+	SAFETY_STATUS_UNLOCKED
+};
+
+struct safety_s {
+	uint64_t timestamp;			/**< output timestamp in us since system boot */
+	enum SAFETY_STATUS status;
+};
+
+/* actuator output sets; this list can be expanded as more drivers emerge */
+ORB_DECLARE(safety);
+
+#endif /* TOPIC_SAFETY_H */

src/systemcmds/pwm/pwm.c

@@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[])
 	}
 
 	/* iterate remaining arguments */
-	unsigned channel = 0;
+	unsigned nchannels = 0;
+	unsigned channel[8] = {0};
 	while (argc--) {
 		const char *arg = argv[0];
 		argv++;
@@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[])
 		}
 		unsigned pwm_value = strtol(arg, &ep, 0);
 		if (*ep == '\0') {
-			ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
+			if (nchannels > sizeof(channel) / sizeof(channel[0]))
-			if (ret != OK)
+				err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
-				err(1, "PWM_SERVO_SET(%d)", channel);
+
-			channel++;
+			channel[nchannels] = pwm_value;
+			nchannels++;
+
 			continue;
 		}
-		usage("unrecognised option");
+		usage("unrecognized option");
 	}
 
 	/* print verbose info */
@@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[])
 		}
 		fflush(stdout);
 	}
+
+	/* perform PWM output */
+	if (nchannels) {
+
+		/* Open console directly to grab CTRL-C signal */
+		int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+		if (!console)
+			err(1, "failed opening console");
+
+		warnx("Press CTRL-C or 'c' to abort.");
+
+		while (1) {
+			for (int i = 0; i < nchannels; i++) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+
+			/* abort on user request */
+			char c;
+			if (read(console, &c, 1) == 1) {
+				if (c == 0x03 || c == 0x63 || c == 'q') {
+					warnx("User abort\n");
+					close(console);
+					exit(0);
+				}
+			}
+
+			/* rate limit to ~ 20 Hz */
+			usleep(50000);
+		}
+	}
+
 	exit(0);
 }