Dropped superseded safety topic, added warning tones before arming

5 files changed, 53 insertions, 91 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 6d3c32ed9..df2f18270 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -81,7 +81,6 @@
 #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>
@@ -990,20 +989,24 @@ PX4IO::io_handle_status(uint16_t status)
 	/**
 	 * Get and handle the safety status
 	 */
-	struct safety_s safety;
+	struct actuator_safety_s safety;
 	safety.timestamp = hrt_absolute_time();
 
+	orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
+
 	if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
-		safety.status = SAFETY_STATUS_UNLOCKED;
+		safety.safety_off = true;
+		safety.safety_switch_available = true;
 	} else {
-		safety.status = SAFETY_STATUS_SAFE;
+		safety.safety_off = false;
+		safety.safety_switch_available = true;
 	}
 
 	/* lazily publish the safety status */
 	if (_to_safety > 0) {
-		orb_publish(ORB_ID(safety), _to_safety, &safety);
+		orb_publish(ORB_ID(actuator_safety), _to_safety, &safety);
 	} else {
-		_to_safety = orb_advertise(ORB_ID(safety), &safety);
+		_to_safety = orb_advertise(ORB_ID(actuator_safety), &safety);
 	}
 
 	return ret;
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index 9d3adaa1d..4c884aed3 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *           Lorenz Meier <lm@inf.ethz.ch>
  *           Thomas Gubler <thomasgubler@student.ethz.ch>
@@ -79,7 +79,6 @@
 #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>
@@ -1159,6 +1158,9 @@ int commander_thread_main(int argc, char *argv[])
 	commander_initialized = false;
 	bool home_position_set = false;
 
+	bool battery_tune_played = false;
+	bool arm_tune_played = false;
+
 	/* set parameters */
 	failsafe_lowlevel_timeout_ms = 0;
 	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
@@ -1248,6 +1250,9 @@ int commander_thread_main(int argc, char *argv[])
 
 	safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
 
+	/* but also subscribe to it */
+	int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
+
 	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);	
 
 	/* home position */
@@ -1477,6 +1482,13 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* update safety topic */
+		orb_check(safety_sub, &new_data);
+
+		if (new_data) {
+			orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
+		}
+
 		/* update global position estimate */
 		orb_check(global_position_sub, &new_data);
 
@@ -1573,25 +1585,6 @@ int commander_thread_main(int argc, char *argv[])
 			last_idle_time = system_load.tasks[0].total_runtime;
 		}
 
-		// // XXX Export patterns and threshold to parameters
-		/* Trigger audio event for low battery */
-		if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
-			/* For less than 10%, start be really annoying at 5 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			ioctl(buzzer, TONE_SET_ALARM, 3);
-
-		} else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
-			/* For less than 20%, start be slightly annoying at 1 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			tune_positive();
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-		}
-
 		/* Check battery voltage */
 		/* write to sys_status */
 		if (battery_voltage_valid) {
@@ -2194,6 +2187,8 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 
+
+
 		current_status.counter++;
 		current_status.timestamp = hrt_absolute_time();
 
@@ -2219,6 +2214,33 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* Store old modes to detect and act on state transitions */
 		voltage_previous = current_status.voltage_battery;
+
+
+		/* play tone according to evaluation result */
+		/* check if we recently armed */
+		if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) {
+			ioctl(buzzer, TONE_SET_ALARM, 12);
+			arm_tune_played = true;
+
+		// // XXX Export patterns and threshold to parameters
+		/* Trigger audio event for low battery */
+		} else if (bat_remain < 0.1f && battery_voltage_valid) {
+			ioctl(buzzer, TONE_SET_ALARM, 14);
+			battery_tune_played = true;
+		} else if (bat_remain < 0.2f && battery_voltage_valid) {
+			ioctl(buzzer, TONE_SET_ALARM, 13);
+			battery_tune_played = true;
+		} else if(battery_tune_played) {
+			ioctl(buzzer, TONE_SET_ALARM, 0);
+			battery_tune_played = false;
+		}
+
+		/* reset arm_tune_played when disarmed */
+		if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
+			arm_tune_played = false;
+		}
+
+
 		/* XXX use this voltage_previous */
 		fflush(stdout);
 		counter++;
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index b5dafd0ca..b602b1714 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -169,6 +169,3 @@ 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);
diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h
index 3a107d41a..c431217ab 100644
--- a/src/modules/uORB/topics/actuator_safety.h
+++ b/src/modules/uORB/topics/actuator_safety.h
@@ -53,7 +53,7 @@
 struct actuator_safety_s {
 
 	uint64_t	timestamp;
-
+	bool	safety_switch_available; /**< Set to true if a safety switch is connected */
 	bool	safety_off;	/**< Set to true if safety is off */
 	bool	armed;		/**< Set to true if system is armed */
 	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h
deleted file mode 100644
index 19e8e8d45..000000000
--- a/src/modules/uORB/topics/safety.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/****************************************************************************
- *
- *   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 */