Several fixes, hex flies, failsafe not really tested yet

4 files changed, 55 insertions, 36 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 262367370..de3a4440b 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -203,7 +203,7 @@ mc_thread_main(int argc, char *argv[])
 						if (state.offboard_control_signal_lost) {
 							/* set failsafe thrust */
 							param_get(failsafe_throttle_handle, &failsafe_throttle);
-							att_sp.thrust = (failsafe_throttle < 0.6f) : failsafe_throttle ? 0.5f;
+							att_sp.thrust = failsafe_throttle;
 						} else {
 							/* no signal loss, normal throttle */
 							att_sp.thrust = offboard_sp.p4;
@@ -252,28 +252,28 @@ mc_thread_main(int argc, char *argv[])
 
 				// XXX disable this for now until its better checked
 
-				// static bool rc_loss_first_time = true;
-				// /* if the RC signal is lost, try to stay level and go slowly back down to ground */
-				// if(state.rc_signal_lost) {
-				// 	/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
-				// 	param_get(failsafe_throttle_handle, &failsafe_throttle);
-				// 	att_sp.roll_body = 0.0f;
-				// 	att_sp.pitch_body = 0.0f;
+				 static bool rc_loss_first_time = true;
+				 /* if the RC signal is lost, try to stay level and go slowly back down to ground */
+				 if(state.rc_signal_lost) {
+				 	/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
+				 	param_get(failsafe_throttle_handle, &failsafe_throttle);
+				 	att_sp.roll_body = 0.0f;
+				 	att_sp.pitch_body = 0.0f;
 					
-				// 	/* keep current yaw, do not attempt to go to north orientation,
-				// 	 * since if the pilot regains RC control, he will be lost regarding
-				// 	 * the current orientation.
-				//	 */
+				 	/* keep current yaw, do not attempt to go to north orientation,
+				 	 * since if the pilot regains RC control, he will be lost regarding
+				 	 * the current orientation.
+					 */
 					 
-				// 	if (rc_loss_first_time)
-				// 		att_sp.yaw_body = att.yaw;
-				// 	// XXX hard-limit it to prevent ballistic mishaps - this is just supposed to
-				// 	// slow a crash down, not actually keep the system in-air.
-				// 	att_sp.thrust = (failsafe_throttle < 0.5f) : failsafe_throttle ? 0.5f;
-				// 	rc_loss_first_time = false;
-				// } else {
-				// 	rc_loss_first_time = true;
-				// }
+				 	if (rc_loss_first_time)
+				 		att_sp.yaw_body = att.yaw;
+				 	// XXX hard-limit it to prevent ballistic mishaps - this is just supposed to
+				 	// slow a crash down, not actually keep the system in-air.
+				 	att_sp.thrust = failsafe_throttle;
+				 	rc_loss_first_time = false;
+				 } else {
+				 	rc_loss_first_time = true;
+				 }
 
 				att_sp.timestamp = hrt_absolute_time();
 			}
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 6cf499c39..572344d00 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -167,7 +167,8 @@ mixer_tick(void *arg)
 	/*
 	 * Decide whether the servos should be armed right now.
 	 */
-	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
+
+	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu;
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 0ad636c0b..2a087705a 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -56,18 +56,20 @@ static struct hrt_call arming_call;
  * Count the number of times in a row that we see the arming button
  * held down.
  */
-static unsigned arm_counter;
-#define ARM_COUNTER_THRESHOLD	10
+static unsigned counter;
 
-static bool safety_led_state;
+#define ARM_COUNTER_THRESHOLD	20
+#define DISARM_COUNTER_THRESHOLD	2
 
+static bool safety_led_state;
+static bool safety_button_pressed;
 static void safety_check_button(void *arg);
 
 void
 safety_init(void)
 {
-	/* arrange for the button handler to be called at 10Hz */
-	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+	/* arrange for the button handler to be called at 20Hz */
+	hrt_call_every(&arming_call, 1000, 50000, safety_check_button, NULL);
 }
 
 static void
@@ -77,18 +79,34 @@ safety_check_button(void *arg)
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
 	 */
+	safety_button_pressed = BUTTON_SAFETY;
+
+	if(safety_button_pressed) {
+		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
+	}
 
-	if (BUTTON_SAFETY) {
-		if (arm_counter < ARM_COUNTER_THRESHOLD) {
-			arm_counter++;
-		} else if (arm_counter == ARM_COUNTER_THRESHOLD) {
-			/* change our armed state and notify the FMU */
-			system_state.armed = !system_state.armed;
-			arm_counter++;
+	/* Keep pressed for a while to arm */
+	if (safety_button_pressed && !system_state.armed) {
+		if (counter < ARM_COUNTER_THRESHOLD) {
+			counter++;
+		} else if (counter == ARM_COUNTER_THRESHOLD) {
+			/* change to armed state and notify the FMU */
+			system_state.armed = true;
+			counter++;
+			system_state.fmu_report_due = true;
+		}
+	/* Disarm quickly */
+	} else if (safety_button_pressed && system_state.armed) {
+		if (counter < DISARM_COUNTER_THRESHOLD) {
+			counter++;
+		} else if (counter == DISARM_COUNTER_THRESHOLD) {
+			/* change to disarmed state and notify the FMU */
+			system_state.armed = false;
+			counter++;
 			system_state.fmu_report_due = true;
 		}
 	} else {
-		arm_counter = 0;
+		counter = 0;
 	}
 
 	/* when armed, toggle the LED; when safe, leave it on */
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index 52d79e369..b5b25e532 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -164,7 +164,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	/* use an output factor to prevent too strong control signals at low throttle */
 	float min_thrust = 0.05f;
 	float max_thrust = 1.0f;
-	float startpoint_full_control = 0.20f;
+	float startpoint_full_control = 0.40f;
 	float output_factor;
 
 	/* keep roll, pitch and yaw control to 0 below min thrust */