Merge remote-tracking branch 'px4/beta' into beta_mavlink

5 files changed, 14 insertions, 9 deletions

diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
index 880e4899b..fe85f7d35 100644
--- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -2,7 +2,7 @@
 #
 # Team Blacksheep Discovery Quadcopter
 #
-# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>
+# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
 #
 
 sh /etc/init.d/rc.mc_defaults
@@ -15,7 +15,7 @@ then
 	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.0017
 	param set MC_PITCH_P 8.0
-	param set MC_PITCHRATE_P 0.14
+	param set MC_PITCHRATE_P 0.1
 	param set MC_PITCHRATE_I 0.1
 	param set MC_PITCHRATE_D 0.0025
 	param set MC_YAW_P 2.8
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index 3e340699f..3a50fcf56 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -10,4 +10,5 @@ then
 	param set NAV_LAND_ALT 90
 	param set NAV_RTL_ALT 100
 	param set NAV_RTL_LAND_T -1
+	param set NAV_ACCEPT_RAD 50
 fi
\ No newline at end of file
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c039b8573..8129dddb3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0;
 static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
+static int parachute_enabled = 0;
 
 static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
@@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
-			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+			//XXX: to enable the parachute, a param needs to be set
+			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
 				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
@@ -615,6 +618,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
 
 	/* welcome user */
 	warnx("starting");
@@ -860,10 +864,10 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* re-check RC calibration */
 				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
-
-				/* navigation parameters */
-				param_get(_param_takeoff_alt, &takeoff_alt);
 			}
+			/* navigation parameters */
+			param_get(_param_takeoff_alt, &takeoff_alt);
+			param_get(_param_enable_parachute, &parachute_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1251,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		// TODO remove this hack
 		/* flight termination in manual mode if assisted switch is on easy position */
-		if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive();
 			}
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 5139ae6cd..260356eca 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1080,9 +1080,8 @@ Navigator::start_loiter()
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
 		}
 
-		_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	}
-
+	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
 	_pos_sp_triplet.current.loiter_direction = 1;
 	_pos_sp_triplet.previous.valid = false;
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 1ba159a8e..9e05bbffa 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -61,3 +61,4 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);	// default TAKEOFF altitude
 PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);		// slow descend from this altitude when landing
 PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);		// min altitude for going home in RTL mode
 PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);	// delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
+PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);	// enable parachute deployment