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

5 files changed, 45 insertions, 15 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f7e93db6c..8323932c7 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1253,6 +1253,8 @@ int commander_thread_main(int argc, char *argv[])
 	current_status.flag_external_manual_override_ok = true;
 	/* flag position info as bad, do not allow auto mode */
 	current_status.flag_vector_flight_mode_ok = false;
+	/* set battery warning flag */
+	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
 
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1510,6 +1512,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				low_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
+				current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
 			}
 
 			low_voltage_counter++;
@@ -1520,6 +1523,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				critical_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
+				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
 				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
 			}
 
diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp
index 695653714..03da35a0a 100644
--- a/apps/controllib/fixedwing.cpp
+++ b/apps/controllib/fixedwing.cpp
@@ -293,17 +293,20 @@ void BlockMultiModeBacksideAutopilot::update()
 	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
 		_actuators.control[i] = 0.0f;
 
+	// only update guidance in auto mode
+	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+	}
+
+	// XXX handle STABILIZED (loiter on spot) as well
+	// once the system switches from manual or auto to stabilized
+	// the setpoint should update to loitering around this position
+
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
-		_stabilization.update(
-			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
-			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
-		_actuators.control[CH_AIL] = _stabilization.getAileron();
-		_actuators.control[CH_ELV] = _stabilization.getElevator();
-		_actuators.control[CH_RDR] = _stabilization.getRudder();
-		_actuators.control[CH_THR] = _manual.throttle;
+	if (_status.state_machine == SYSTEM_STATE_AUTO ||
+		_status.state_machine == SYSTEM_STATE_STABILIZED) {
 
-	} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
 		// update guidance
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
 
@@ -325,10 +328,23 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
 
 	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-		_actuators.control[CH_AIL] = _manual.roll;
-		_actuators.control[CH_ELV] = _manual.pitch;
-		_actuators.control[CH_RDR] = _manual.yaw;
-		_actuators.control[CH_THR] = _manual.throttle;
+
+		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+
+			_actuators.control[CH_AIL] = _manual.roll;
+			_actuators.control[CH_ELV] = _manual.pitch;
+			_actuators.control[CH_RDR] = _manual.yaw;
+			_actuators.control[CH_THR] = _manual.throttle;
+
+		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+			_stabilization.update(
+			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+			_actuators.control[CH_AIL] = _stabilization.getAileron();
+			_actuators.control[CH_ELV] = _stabilization.getElevator();
+			_actuators.control[CH_RDR] = _stabilization.getRudder();
+			_actuators.control[CH_THR] = _manual.throttle;
+		}
 	}
 
 	// update all publications
diff --git a/apps/mathlib/CMSIS/Makefile b/apps/mathlib/CMSIS/Makefile
index 0b6794470..fa5de668a 100644
--- a/apps/mathlib/CMSIS/Makefile
+++ b/apps/mathlib/CMSIS/Makefile
@@ -38,7 +38,7 @@
 #
 # Find sources
 #
-DSPLIB_SRCDIR		 = $(dir $(lastword $(MAKEFILE_LIST)))
+DSPLIB_SRCDIR		:= $(dir $(lastword $(MAKEFILE_LIST)))
 CSRCS			:= $(wildcard $(DSPLIB_SRCDIR)/DSP_Lib/Source/*/*.c)
 
 INCLUDES		+= $(DSPLIB_SRCDIR)/Include \
diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c
index 9d9b9914a..f7638550d 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/apps/mavlink/mavlink_parameters.c
@@ -143,7 +143,9 @@ int mavlink_pm_send_param(param_t param)
 	 */
 
 	int ret;
-	if ((ret = param_get(param, &val_buf)) != OK) return ret;
+	if ((ret = param_get(param, &val_buf)) != OK) {
+		return ret;
+	}
 
 	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
 					  mavlink_system.compid,
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index ccd00c52f..06b4c5ca5 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -131,6 +131,13 @@ enum VEHICLE_TYPE {
 	VEHICLE_TYPE_ENUM_END=18, /*  | */
 };
 
+enum VEHICLE_BATTERY_WARNING {
+    VEHICLE_BATTERY_WARNING_NONE = 0,    /**< no battery low voltage warning active */
+    VEHICLE_BATTERY_WARNING_WARNING,        /**< warning of low voltage 1. stage */
+    VEHICLE_BATTERY_WARNING_ALERT            /**< aleting of low voltage 2. stage */
+};
+
+
 /**
  * state machine / state of vehicle.
  *
@@ -188,6 +195,7 @@ struct vehicle_status_s
 	float voltage_battery;
 	float current_battery;
 	float battery_remaining;
+	enum VEHICLE_BATTERY_WARNING battery_warning;    /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
 	uint16_t drop_rate_comm;
 	uint16_t errors_comm;
 	uint16_t errors_count1;