Minor fixes, pushing WIP

6 files changed, 60 insertions, 31 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 50544320b..be50289c3 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -147,7 +147,6 @@ int commander_thread_main(int argc, char *argv[]);
 
 static int buzzer_init(void);
 static void buzzer_deinit(void);
-static void tune_confirm(void);
 static int led_init(void);
 static void led_deinit(void);
 static int led_toggle(int led);
@@ -272,6 +271,10 @@ void tune_confirm(void) {
 	ioctl(buzzer, TONE_SET_ALARM, 3);
 }
 
+void tune_error(void) {
+	ioctl(buzzer, TONE_SET_ALARM, 4);
+}
+
 void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
 {
 	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -980,8 +983,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		break;
 
 		case MAV_CMD_PREFLIGHT_STORAGE: {
-			if (current_status.flag_system_armed) {
-				mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed");
+			if (current_status.flag_system_armed &&
+				((current_status.system_type == MAV_TYPE_QUADROTOR) ||
+						(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
+						(current_status.system_type == MAV_TYPE_OCTOROTOR))) {
+				/* do not perform expensive memory tasks on multirotors in flight */
+				// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
+				mavlink_log_info(mavlink_fd, "[cmd] REJECTING save cmd while multicopter armed");
 			} else {
 
 				// XXX move this to LOW PRIO THREAD of commander app
@@ -1680,6 +1688,7 @@ int commander_thread_main(int argc, char *argv[])
 				 * Check if manual control modes have to be switched
 				 */
 				if (!isfinite(sp_man.manual_mode_switch)) {
+					printf("man mode sw not finite\n");
 
 					/* this switch is not properly mapped, set default */
 					if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
@@ -1688,13 +1697,17 @@ int commander_thread_main(int argc, char *argv[])
 
 						/* assuming a rotary wing, fall back to SAS */
 						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+						current_status.flag_control_attitude_enabled = true;
+						current_status.flag_control_rates_enabled = true;
 					} else {
 
 						/* assuming a fixed wing, fall back to direct pass-through */
 						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+						current_status.flag_control_attitude_enabled = false;
+						current_status.flag_control_rates_enabled = false;
 					}
 
-				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
+				} else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
 
 					/* bottom stick position, set direct mode for vehicles supporting it */
 					if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
@@ -1703,22 +1716,32 @@ int commander_thread_main(int argc, char *argv[])
 
 						/* assuming a rotary wing, fall back to SAS */
 						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+						current_status.flag_control_attitude_enabled = true;
+						current_status.flag_control_rates_enabled = true;
 					} else {
 
-						/* assuming a fixed wing, fall back to direct pass-through */
+						/* assuming a fixed wing, set to direct pass-through as requested */
 						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+						current_status.flag_control_attitude_enabled = false;
+						current_status.flag_control_rates_enabled = false;
 					}
 
-				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
+				} else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
 
 					/* top stick position, set SAS for all vehicle types */
 					current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+					current_status.flag_control_attitude_enabled = true;
+					current_status.flag_control_rates_enabled = true;
 
 				} else {
 					/* center stick position, set rate control */
 					current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
+					current_status.flag_control_attitude_enabled = false;
+					current_status.flag_control_rates_enabled = true;
 				}
 
+				printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
+
 				/*
 				 * Check if manual stability control modes have to be switched
 				 */
diff --git a/apps/commander/commander.h b/apps/commander/commander.h
index bea67bead..04b4e72ab 100644
--- a/apps/commander/commander.h
+++ b/apps/commander/commander.h
@@ -52,4 +52,7 @@
 #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
 #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
 
+void tune_confirm(void);
+void tune_error(void);
+
 #endif /* COMMANDER_H_ */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 4152142df..a8cef8c01 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -535,6 +535,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
 {
 	if (!current_status->flag_vector_flight_mode_ok) {
 		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE");
+		tune_error();
 		return;
 	}
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 5ededa2e3..c51d06579 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -201,7 +201,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
 
 				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-				if(vstatus.rc_signal_lost) {
+				if (vstatus.rc_signal_lost) {
 					
 					// XXX define failsafe throttle param
 					//param_get(failsafe_throttle_handle, &failsafe_throttle);
@@ -244,7 +244,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 				if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
 
 					/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-					if(vstatus.rc_signal_lost) {
+					if (vstatus.rc_signal_lost) {
 						
 						// XXX define failsafe throttle param
 						//param_get(failsafe_throttle_handle, &failsafe_throttle);
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 3351d9cfd..ccc9d6d7d 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -189,10 +189,34 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 	*mavlink_mode = 0;
 
 	/* set mode flags independent of system state */
+
+	/* HIL */
 	if (v_status.flag_hil_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
+	/* manual input */
+	if (v_status.flag_control_manual_enabled) {
+		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	}
+
+	/* attitude or rate control */
+	if (v_status.flag_control_attitude_enabled ||
+		v_status.flag_control_rates_enabled) {
+		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+	}
+
+	/* vector control */
+	if (v_status.flag_control_velocity_enabled ||
+		v_status.flag_control_position_enabled) {
+		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+	}
+
+	/* autonomous mode */
+	if (v_status.state_machine == SYSTEM_STATE_AUTO) {
+		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
+	}
+
 	/* set arming state */
 	if (armed.armed) {
 		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
@@ -221,20 +245,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 
 	case SYSTEM_STATE_MANUAL:
 		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 		break;
 
 	case SYSTEM_STATE_STABILIZED:
 		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
 		break;
 
 	case SYSTEM_STATE_AUTO:
 		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
 		break;
 
 	case SYSTEM_STATE_MISSION_ABORT:
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 10d25d347..c331ec3e3 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1088,24 +1088,8 @@ Sensors::ppm_poll()
 
 		manual_control.timestamp = rc_input.timestamp;
 
-		// /* check if input needs to be de-mixed */
-		// if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
-		// 	// XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
-
-		// 	/* roll input - mixed roll and pitch channels */
-		// 	manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
-		// 	 pitch input - mixed roll and pitch channels 
-		// 	manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
-		// 	/* yaw input - stick right is positive and positive rotation */
-		// 	manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
-		// 	/* throttle input */
-		// 	manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-
-		// /* direct pass-through of manual input */
-		// } else {
-
 		/* roll input - rolling right is stick-wise and rotation-wise positive */
-		manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
+		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
 		/*
 		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
 		 * so reverse sign.